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
dipla_worker.cpp
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 dipla_worker.cpp
20 * @brief Keep IPC responsive while one planner owns the survey and retained route.
21 * @details
22 * Two threads divide the work by ownership, not by algorithm:
23 * - The calling thread validates packets, admits observations, and replies from
24 * small route snapshots. It never reads or mutates the dense GP covariance.
25 * - The planner thread consumes the bounded FIFO in order, conditions the GP,
26 * and computes at most one completed route for each requested chunk token.
27 * Credit-aware SHOT separately acknowledges assimilation so its owner can bound
28 * new measurements without blocking packet validation or dropping evidence.
29 *
30 * Three identities answer different questions. The session token names the
31 * requested route chunk; survey_epoch names the continuing GP experiment;
32 * generation identifies a completed route within that chunk. An active START
33 * changes the chunk without erasing observations. STOP ends the survey, and
34 * the next START advances the epoch so its first observation gets a new GP.
35 *
36 * @par Why retain routes?
37 * A waypoint retrieval is not a waypoint arrival. Only the flight controller
38 * knows when its path is complete and when to request another chunk. Letting
39 * every pose trigger a new route would move the targets while they are followed.
40 * Whole-plan replies copy one generation; legacy point replies pin all eight
41 * retrieval slots so a single cycle cannot assemble a patchwork of routes.
42 *
43 * @par When adjusting timing or capacity
44 * Accepted measurements are evidence, not replaceable planning suggestions.
45 * Queue overflow fails the survey rather than quietly losing evidence, and
46 * freshness is measured from receipt, not from the end of expensive processing.
47 * Keep those rules intact when changing cadence, queue size, or reply handling.
48 *
49 * @warning This is a trusted local SOCK_SEQPACKET ABI, not the UART protocol.
50 * Socket calls and planner cancellation do not provide hard real-time bounds;
51 * the owning process must still supervise a stalled or failed worker.
52 * @see dipla_worker.hpp Public service and descriptor-ownership contract.
53 * @see dipla_ipc.h Packet layout, fixed-point units, and reply validity rules.
54 */
55#include "dipla_worker.hpp"
56#include "dipla_planner.hpp"
57#include "dipla_ipc.h"
59#include <chrono>
60#include <cmath>
61#include <condition_variable>
62#include <iostream>
63#include <mutex>
64#include <random>
65#include <stdexcept>
66#include <thread>
67#include <sys/socket.h>
68#include <unistd.h>
69
70namespace dipla {
71namespace {
72/** @brief Monotonic time keeps clock synchronization or wall-clock edits from renewing stale data. */
73using Clock = std::chrono::steady_clock;
74constexpr std::size_t observation_queue_capacity = 64;
75constexpr std::size_t worker_ini_byte_limit = 8192;
76constexpr int minimum_cadence_ms = 100;
77constexpr int maximum_cadence_ms = 5000;
78constexpr double maximum_anchor_latitude_deg = 80.0;
79constexpr double maximum_anchor_longitude_deg = 179.99;
80constexpr double earth_radius_m = 6378137.0;
81constexpr double radians_per_degree = 3.14159265358979323846 / 180.0;
82constexpr double field_centre_m = DIPLA_FIELD_EXTENT_M / 2.0;
83
84/**
85 * @brief Immutable worker inputs and scheduling.
86 * @details Separate worker scheduling from the model's inference
87 * settings. Both threads borrow this object until the planner has been joined;
88 * no configuration mutex or repeated trigonometry is needed in request handling.
89 */
90struct Configuration {
91 /** @var Configuration::field
92 * @brief Synthetic truth CSV path; required, even in comparison mode. */
93 /** @var Configuration::model
94 * @brief Model fallback or worker override; used only by diffusion. */
95 /** Deployment metadata fallback or worker override, paired with model. */
96 std::string field, model, metadata;
97 /** Post-attempt planning cooldown in milliseconds, not a promised update frequency. */
98 int cadence = 1000;
99 /** Maximum receipt age of a pose allowed to start planning, in milliseconds. */
100 unsigned planning_pose_max_age_ms = 1000;
101 double field_size_m = DIPLA_FIELD_EXTENT_M;
102 double field_rotation_deg = 0;
103 double flight_height_m = DIPLA_DEMO_HEIGHT_MM / DIPLA_MM_PER_M;
104 double flight_height_tolerance_m = DIPLA_HEIGHT_TOLERANCE_M;
105
106 /**
107 * @brief Parse the bounded worker INI before either thread can use it.
108 * @details Require the synthetic field explicitly. Reject
109 * duplicate and unknown keys so configuration never depends on which value
110 * happened to appear last. Optional model paths override entry-point defaults.
111 * @param[in] filename Worker configuration file.
112 * @param[in] default_model Executable-level model fallback.
113 * @param[in] default_metadata Executable-level metadata fallback.
114 * @throws std::exception If INI syntax, keys, file size, or cadence are invalid.
115 * @note Worker path values are retained as supplied. Do not assume this
116 * parser rebases them against the INI directory like model-settings discovery.
117 */
118 Configuration(const std::string& filename, const std::string& default_model, const std::string& default_metadata)
119 : model(default_model), metadata(default_metadata) {
120 /** @brief Callback context borrowed only during synchronous INI parsing. */
121 struct Fields {
122 /** Object under construction; never exposed to the planner yet. */
123 Configuration& configuration;
124 /** One bit per known key, separating absent values from defaults. */
125 unsigned seen = 0;
126 } fields{*this};
127 const auto handler = [](void* context, const char* key, const char* value) {
128 auto& parsed = *static_cast<Fields*>(context);
129 auto& config = parsed.configuration;
130 // The key order also defines the presence-mask bits below. Keep
131 // that pairing together when extending the configuration grammar.
132 const char* const keys[] = {"field", "model", "metadata", "cadence_ms",
133 "planning_pose_max_age_ms", "field_size_m", "field_rotation_deg",
134 "flight_height_m", "flight_height_tolerance_m"};
135 size_t index = 0;
136 while (index < std::size(keys) && strcmp(key, keys[index])) ++index;
137 if (index == std::size(keys) || (parsed.seen & (1U << index))) return false;
138 parsed.seen |= 1U << index;
139 switch (index) {
140 case 0: config.field = value; return true;
141 case 1: config.model = value; return true;
142 case 2: config.metadata = value; return true;
143 case 3: {
144 uint64_t cadence_value = 0;
145 if (!ini_uint(value, &cadence_value) || cadence_value > maximum_cadence_ms) return false;
146 config.cadence = static_cast<int>(cadence_value);
147 return true;
148 }
149 case 4: {
150 uint64_t age = 0;
151 if (!ini_uint(value, &age) || !age || age > DIPLA_POINT_MAX_AGE_MS) return false;
152 config.planning_pose_max_age_ms = static_cast<unsigned>(age);
153 return true;
154 }
155 case 5: return ini_real(value, &config.field_size_m);
156 case 6: return ini_real(value, &config.field_rotation_deg);
157 case 7: return ini_real(value, &config.flight_height_m);
158 case 8: return ini_real(value, &config.flight_height_tolerance_m);
159 default: return false;
160 }
161 };
162 // One extra byte accommodates the bounded reader's terminator, not a
163 // larger settings file. Reuse the common parser instead of a second INI dialect.
164 char contents[worker_ini_byte_limit + 1];
165 size_t length = 0;
166 const unsigned required = 1U << 0;
167 if (!ini_read_file(filename.c_str(), contents, sizeof(contents), &length) ||
168 !ini_parse(contents, length, handler, &fields) || (fields.seen & required) != required)
169 throw std::runtime_error("Invalid diplacam configuration: required field, unique known keys, maximum 8192 bytes");
170 if (cadence < minimum_cadence_ms || cadence > maximum_cadence_ms || field.empty())
171 throw std::runtime_error("Invalid field or cadence (100..5000 ms)");
172 if (field_size_m < 1 || field_size_m > DIPLA_FIELD_EXTENT_M ||
173 field_rotation_deg < -180 || field_rotation_deg > 180 ||
174 flight_height_m < DIPLA_FLIGHT_HEIGHT_MIN_MM / DIPLA_MM_PER_M ||
175 flight_height_m > DIPLA_FLIGHT_HEIGHT_MAX_MM / DIPLA_MM_PER_M ||
176 flight_height_tolerance_m <= 0 || flight_height_tolerance_m >= flight_height_m)
177 throw std::runtime_error("Invalid field size, rotation or flight height/tolerance");
178 if (field_size_m != DIPLA_FIELD_EXTENT_M &&
179 ((flight_height_m - flight_height_tolerance_m) * DIPLA_FIELD_EXTENT_M / field_size_m < DIPLA_MODEL_Z_MIN_M ||
180 (flight_height_m + flight_height_tolerance_m) * DIPLA_FIELD_EXTENT_M / field_size_m > DIPLA_MODEL_Z_MAX_M))
181 throw std::runtime_error("Rescaled flight height interval must remain within the model's 10..40 m domain");
182 }
183};
184
185/** @brief Socket-owned geographic frame, immutable throughout one survey. */
186struct SurveyFrame {
187 double latitude; /**< Actual anchor latitude in degrees. */
188 double longitude; /**< Actual anchor longitude in degrees. */
189 double north_scale = earth_radius_m * radians_per_degree; /**< Metres per latitude degree. */
190 double east_scale; /**< Metres per longitude degree at the anchor. */
191 double physical_scale;
192 double rotation_cosine, rotation_sine;
193
194 /** @brief Capture the first admitted position. @param request Validated anchor pose. */
195 SurveyFrame(const dipla_request& request, const Configuration& config)
196 : latitude(request.lat / DIPLA_GEO_UNITS_PER_DEGREE),
197 longitude(request.lon / DIPLA_GEO_UNITS_PER_DEGREE),
198 east_scale(north_scale * std::cos(latitude * radians_per_degree)),
199 physical_scale(config.field_size_m / DIPLA_FIELD_EXTENT_M),
200 rotation_cosine(std::cos(config.field_rotation_deg * radians_per_degree)),
201 rotation_sine(std::sin(config.field_rotation_deg * radians_per_degree)) {}
202
203 /** @brief Map a pose into the centred field. @param request Geographic pose. @return XYZ in metres. */
204 std::array<double, 3> position(const dipla_request& request) const {
205 const double east = (request.lon / DIPLA_GEO_UNITS_PER_DEGREE - longitude) * east_scale;
206 const double north = (request.lat / DIPLA_GEO_UNITS_PER_DEGREE - latitude) * north_scale;
207 return {{field_centre_m + (east * rotation_cosine + north * rotation_sine) / physical_scale,
208 field_centre_m + (-east * rotation_sine + north * rotation_cosine) / physical_scale,
209 request.agl_q8 / DIPLA_AGL_UNITS_PER_M / physical_scale}};
210 }
211
212 std::array<double, 2> physical_offset(const std::array<double, 3>& point) const {
213 const double field_x = (point[0] - field_centre_m) * physical_scale;
214 const double field_y = (point[1] - field_centre_m) * physical_scale;
215 return {{field_x * rotation_cosine - field_y * rotation_sine,
216 field_x * rotation_sine + field_y * rotation_cosine}};
217 }
218};
219
220/**
221 * @brief Shared state exchanged between the socket loop and planner thread.
222 * @details The mutex protects operational fields during concurrent use. Copy a
223 * small observation or route while locked, then release the mutex before GP,
224 * inference, or socket work. The condition variable announces work; its predicate,
225 * not the notification count, decides whether there is anything to consume.
226 * @invariant count never exceeds observations.size(); first identifies the next
227 * FIFO item, and each queued item retains its original receipt time and epoch.
228 * @warning failed is latched for this worker's lifetime. START is a chunk request,
229 * not a magic eraser for failed evidence; recovery requires a new worker.
230 */
231struct Shared {
232 /** Protects state and pairs with `changed` for planner wakeups. */
233 std::mutex mutex;
234 /** Wakes the planner when queued work or terminal state may have changed. */
235 std::condition_variable changed;
236 /** @var Shared::stopping
237 * @brief Terminal shutdown request, observed before taking another FIFO item. */
238 /** Latched failure; invalidates publication even if route bytes still exist. */
239 bool stopping = false, failed = false;
240 /** @var Shared::session
241 * @brief Active chunk token, or zero when STOP has ended the survey. */
242 /** Published route identity within the active chunk; zero means no usable route. */
243 uint32_t session = 0, generation = 0;
244 /** GP lifetime identity; advances only on START from the stopped state. */
245 std::uint64_t survey_epoch = 0;
246 /**
247 * @brief One admitted pose with the context needed after delayed processing.
248 * @details A chunk transition may retain this item, but an epoch transition
249 * must not let an old survey's measurement become new-survey evidence.
250 */
251 struct Observation {
252 /** Original pose and request identity; not replaced by a later request. */
253 dipla_request request{};
254 /** Position converted in the frame that admitted this observation. */
255 std::array<double, 3> position{};
256 /** Local admission time, never refreshed merely because processing was slow. */
257 Clock::time_point received{};
258 /** Survey identity at admission, independent of the request's chunk token. */
259 std::uint64_t epoch = 0;
260 };
261 /** Bounded evidence FIFO; dropping or coalescing entries would change GP history. */
262 std::array<Observation, observation_queue_capacity> observations{};
263 /** @var Shared::first
264 * @brief Ring index of the oldest admitted observation. */
265 /** Number of occupied ring entries, excluding any item already owned by the planner. */
266 std::size_t first = 0, count = 0;
267 /** Receipt-time observation lease; later assimilation may renew it without replacing route. */
268 Clock::time_point plan_time{};
269 /** Most recently completed route, copied under the mutex. */
270 Waypoints route{};
271
272 /**
273 * @brief Admit a pose exactly once without silently replacing older evidence.
274 * @details Overload invalidates the route immediately. Overwriting the oldest
275 * item or keeping only the latest pose would make the GP claim a measurement
276 * history different from the one the socket owner successfully submitted.
277 * @param[in] request Validated, increasing observation request for the active chunk.
278 * @param[in] position Pose in the survey's immutable local frame.
279 * @param[in] received Original local receipt time for freshness accounting.
280 * @return True after FIFO admission; false after latching overflow failure.
281 * @pre The caller holds mutex and has checked the active session and failed flag.
282 * @post A waiting planner is notified on either admission or overflow.
283 * @note The caller must still send a correlated invalid reply on overflow;
284 * a failed survey is not a reason to leave its requester waiting in silence.
285 */
286 bool enqueue(const dipla_request& request, const std::array<double, 3>& position, Clock::time_point received) {
287 if (count == observations.size()) {
288 failed = true;
289 generation = 0;
290 std::cerr << "diplacam observation queue overflow; survey stopped\n";
291 changed.notify_one();
292 return false;
293 }
294 // Append at the tail rather than shifting entries. The small fixed ring
295 // bounds queue storage; it does not bound the GP or ONNX Runtime's allocations.
296 observations[(first + count) % observations.size()] = {request, position, received, survey_epoch};
297 ++count;
298 changed.notify_one();
299 return true;
300 }
301};
302
303/**
304 * @brief Check geographic representation, local field bounds and demo height.
305 * @param request Original integer-valued IPC pose.
306 * @param position Pose converted to local metres.
307 * @return True if the pose is compatible with the fixed demonstration domain.
308 */
309bool valid_pose(const dipla_request& request, const std::array<double, 3>& position, const Configuration& config) {
312 (request.lat != 0 || request.lon != 0) &&
313 position[0] >= 0 && position[0] <= DIPLA_FIELD_EXTENT_M &&
314 position[1] >= 0 && position[1] <= DIPLA_FIELD_EXTENT_M &&
315 std::abs(request.agl_q8 / DIPLA_AGL_UNITS_PER_M - config.flight_height_m) <= config.flight_height_tolerance_m;
316}
317
318/**
319 * @brief Run the sole planner thread until shutdown or an unrecoverable error.
320 * @details Assimilates each queued observation once. A new chunk is planned
321 * only from a pose within the configured startup age; delayed measurements still update
322 * the GP without becoming fresh planning input. Expensive work runs unlocked;
323 * publication rechecks the chunk token so a completed old job cannot overwrite
324 * the current chunk. The flight controller alone requests the next chunk.
325 * @param[in,out] shared Synchronized evidence FIFO, lifecycle, and publication state.
326 * @param[in] config Immutable worker configuration, alive until this thread exits.
327 * @param[in] cmaes Explicit comparison-only selector; never an inference-failure fallback.
328 * @param[in] comparison Comparison search bounds and cooperative deadline.
329 * @param[in] network_options Descriptor and whole-DDIM execution policy.
330 * @param[in] experiment Immutable GP, sensor, seed and publication-margin policy.
331 * @pre The service keeps all referenced inputs alive until joining this thread.
332 * @note Standard exceptions latch failure and clear route validity. A retained
333 * route must not conceal that its GP or inference owner has failed.
334 * @warning CMA-ES polls session-aware cancellation. Diffusion uses its network
335 * deadline and checks session validity at commit, not between each step here.
336 */
337void plan(int descriptor, Shared& shared, const Configuration& config, bool cmaes, const CmaesOptions& comparison,
338 const NetworkOptions& network_options, const ExperimentOptions& experiment) {
339 try {
340 // Load once, not once per observation or chunk. Comparison mode must
341 // remain usable without opening the diffusion graph or ONNX session.
342 std::unique_ptr<Network> network;
343 if (!cmaes) network = std::make_unique<Network>(config.model, config.metadata, network_options);
344 std::cerr << "diplacam planner=" << (cmaes ? "cmaes comparison_only=true onnx=false" : "diffusion onnx=true") << '\n';
345 std::unique_ptr<Survey> survey;
346 // This stream belongs to diffusion only; Survey owns a separate sensor
347 // noise stream. Changing inference cadence must not change sensor noise.
348 std::mt19937_64 generator(experiment.diffusion_seed);
349 std::normal_distribution<double> normal;
350 // Wire observation numbers restart at chunk boundaries, but GP evidence
351 // numbers must increase throughout the survey. Keep a private counter.
352 uint32_t planned_session = 0, observation_number = 0;
353 std::uint64_t previous_epoch = 0;
354 auto next = Clock::now();
355 for (;;) {
356 std::unique_lock<std::mutex> lock(shared.mutex);
357 // Predicate-based waiting handles both spurious wakeups and notices
358 // delivered before we started waiting. No queued pose, no busy loop.
359 shared.changed.wait(lock, [&] { return shared.stopping || shared.failed || shared.count; });
360 if (shared.stopping || shared.failed) return;
361 // Take one item by value so the socket thread can immediately reuse
362 // its ring slot while the planner works on this private copy.
363 const auto observation = shared.observations[shared.first];
364 shared.first = (shared.first + 1) % shared.observations.size();
365 --shared.count;
366 const auto session = shared.session;
367 const auto survey_epoch = observation.epoch;
368 const auto request = observation.request;
369 const auto pose_time = observation.received;
370 if (!session || survey_epoch != shared.survey_epoch) continue;
371 // Planning touches large GP state and may run for hundreds of
372 // milliseconds; never hold the IPC mutex across that work.
373 lock.unlock();
374 const auto update_started = Clock::now();
375 if (survey_epoch != previous_epoch) {
376 // Only a new survey epoch resets knowledge, not a new route
377 // chunk. Release the old dense covariance before allocating its
378 // replacement; briefly keeping both would need another ~51.6 MiB.
379 survey.reset();
380 survey = std::make_unique<Survey>(config.field, experiment);
381 previous_epoch = survey_epoch;
382 observation_number = 0;
383 planned_session = 0;
384 next = Clock::time_point{};
385 std::cerr << "diplacam field_fnv1a=" << survey->field_digest()
386 << " physical_flight_height_m=" << config.flight_height_m
387 << " predicted_z_discarded=true\n";
388 }
389 const auto position = observation.position;
390 // Wraparound would make fresh evidence look like a duplicate to
391 // Survey::observe. Exhaustion fails explicitly instead of losing updates.
392 if (observation_number == UINT32_MAX) throw std::runtime_error("Observation sequence exhausted");
393 const double sensor_height = config.field_size_m == DIPLA_FIELD_EXTENT_M &&
394 config.flight_height_m == DIPLA_DEMO_HEIGHT_MM / DIPLA_MM_PER_M
395 ? DIPLA_DEMO_HEIGHT_MM / DIPLA_MM_PER_M : position[2];
396 survey->observe(position[0], position[1], ++observation_number, sensor_height);
397 if (request.operation == DIPLA_IPC_CREDIT_SHOT) {
398 const dipla_reply acknowledgement{DIPLA_IPC_VERSION, request.session, request.number,
399 0, 0, 0, 0, 0, DIPLA_IPC_ASSIMILATED_METRIC};
400 ssize_t sent;
401 do { sent = send(descriptor, &acknowledgement, sizeof(acknowledgement), MSG_NOSIGNAL | MSG_DONTWAIT); }
402 while (sent < 0 && errno == EINTR);
403 if (sent != sizeof(acknowledgement)) throw std::runtime_error("Observation acknowledgement failed");
404 }
405 if (observation_number == 1)
406 std::cerr << "diplacam initial_gp_update_ms="
407 << std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - update_started).count() << '\n';
408 // Assimilate first, then decide whether this item may START planning.
409 // Old-chunk FIFO items still belong to this survey, and delayed poses
410 // are still measurements; neither is a fresh planning authorization.
411 // planned_session retains the route while later observations refine GP.
412 if (request.session != session || planned_session == session || Clock::now() < next ||
413 Clock::now() - pose_time > std::chrono::milliseconds(config.planning_pose_max_age_ms)) {
414 lock.lock();
415 // Refresh a retained route's observation lease only after this
416 // chunk's pose has reached the GP. Preserve its receipt time:
417 // finishing a late update cannot make old evidence young again.
418 if (request.session == shared.session && survey_epoch == shared.survey_epoch &&
419 shared.generation && !shared.stopping)
420 shared.plan_time = pose_time;
421 continue;
422 }
423 std::optional<Waypoints> route;
424 const auto inference_started = Clock::now();
425 if (cmaes) {
426 route = survey->compare(position, comparison, [&] {
427 std::lock_guard<std::mutex> guard(shared.mutex);
428 return shared.stopping || shared.session != session || shared.survey_epoch != survey_epoch;
429 });
430 } else {
431 Noise noise{};
432 for (double& value : noise) value = normal(generator);
433 route = network->sample(survey->belief(), position, noise);
434 }
435 // Cool down after the attempt, not from its start. This is a minimum
436 // idle budget, not a fixed-rate timer; another eligible pose is needed
437 // to start future work, and FIFO assimilation continues meanwhile.
438 next = Clock::now() + std::chrono::milliseconds(config.cadence);
439 if (!route) {
440 std::cerr << "diplacam CMA-ES comparison interrupted before a complete route; no new points\n";
441 continue;
442 }
443 // Both planners share the same application flight padding. Keep it
444 // outside Network::sample so raw DDIM output stays reference-testable.
445 apply_flight_margin(*route, experiment.flight_margin_cells);
446 lock.lock();
447 // A stop/start may have happened while planning. Discard stale
448 // output rather than exposing points from the previous session.
449 if (session == shared.session && survey_epoch == shared.survey_epoch && !shared.stopping && !shared.failed) {
450 // Legacy metric packs three slot bits below generation into a
451 // signed int32. Cap generation at 28 bits rather than wrapping
452 // route identity or setting the metric's sign bit.
453 if (shared.generation == DIPLA_MAX_GENERATION) throw std::runtime_error("Plan generation exhausted");
454 shared.route = *route;
455 ++shared.generation;
456 planned_session = session;
457 std::cerr << "diplacam chunk=" << session << " observations=" << observation_number
458 << " posterior_variance=" << survey->total_variance()
459 << " inference_ms=" << std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - inference_started).count() << '\n';
460 // Publish route, generation, and lease as one locked state change.
461 // Planning time consumes freshness; it does not earn a new lease.
462 shared.plan_time = pose_time;
463 }
464 }
465 } catch (const std::exception& error) {
466 std::cerr << "diplacam worker: " << error.what() << '\n';
467 // The socket owner may keep receiving long enough to answer invalid.
468 // Invalidate even a previously good route; stale success must not hide failure.
469 std::lock_guard<std::mutex> lock(shared.mutex);
470 shared.failed = true;
471 shared.generation = 0;
472 }
473}
474}
475
476/*
477 * Service implementation notes; the public Doxygen contract lives in the header.
478 *
479 * Keep session transitions and publication snapshots under the same mutex, but
480 * keep blocking socket I/O outside it. The planner can then condition its GP even
481 * while the owner is slow to receive. This does not impose a socket timeout;
482 * the parent still owns transport supervision and closes the borrowed descriptor.
483 *
484 * PLAN returns one complete bank. SHOT returns a slot from a pinned bank and
485 * caches that exact reply for retries. Both read the same completed route; neither
486 * advances aircraft navigation. Slot seven is not a tiny finish-line detector.
487 *
488 * Owners must give each new chunk a distinct token while old work can still be
489 * in flight. Publication rejects a changed token, not a history of reused tokens.
490 */
491int serve(int descriptor, const std::string& config_path, const std::string& model, const std::string& metadata,
492 bool cmaes, const CmaesOptions& comparison, const NetworkOptions& network_options,
493 const ExperimentOptions& experiment) {
494 experiment.validate();
495 comparison.validate();
496 Configuration config(config_path, model, metadata);
497 if (cmaes && (config.field_size_m != DIPLA_FIELD_EXTENT_M ||
498 config.flight_height_m != DIPLA_DEMO_HEIGHT_MM / DIPLA_MM_PER_M))
499 throw std::runtime_error("Resized or nondefault-height worker profiles require the diffusion planner");
500 Shared shared;
501 // Start only after config is validated. References remain valid until join;
502 // neither the worker nor its exception handler takes ownership of the socket.
503 std::thread worker(plan, descriptor, std::ref(shared), std::cref(config), cmaes, std::cref(comparison),
504 std::cref(network_options), std::cref(experiment));
505 // These retrieval/cache fields belong to the socket thread alone. The GP
506 // never sees a slot cursor, which prevents retrieval from becoming navigation.
507 Waypoints pinned{};
508 uint32_t slot = 0, generation = 0, last_number = 0;
509 bool last_uses_credit = false;
510 dipla_reply cached{};
511 auto last_request = Clock::time_point{}, pinned_time = Clock::time_point{};
512 bool transport_failed = false;
513 std::optional<SurveyFrame> frame;
514 const auto admit = [&](const dipla_request& request, const std::array<double, 3>& position, Clock::time_point received) {
515 if (!shared.enqueue(request, position, received)) return false;
516 if (!frame) {
517 frame.emplace(request, config);
518 std::cerr << "diplacam anchor_lat_e7=" << request.lat << " anchor_lon_e7=" << request.lon
519 << " anchor_shot=" << request.number << " field_centre_m=" << field_centre_m
520 << " field_size_m=" << config.field_size_m << " field_rotation_deg=" << config.field_rotation_deg
521 << " physical_agl_m=" << request.agl_q8 / DIPLA_AGL_UNITS_PER_M
522 << " model_agl_m=" << position[2] << '\n';
523 }
524 return true;
525 };
526 for (;;) {
528 // SOCK_SEQPACKET preserves boundaries. MSG_TRUNC reports an oversized
529 // packet's full length so its plausible prefix cannot pass the size check.
530 // This is not a byte-stream parser: reconnect rather than resynchronize
531 // after a malformed packet or an incompatible layout version.
532 const auto count = recv(descriptor, &request, sizeof(request), MSG_TRUNC);
533 if (count < 0 && errno == EINTR) continue;
534 if (!count) break;
535 if (count != sizeof(request) || request.version != DIPLA_IPC_VERSION || !request.session) {
536 transport_failed = true;
537 break;
538 }
539 std::unique_lock<std::mutex> lock(shared.mutex);
540 if (request.operation == DIPLA_IPC_START || request.operation == DIPLA_IPC_STOP) {
541 // A repeated START is idempotent; a stale STOP must not cancel a
542 // newer chunk. Active START requests another route while retaining
543 // the same survey epoch and its accepted, still-pending observations.
544 if (request.operation == DIPLA_IPC_STOP && shared.session != request.session) continue;
545 if (request.operation == DIPLA_IPC_START) {
546 if (shared.session == request.session) continue;
547 if (!shared.session) {
548 // A reused epoch could mislabel queued evidence. Even this
549 // practically remote counter limit must fail instead of wrap.
550 if (shared.survey_epoch == UINT64_MAX) { transport_failed = true; break; }
551 ++shared.survey_epoch;
552 frame.reset();
553 }
554 }
555 shared.session = request.operation == DIPLA_IPC_START ? request.session : 0;
556 // Invalidate publication immediately; old bytes may stay in route,
557 // but generation zero prevents treating them as this chunk's result.
558 shared.generation = 0;
559 if (!shared.session) {
560 shared.first = shared.count = 0;
561 frame.reset();
562 }
563 // Every chunk begins a new retrieval cycle and wire-number history.
564 // Only STOP discards pending evidence; chunk handoff must not lose it.
565 slot = generation = last_number = 0;
566 last_uses_credit = false;
567 cached = {};
568 shared.changed.notify_one();
569 continue;
570 }
571 const auto position = frame ? frame->position(request) :
572 std::array<double, 3>{{field_centre_m, field_centre_m,
573 request.agl_q8 / DIPLA_AGL_UNITS_PER_M * DIPLA_FIELD_EXTENT_M / config.field_size_m}};
574 const bool pose_valid = valid_pose(request, position, config) &&
575 (frame || (std::abs(request.lat / DIPLA_GEO_UNITS_PER_DEGREE) <= maximum_anchor_latitude_deg &&
576 std::abs(request.lon / DIPLA_GEO_UNITS_PER_DEGREE) <= maximum_anchor_longitude_deg));
577 if (request.operation == DIPLA_IPC_PLAN && request.session == shared.session && request.number) {
578 const auto now = Clock::now();
579 // Check geographic range as well as local XY, and require the fixed
580 // 2 m experiment's altitude tolerance. This validates the input pose,
581 // not arrival at any planned waypoint or suitability of model Z.
582 // Even an unready or rejected request gets its own correlated INVALID
583 // reply. Zero initialization ensures no leftover route can leak as data.
584 dipla_plan_reply reply{};
586 reply.session = request.session;
587 reply.number = request.number;
588 if (request.number > last_number && pose_valid && !shared.failed) {
589 // Admission is asynchronous: the route returned below, if any,
590 // predates assimilation of this pose. Do not call it a new plan.
591 // Overflow clears generation under this lock, suppressing validity.
592 (void)admit(request, position, now);
593 last_number = request.number;
594 last_uses_credit = false;
595 if (shared.generation && now - shared.plan_time <= std::chrono::milliseconds(DIPLA_POINT_MAX_AGE_MS)) {
596 // Copy all eight points under the lock so one reply cannot
597 // mix generations even if the planner completes concurrently.
598 reply.valid = 1;
599 reply.generation = shared.generation;
600 reply.age_ms = static_cast<uint32_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
601 now - shared.plan_time).count());
602 reply.count = points;
603 for (size_t index = 0; index < shared.route.size(); ++index) {
604 const auto& point = shared.route[index];
605 if (!std::isfinite(point[0]) || !std::isfinite(point[1]) ||
606 point[0] < 0 || point[0] > DIPLA_FIELD_EXTENT_M ||
607 point[1] < 0 || point[1] > DIPLA_FIELD_EXTENT_M) {
608 reply.valid = reply.count = 0;
609 break;
610 }
611 // Bounds precede narrowing to integer millimetres. The
612 // repeated field check defends this publication boundary;
613 // the shared flight-margin helper already padded the route.
614 reply.points[index].east_mm = static_cast<int32_t>(std::llround(point[0] * DIPLA_MM_PER_M));
615 reply.points[index].north_mm = static_cast<int32_t>(std::llround(point[1] * DIPLA_MM_PER_M));
616 // Predicted model Z is intentionally not an altitude command.
617 reply.points[index].height_mm = static_cast<int32_t>(std::llround(config.flight_height_m * DIPLA_MM_PER_M));
618 }
619 }
620 }
621 // Unlike SHOT, PLAN does not replay cached data for a duplicate
622 // number: it returns correlated INVALID without another observation.
623 lock.unlock();
624 // Socket I/O must not hold the planner mutex: a blocked owner may
625 // lose this worker, but must not freeze GP progress under the lock.
626 ssize_t sent;
627 // A disconnected owner is an error, not SIGPIPE termination. Retry
628 // interrupted calls only; a short packet send is a broken contract,
629 // not permission to send a second fragment as a separate message.
630 do { sent = send(descriptor, &reply, sizeof(reply), MSG_NOSIGNAL | MSG_DONTWAIT); } while (sent < 0 && errno == EINTR);
631 if (sent != sizeof(reply)) { transport_failed = true; break; }
632 continue;
633 }
634 // Unknown operations and inactive/mismatched requests never enter the GP.
635 // SHOT follows the one-point contract below, not PLAN's whole-bank format.
636 if ((request.operation != DIPLA_IPC_SHOT && request.operation != DIPLA_IPC_CREDIT_SHOT &&
637 request.operation != DIPLA_IPC_POINT) ||
638 request.session != shared.session || !request.number) continue;
639 const auto now = Clock::now();
640 dipla_reply reply{DIPLA_IPC_VERSION, request.session, request.number, 0, 0, 0, 0, 0, 0};
641 bool observation_admitted = false;
642 if (request.number == last_number) {
643 // Replay the original reply, including an originally INVALID one.
644 // Retrying is neither another independent measurement nor permission
645 // to consume the next target or upgrade an earlier reply to success.
646 if (cached.session == request.session && cached.number == request.number) reply = cached;
647 // Failure and expiry override cached success; caching is not a lease extension.
648 if (shared.failed || now - pinned_time > std::chrono::milliseconds(DIPLA_POINT_MAX_AGE_MS)) reply.valid = 0;
649 }
650 else if (request.number > last_number && pose_valid && !shared.failed) {
651 if (request.operation != DIPLA_IPC_POINT) observation_admitted = admit(request, position, now);
652 // A long retrieval gap restarts at P1. It says nothing about aircraft
653 // arrival: the slot cursor belongs to transport, not the flight plan.
654 if (now - last_request > std::chrono::milliseconds(DIPLA_CYCLE_TIMEOUT_MS)) slot = 0;
655 if (!slot && shared.generation && now - shared.plan_time <= std::chrono::milliseconds(DIPLA_POINT_MAX_AGE_MS)) {
656 // Capture route, identity, and lease together ONLY at cycle start.
657 // Later assimilation may refresh shared.plan_time, but cannot
658 // rejuvenate a bank already being delivered partway through.
659 pinned = shared.route;
660 generation = shared.generation;
661 pinned_time = shared.plan_time;
662 }
663 if (!shared.failed && generation && now - pinned_time <= std::chrono::milliseconds(DIPLA_POINT_MAX_AGE_MS)) {
664 // Invert the anchored frame consistently: north -> latitude, east ->
665 // longitude. Keep FC altitude as supplied and declare fixed AGL;
666 // neither the sampler's Z nor an invented altitude datum belongs here.
667 const auto offset = frame->physical_offset(pinned[slot]);
668 const double latitude = frame->latitude + offset[1] / frame->north_scale;
669 const double longitude = frame->longitude + offset[0] / frame->east_scale;
670 reply.valid = 1;
671 reply.lat = static_cast<int32_t>(std::llround(latitude * DIPLA_GEO_UNITS_PER_DEGREE));
672 reply.lon = static_cast<int32_t>(std::llround(longitude * DIPLA_GEO_UNITS_PER_DEGREE));
673 reply.alt_mm = request.alt_mm;
674 reply.agl_mm = static_cast<int32_t>(std::llround(config.flight_height_m * DIPLA_MM_PER_M));
675 // Internal slots 0..7 map to P1..P8. Generation distinguishes the
676 // bank; advancing the slot does not tell the aircraft where to fly next.
677 reply.metric = static_cast<int32_t>((generation << DIPLA_SLOT_BITS) | slot);
678 slot = (slot + 1) % points;
679 } else slot = generation = 0;
680 } else slot = generation = 0;
681 // Remember every newer SHOT reply, including rejection. Reusing its number
682 // must not turn an invalid attempt into fresh evidence after a pose change.
683 // Older numbers never replace this high-water mark or its replay cache.
684 const bool rejected_credit = request.operation == DIPLA_IPC_CREDIT_SHOT &&
685 ((request.number > last_number && !observation_admitted) ||
686 (request.number == last_number && !last_uses_credit));
687 if (request.number > last_number) {
688 last_number = request.number;
689 last_uses_credit = request.operation == DIPLA_IPC_CREDIT_SHOT;
690 cached = reply;
691 last_request = now;
692 }
693 lock.unlock();
694 ssize_t sent;
695 do { sent = send(descriptor, &reply, sizeof(reply), MSG_NOSIGNAL | MSG_DONTWAIT); } while (sent < 0 && errno == EINTR);
696 if (sent != sizeof(reply)) { transport_failed = true; break; }
697 if (rejected_credit) {
698 const dipla_reply acknowledgement{DIPLA_IPC_VERSION, request.session, request.number,
699 0, 0, 0, 0, 0, DIPLA_IPC_ASSIMILATED_METRIC};
700 do { sent = send(descriptor, &acknowledgement, sizeof(acknowledgement), MSG_NOSIGNAL | MSG_DONTWAIT); }
701 while (sent < 0 && errno == EINTR);
702 if (sent != sizeof(acknowledgement)) { transport_failed = true; break; }
703 }
704 }
705 // Wake an idle planner as well as one checking cooperative cancellation.
706 // Join before config/shared/reference arguments leave scope; a detached GP
707 // owner would otherwise outlive the very state that keeps it synchronized.
708 {
709 std::lock_guard<std::mutex> lock(shared.mutex);
710 shared.stopping = true;
711 shared.changed.notify_one();
712 }
713 worker.join();
714 // After join there is no concurrent writer, so reading the failure latch
715 // needs no lock. Peer closure alone is orderly; malformed transport is not.
716 return transport_failed || shared.failed ? 1 : 0;
717}
718}
Allocation-free flat key=value mechanics; callers own schemas and storage.
static bool ini_uint(const char *text, uint64_t *number)
Parse digits-only uint64; overflow and signs are rejected.
Definition bounded_ini.h:52
static bool ini_real(const char *text, double *number)
Parse finite decimal/exponent notation, rejecting range errors and suffixes.
Definition bounded_ini.h:67
static bool ini_read_file(const char *filename, char *buffer, size_t capacity, size_t *length)
Read a nonempty regular file smaller than capacity and append NUL.
Definition bounded_ini.h:98
static bool ini_parse(char *buffer, size_t length, ini_field_handler handler, void *context)
Destructively parse flat fields with blank lines and #/; comment lines.
static pthread_mutex_t guard
static double latitude
static uint32_t session
static double longitude
int descriptor
Definition chdk_pipe.c:142
static char config_path[configuration_path_capacity]
#define DIPLA_FIELD_EXTENT_M
#define DIPLA_FLIGHT_HEIGHT_MAX_MM
#define DIPLA_LATITUDE_LIMIT
#define DIPLA_FLIGHT_HEIGHT_MIN_MM
#define DIPLA_MM_PER_M
#define DIPLA_MODEL_Z_MIN_M
#define DIPLA_HEIGHT_TOLERANCE_M
#define DIPLA_DEMO_HEIGHT_MM
#define DIPLA_MODEL_Z_MAX_M
#define DIPLA_GEO_UNITS_PER_DEGREE
#define DIPLA_LONGITUDE_LIMIT
#define DIPLA_MAX_GENERATION
#define DIPLA_SLOT_BITS
#define DIPLA_AGL_UNITS_PER_M
Private local worker messages; not a flight-controller wire protocol.
#define DIPLA_IPC_CREDIT_SHOT
Definition dipla_ipc.h:32
#define DIPLA_POINT_MAX_AGE_MS
Definition dipla_ipc.h:37
#define DIPLA_IPC_POINT
Definition dipla_ipc.h:33
#define DIPLA_IPC_VERSION
Definition dipla_ipc.h:27
#define DIPLA_IPC_ASSIMILATED_METRIC
Definition dipla_ipc.h:34
#define DIPLA_IPC_START
Definition dipla_ipc.h:28
#define DIPLA_IPC_SHOT
Definition dipla_ipc.h:30
#define DIPLA_IPC_STOP
Definition dipla_ipc.h:29
#define DIPLA_CYCLE_TIMEOUT_MS
Definition dipla_ipc.h:38
#define DIPLA_IPC_PLAN
Definition dipla_ipc.h:31
Public planning, synthetic-survey, and comparison APIs for Diplacam.
Public entry point for Diplacam's bounded local IPC worker.
union dc_shot_union request
static struct inference_context network
static atomic_uint_fast64_t generation
struct jpeg_error_mgr error
Definition lwir_image.c:60
uint8_t frame[LWIR_MAX_FRAME_BYTES]
Definition lwir_mock.c:58
std::vector< Point > plan(const Grid &grid, const Vector &mean, const Matrix &covariance, const Point &start, const PlannerOptions &options)
Run greedy route selection followed by optional CMA-ES refinement.
void apply_flight_margin(Waypoints &route, unsigned margin_cells)
Apply the configured horizontal flight margin.
int serve(int descriptor, const std::string &config_path, const std::string &model, const std::string &metadata, bool cmaes, const CmaesOptions &comparison, const NetworkOptions &network_options, const ExperimentOptions &experiment)
Serve a Diplacam session over an already-connected IPC descriptor.
std::array< double, DIPLA_COORDINATE_COUNT *points > Noise
std::array< std::array< double, DIPLA_COORDINATE_COUNT >, points > Waypoints
constexpr std::size_t points
constexpr unsigned int sensor_height
Sensor-space height, separate from model and encoded-image dimensions.
static size_t count
Definition pose_log.c:79
Bounded parameters for the comparison-only CMA-ES planner.
void validate() const
Validate the complete comparison policy before planning.
Startup-only synthetic experiment settings shared by both planners.
void validate() const
Reject nonfinite values and incompatible margins before allocation.
Model identity and execution limits for a diffusion network.
int32_t east_mm
Flight-padded model X times 1000; default bounds 4000..96000.
Definition dipla_ipc.h:104
int32_t north_mm
Flight-padded model Y times 1000; default bounds 4000..96000.
Definition dipla_ipc.h:104
Complete immutable route reply on the local worker socket, not the FC bus.
Definition dipla_ipc.h:81
uint32_t generation
Monotonically increasing completed-route identity within this session.
Definition dipla_ipc.h:94
struct dipla_plan_reply::dipla_plan_point points[DIPLA_WAYPOINT_COUNT]
uint32_t version
Complete-plan layout version.
Definition dipla_ipc.h:94
uint32_t valid
One only for an entire fresh, bounded route; zero authorizes no points.
Definition dipla_ipc.h:94
uint32_t session
Exact session echo, independent of route generation.
Definition dipla_ipc.h:94
uint32_t age_ms
Milliseconds since the latest assimilated active-chunk pose was received.
Definition dipla_ipc.h:94
uint32_t count
Definition dipla_ipc.h:94
uint32_t number
Exact request echo; route data may originate from an earlier admitted pose.
Definition dipla_ipc.h:94
Legacy one-point retrieval from a generation pinned across eight slots.
Definition dipla_ipc.h:59
uint32_t number
Exact observation echo; a retry must not consume another slot.
Definition dipla_ipc.h:66
uint32_t valid
Definition dipla_ipc.h:66
int32_t agl_mm
Configured physical flight height in millimetres; default 2000.
Definition dipla_ipc.h:75
int32_t lon
Proposed geographic longitude in degrees times 1e7.
Definition dipla_ipc.h:75
int32_t lat
Proposed geographic latitude in degrees times 1e7.
Definition dipla_ipc.h:75
int32_t alt_mm
Unchanged owner-supplied FC altitude in millimetres, not predicted model Z.
Definition dipla_ipc.h:75
int32_t metric
Definition dipla_ipc.h:75
uint32_t session
Exact session echo; prevents accepting a previous sortie's completion.
Definition dipla_ipc.h:66
Pose/control request; session and observation number have separate lifetimes.
Definition dipla_ipc.h:42
One timestamped geographic position for a replay annotation.
std::chrono::steady_clock Clock
Definition survey.cpp:46