Illustrated C/C++ source sheets, symbol index, API relationship graph, compiler console and debug trace

LIVE AI / FLEXIBLE SENSING / FLIGHT CONTEXT

Sensoraia API

Live AI and sensing, connected to the flight.
See, listen, inspect and return supported results while airborne.

Loading...
Searching...
No Matches
coverage_scheduler.c
Go to the documentation of this file.
1/*
2 * Copyright 2026 OpenUAS
3 * SPDX-License-Identifier: Apache-2.0
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17
18/**
19 * @file coverage_scheduler.c
20 * @brief Bounded geometric decisions without pixel decoding or camera dependencies.
21 * @details Two different margins are intentional: shrink footprints when granting
22 * credit, but expand them before deciding a new photograph is redundant. Using
23 * the same test for both would turn uncertainty into falsely claimed coverage.
24 */
25#include "coverage_scheduler.h"
26
27#include <math.h>
28#include <string.h>
29
30/** @brief Validated inclusive grid bounds and polygon winding for half-plane tests. */
31struct coverage_bounds { int left, right, bottom, top; double winding; };
32
33/** @brief Apply Rz*Ry*Rx in place, matching the mount/body order used by PANGEA. */
34static void rotate(double vector[3], double roll_deg, double pitch_deg, double yaw_deg)
35{
36 const double radians = 0.017453292519943295;
37 const double roll = roll_deg * radians, pitch = pitch_deg * radians, yaw = yaw_deg * radians;
38 const double east = cos(roll) * vector[1] - sin(roll) * vector[2];
39 const double down = sin(roll) * vector[1] + cos(roll) * vector[2];
40 const double north = cos(pitch) * vector[0] + sin(pitch) * down;
41 vector[2] = -sin(pitch) * vector[0] + cos(pitch) * down;
42 vector[0] = cos(yaw) * north - sin(yaw) * east;
43 vector[1] = sin(yaw) * north + cos(yaw) * east;
44}
45
46bool coverage_project(const struct coverage_camera *camera, const struct coverage_pose *pose,
47 double origin_latitude_deg, double origin_longitude_deg, struct coverage_footprint *footprint)
48{
49 if (!camera || !pose || !footprint || camera->width < 2 || camera->width > 4096
50 || camera->height < 2 || camera->height > 4096 || !isfinite(camera->fx) || camera->fx < 1
51 || !isfinite(camera->fy) || camera->fy < 1 || !isfinite(camera->cx) || camera->cx < 0
52 || camera->cx >= camera->width || !isfinite(camera->cy) || camera->cy < 0 || camera->cy >= camera->height
53 || !isfinite(camera->mount_roll_deg) || !isfinite(camera->mount_pitch_deg) || !isfinite(camera->mount_yaw_deg)
54 || fabs(camera->mount_roll_deg) > 180 || fabs(camera->mount_pitch_deg) > 180 || fabs(camera->mount_yaw_deg) > 180
55 || !isfinite(pose->latitude_deg) || fabs(pose->latitude_deg) > 85
56 || !isfinite(pose->longitude_deg) || fabs(pose->longitude_deg) > 180
57 || !isfinite(origin_latitude_deg) || fabs(origin_latitude_deg) > 85
58 || !isfinite(origin_longitude_deg) || fabs(origin_longitude_deg) > 180
59 || !isfinite(pose->agl_m) || pose->agl_m < 0.1 || pose->agl_m > 10000
60 || !isfinite(pose->roll_deg) || !isfinite(pose->pitch_deg) || !isfinite(pose->yaw_deg)
61 || !isfinite(pose->uncertainty_m) || pose->uncertainty_m < 0) return false;
62 const double radians = 0.017453292519943295, earth_radius = 6378137;
63 /* A local plane keeps decisions cheap. Reject distant poses instead of silently
64 * stretching the approximation; the caller can still take an uncredited photo. */
65 const double north = earth_radius * (pose->latitude_deg - origin_latitude_deg) * radians;
66 const double east = earth_radius * cos(origin_latitude_deg * radians)
67 * remainder(pose->longitude_deg - origin_longitude_deg, 360) * radians;
68 if (hypot(east, north) > 5000) return false;
69 struct coverage_footprint projected = {.uncertainty_m = pose->uncertainty_m + (east * east + north * north) / (2 * earth_radius)};
70 for (unsigned index = 0; index < 4; ++index) {
71 const double column = index == 1 || index == 2 ? camera->width - 1 : 0;
72 const double row = index >= 2 ? camera->height - 1 : 0;
73 /* Image-up is body-forward, image-right is starboard, optical-axis is down.
74 * Mount first, then aircraft attitude: reversing these changes the footprint. */
75 double ray[3] = {-(row - camera->cy) / camera->fy, (column - camera->cx) / camera->fx, 1};
76 rotate(ray, camera->mount_roll_deg, camera->mount_pitch_deg, camera->mount_yaw_deg);
77 rotate(ray, pose->roll_deg, pose->pitch_deg, pose->yaw_deg);
78 if (!isfinite(ray[2]) || ray[2] <= 0.1) return false;
79 projected.corner[index] = (struct coverage_point) {
80 east + pose->agl_m * ray[1] / ray[2],
81 north + pose->agl_m * ray[0] / ray[2]
82 };
83 }
84 *footprint = projected;
85 return true;
86}
87
88bool coverage_project_pixel(const struct coverage_camera *camera, const struct coverage_pose *pose,
89 double column, double row, struct coverage_point *offset)
90{
91 struct coverage_footprint footprint;
92 if (!camera || !pose || !offset || !isfinite(column) || !isfinite(row) ||
93 column < 0 || row < 0 || column >= camera->width || row >= camera->height ||
94 !coverage_project(camera, pose, pose->latitude_deg, pose->longitude_deg, &footprint)) return false;
95 double ray[3] = {-(row - camera->cy) / camera->fy, (column - camera->cx) / camera->fx, 1};
96 rotate(ray, camera->mount_roll_deg, camera->mount_pitch_deg, camera->mount_yaw_deg);
97 rotate(ray, pose->roll_deg, pose->pitch_deg, pose->yaw_deg);
98 if (!isfinite(ray[2]) || ray[2] <= 0.1) return false;
99 const struct coverage_point projected = {pose->agl_m * ray[1] / ray[2], pose->agl_m * ray[0] / ray[2]};
100 if (!isfinite(projected.east) || !isfinite(projected.north)) return false;
101 *offset = projected;
102 return true;
103}
104
105bool coverage_init(struct coverage_state *state, double east, double north, double cell_m)
106{
107 if (!state || !isfinite(east) || !isfinite(north) || !isfinite(cell_m) || cell_m < 0.25 || cell_m > 100)
108 return false;
109 memset(state, 0, sizeof(*state));
110 state->origin_east = east;
111 state->origin_north = north;
112 state->cell_m = cell_m;
113 return true;
114}
115
116/** @brief Reject unusable polygons before converting coordinates or scanning cells.
117 * @details Convexity and work limits belong here so every scoring/credit path
118 * shares the same conservative definition of a computable footprint.
119 */
120static bool bounds(const struct coverage_state *state, const struct coverage_candidate *candidate,
121 struct coverage_bounds *box)
122{
123 if (!state || !candidate || state->cell_m < 0.25 || !isfinite(state->cell_m)
124 || !isfinite(state->origin_east) || !isfinite(state->origin_north)
125 || candidate->camera_id < 1 || candidate->camera_id > COVERAGE_CAMERAS || !candidate->quality
126 || !isfinite(candidate->footprint.uncertainty_m) || candidate->footprint.uncertainty_m < 0)
127 return false;
128 double left = INFINITY, right = -INFINITY, bottom = INFINITY, top = -INFINITY;
129 double winding = 0;
130 for (unsigned index = 0; index < 4; ++index) {
131 const struct coverage_point first = candidate->footprint.corner[index];
132 const struct coverage_point second = candidate->footprint.corner[(index + 1) % 4];
133 const struct coverage_point third = candidate->footprint.corner[(index + 2) % 4];
134 if (!isfinite(first.east) || !isfinite(first.north)) return false;
135 const double turn = (second.east - first.east) * (third.north - second.north)
136 - (second.north - first.north) * (third.east - second.east);
137 if (!isfinite(turn) || fabs(turn) < 1e-9 || (winding != 0 && turn * winding <= 0)) return false;
138 winding = turn;
139 left = fmin(left, first.east);
140 right = fmax(right, first.east);
141 bottom = fmin(bottom, first.north);
142 top = fmax(top, first.north);
143 }
144 left = floor((left - state->origin_east) / state->cell_m);
145 right = ceil((right - state->origin_east) / state->cell_m) - 1;
146 bottom = floor((bottom - state->origin_north) / state->cell_m);
147 top = ceil((top - state->origin_north) / state->cell_m) - 1;
148 if (left < 0 || bottom < 0 || right >= COVERAGE_SIDE || top >= COVERAGE_SIDE
149 || right < left || top < bottom || (right - left + 1) * (top - bottom + 1) > COVERAGE_SCAN_LIMIT)
150 return false;
151 *box = (struct coverage_bounds) {(int)left, (int)right, (int)bottom, (int)top, winding > 0 ? 1 : -1};
152 return true;
153}
154
155/** @brief Require the whole cell, not merely its centre, inside every polygon edge. */
156static bool contains_cell(const struct coverage_state *state, const struct coverage_candidate *candidate,
157 const struct coverage_bounds *box, int column, int row)
158{
159 const double east = state->origin_east + (column + 0.5) * state->cell_m;
160 const double north = state->origin_north + (row + 0.5) * state->cell_m;
161 for (unsigned index = 0; index < 4; ++index) {
162 const struct coverage_point first = candidate->footprint.corner[index];
163 const struct coverage_point second = candidate->footprint.corner[(index + 1) % 4];
164 const double delta_east = second.east - first.east, delta_north = second.north - first.north;
165 const double distance = box->winding * (delta_east * (north - first.north) - delta_north * (east - first.east));
166 /* The square-cell support term plus the position margin erodes each edge.
167 * This avoids crediting a boundary cell whose unobserved corner hides a gap. */
168 const double margin = 0.5 * state->cell_m * (fabs(delta_east) + fabs(delta_north))
169 + candidate->footprint.uncertainty_m * hypot(delta_east, delta_north);
170 if (distance + 1e-9 < margin) return false;
171 }
172 return true;
173}
174
175/** @brief Score a second view against a hypothetical first, without committing either.
176 * @details The overlay is local to this calculation. A predicted image must never
177 * leak into the saved ledger, even if it scores well or is next in the queue.
178 */
179static double gain_after(const struct coverage_state *state, const struct coverage_candidate *candidate,
180 const struct coverage_candidate *previous)
181{
182 struct coverage_bounds box, previous_box;
183 if (!bounds(state, candidate, &box)) return -1;
184 const bool same_camera = previous && previous->camera_id == candidate->camera_id;
185 if (same_camera && !bounds(state, previous, &previous_box)) return -1;
186 uint64_t gain = 0;
187 for (int row = box.bottom; row <= box.top; ++row) {
188 for (int column = box.left; column <= box.right; ++column) {
189 if (!contains_cell(state, candidate, &box, column, row)) continue;
190 uint8_t saved = state->saved[candidate->camera_id - 1][row * COVERAGE_SIDE + column];
191 if (same_camera && contains_cell(state, previous, &previous_box, column, row) && previous->quality > saved)
192 saved = previous->quality;
193 if (candidate->quality > saved) gain += candidate->quality - saved;
194 }
195 }
196 return gain * state->cell_m * state->cell_m / 255.0;
197}
198
199double coverage_gain(const struct coverage_state *state, const struct coverage_candidate *candidate)
200{
201 return gain_after(state, candidate, NULL);
202}
203
204bool coverage_commit(struct coverage_state *state, const struct coverage_candidate *candidate)
205{
206 struct coverage_bounds box;
207 if (!bounds(state, candidate, &box)) return false;
208 for (int row = box.bottom; row <= box.top; ++row) {
209 for (int column = box.left; column <= box.right; ++column) {
210 if (!contains_cell(state, candidate, &box, column, row)) continue;
211 uint8_t *saved = &state->saved[candidate->camera_id - 1][row * COVERAGE_SIDE + column];
212 if (candidate->quality > *saved) *saved = candidate->quality;
213 }
214 }
215 return true;
216}
217
218int coverage_choose(const struct coverage_state *state, const struct coverage_candidate *candidates,
219 size_t count, uint64_t now_us, uint64_t service_us)
220{
221 if (!state || !candidates || !count || count > COVERAGE_CANDIDATES || !service_us) return -1;
222 double best = 0, best_first_gain = 0;
223 /* Enumerating four candidates is cheap and deterministic. Looking one view
224 * ahead preserves a small last-chance area that a greedy largest-area choice
225 * could miss before taking a larger, still-visible area in the following shot. */
226 uint64_t best_time = UINT64_MAX;
227 int chosen = -1;
228 for (size_t first = 0; first < count; ++first) {
229 const uint64_t start = candidates[first].ready_us > now_us ? candidates[first].ready_us : now_us;
230 if (start > candidates[first].deadline_us || service_us > UINT64_MAX - start) continue;
231 const double first_gain = coverage_gain(state, &candidates[first]);
232 if (first_gain <= 0) continue;
233 double total = first_gain;
234 for (size_t second = 0; second < count; ++second) {
235 if (second == first) continue;
236 const uint64_t next = candidates[second].ready_us > start + service_us
237 ? candidates[second].ready_us : start + service_us;
238 if (next > candidates[second].deadline_us) continue;
239 const double additional = gain_after(state, &candidates[second], &candidates[first]);
240 if (additional > 0) total = fmax(total, first_gain + additional);
241 }
242 if (total > best || (total == best && (start < best_time || (start == best_time && first_gain > best_first_gain)))) {
243 best = total;
244 best_time = start;
245 best_first_gain = first_gain;
246 chosen = (int)first;
247 }
248 }
249 return chosen;
250}
251
252bool coverage_wholly_saved(const struct coverage_state *state, const struct coverage_candidate *candidate)
253{
254 struct coverage_bounds box;
255 if (!bounds(state, candidate, &box)) return false;
256 const double margin = ceil(candidate->footprint.uncertainty_m / state->cell_m) + 1;
257 /* A bounding rectangle deliberately asks for more evidence than the polygon.
258 * At the map boundary or work limit, prefer an extra photo over a false skip. */
259 if (margin > COVERAGE_SIDE || box.left < margin || box.bottom < margin
260 || box.right + margin >= COVERAGE_SIDE || box.top + margin >= COVERAGE_SIDE
261 || (box.right - box.left + 1 + 2 * margin) * (box.top - box.bottom + 1 + 2 * margin) > COVERAGE_SCAN_LIMIT) return
262 false;
263 for (int row = box.bottom - (int)margin; row <= box.top + (int)margin; ++row)
264 for (int column = box.left - (int)margin; column <= box.right + (int)margin; ++column)
265 if (state->saved[candidate->camera_id - 1][row * COVERAGE_SIDE + column] < candidate->quality) return false;
266 return true;
267}
268
269double coverage_overlap(const struct coverage_state *state, const struct coverage_candidate *first,
270 const struct coverage_candidate *second)
271{
272 struct coverage_bounds first_box, second_box;
273 if (!bounds(state, first, &first_box) || !bounds(state, second, &second_box)
274 || first->camera_id != second->camera_id) return -1;
275 unsigned total = 0, intersection = 0;
276 /* Count uncertain/bounding cells in the denominator too. This lower estimate
277 * prevents an optimistic overlap fraction from justifying an unsafe delay. */
278 for (int row = first_box.bottom; row <= first_box.top; ++row)
279 for (int column = first_box.left; column <= first_box.right; ++column) {
280 ++total;
281 if (!contains_cell(state, first, &first_box, column, row)) continue;
282 if (contains_cell(state, second, &second_box, column, row)) ++intersection;
283 }
284 return total ? (double)intersection / total : -1;
285}
static uint64_t service_us[COVERAGE_CAMERAS]
double coverage_gain(const struct coverage_state *state, const struct coverage_candidate *candidate)
Score additional detail in conservatively contained cells without mutation.
static void rotate(double vector[3], double roll_deg, double pitch_deg, double yaw_deg)
Apply Rz*Ry*Rx in place, matching the mount/body order used by PANGEA.
static double gain_after(const struct coverage_state *state, const struct coverage_candidate *candidate, const struct coverage_candidate *previous)
Score a second view against a hypothetical first, without committing either.
bool coverage_commit(struct coverage_state *state, const struct coverage_candidate *candidate)
Credit a successfully saved observation, retaining the best detail per cell.
bool coverage_init(struct coverage_state *state, double east, double north, double cell_m)
Clear the ledger and set its fixed spatial domain.
bool coverage_project(const struct coverage_camera *camera, const struct coverage_pose *pose, double origin_latitude_deg, double origin_longitude_deg, struct coverage_footprint *footprint)
Project delivered-image corners onto a local flat ground plane.
int coverage_choose(const struct coverage_state *state, const struct coverage_candidate *candidates, size_t count, uint64_t now_us, uint64_t service_us)
Choose the first observation of the best feasible one- or two-shot plan.
bool coverage_wholly_saved(const struct coverage_state *state, const struct coverage_candidate *candidate)
Check whether skipping this observation is geometrically conservative.
static bool bounds(const struct coverage_state *state, const struct coverage_candidate *candidate, struct coverage_bounds *box)
Reject unusable polygons before converting coordinates or scanning cells.
bool coverage_project_pixel(const struct coverage_camera *camera, const struct coverage_pose *pose, double column, double row, struct coverage_point *offset)
Project an image pixel onto the same ground plane as the coverage footprint.
double coverage_overlap(const struct coverage_state *state, const struct coverage_candidate *first, const struct coverage_candidate *second)
Estimate conservative overlap for deciding whether a delay is acceptable.
static bool contains_cell(const struct coverage_state *state, const struct coverage_candidate *candidate, const struct coverage_bounds *box, int column, int row)
Require the whole cell, not merely its centre, inside every polygon edge.
Fixed-memory geometric coverage accounting, independent of camera I/O.
#define COVERAGE_CAMERAS
#define COVERAGE_SIDE
#define COVERAGE_CANDIDATES
#define COVERAGE_SCAN_LIMIT
static bool start(uint32_t session)
Admit a nonzero session and discard correlation from the previous one.
uint8_t row[LWIR_MAX_WIDTH *3U]
Definition lwir_image.c:63
static size_t count
Definition pose_log.c:79
Validated inclusive grid bounds and polygon winding for half-plane tests.
Measured pinhole calibration for the delivered image, including its mount.
One feasible observation; deadlines constrain acquisition start, not saving.
struct coverage_footprint footprint
uint64_t ready_us
Earliest acquisition start; all candidates must share a monotonic time domain.
Convex ground quadrilateral with a conservative boundary margin.
struct coverage_point corner[4]
Local ground coordinates in metres, east and north of the projection origin.
double north
Metres north of the shared local geographic reference.
double east
Metres east of the shared local geographic reference.
Pose estimate and its positional margin for a single observation.
double pitch_deg
Aircraft pitch in degrees.
double latitude_deg
Observation latitude in geographic degrees, not FC fixed-point units.
double roll_deg
Aircraft roll in degrees.
double longitude_deg
Observation longitude in geographic degrees.
Fixed coverage ledger; zero cells are unknown, not necessarily unphotographed.
uint8_t saved[COVERAGE_CAMERAS][COVERAGE_SIDE *COVERAGE_SIDE]
double origin_north
Lower north grid edge in metres relative to the projection origin.
double origin_east
Lower east grid edge in metres relative to the projection origin.
static struct camera_backend camera
Definition usher.c:521