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
pose_log.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 pose_log.c
20 * @brief Bounded asynchronous CSV recorder for flight-controller pose evidence.
21 * @details The UART path uses trylock and drops rather than blocks when the writer is
22 * busy. A dedicated thread batches durable writes, preserving acquisition latency while
23 * recording queue loss and clock-alignment provenance explicitly in each row.
24 */
25#include <errno.h>
26#include <fcntl.h>
27#include <limits.h>
28#include <pthread.h>
29#include <stdatomic.h>
30#include <stdio.h>
31#include <stdlib.h>
32#include <time.h>
33#include <unistd.h>
34
35#include "pose_log.h"
36#include "usher_protocol.h"
37#include "boot_id.h"
38#include "path_utils.h"
39
40#if __BYTE_ORDER__ != __ORDER_LITTLE_ENDIAN__
41 #error "USHER pose wire decoding requires a little-endian host"
42#endif
43_Static_assert(sizeof(union usher_pose_sample_union) == USHER_POSE_SAMPLE_MSG_SIZE, "Pose wire size");
44
45#ifndef POSE_LOG_CAPACITY
46 /** Fixed record slots keep UART enqueueing allocation-free and memory-predictable. */
47 #define POSE_LOG_CAPACITY 256
48#endif
49#ifndef POSE_LOG_MAX_BYTES
50 /** Rotate by starting a new session rather than allowing unbounded storage growth. */
51 #define POSE_LOG_MAX_BYTES (64U * 1024U * 1024U)
52#endif
53/** Writer batch size amortizes durable sync calls while bounding unsynced records. */
54#define POSE_LOG_BATCH 32
55/** Pose coordinates on the wire are signed degrees scaled by 10^7. */
56#define POSE_LOG_COORDINATE_SCALE 10000000
57/** Reject sample intervals longer than one second as malformed timing evidence. */
58#define POSE_LOG_MAX_SAMPLE_WINDOW_US 1000000U
59/** CSV schema revision stored in the first column. */
60#define POSE_LOG_SCHEMA_VERSION 2
61_Static_assert(POSE_LOG_CAPACITY >= POSE_LOG_BATCH, "Pose queue must hold one batch");
62
63/** Snapshot retained until the writer has durably flushed the corresponding batch. */
71
72static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
73static pthread_cond_t available;
74static pthread_once_t condition_once = PTHREAD_ONCE_INIT;
75static int condition_error;
76static pthread_t writer;
77static FILE *output;
79static size_t head, count, bytes_written;
80static bool active, stopping;
81static uint64_t accepted, synced, rejected;
82static atomic_uint_fast64_t dropped;
83static int log_error;
84static char boot_id[37];
85
86/* The header defines schema 2's stable column order; preserve it for offline evidence readers. */
87static const char header[] =
88 "schema,mora_boot_id,receive_monotonic_us,accepted_index,queue_dropped,fc_sequence,fc_sample_begin_us,fc_sample_end_us,"
89 "next_shot_nr,lat_e7deg,lon_e7deg,ellipsoid_alt_mm,roll_bfp,pitch_bfp,yaw_bfp,ground_speed_bfp,"
90 "course_bfp,agl_bfp,velocity_north_bfp,velocity_east_bfp,velocity_down_bfp,gps_tow_ms,gps_week,"
91 "gps_hacc_cm,gps_vacc_cm,gps_sacc_cm_s,gps_fix,gps_num_sv,gps_valid_fields,flags,"
92 "clock_token_low,clock_token_high,clock_mapped,sample_earliest_monotonic_us,sample_latest_monotonic_us,"
93 "probe_sent_monotonic_us,probe_received_monotonic_us,probe_receive_fc_us,probe_transmit_fc_us,clock_drift_bound_ppm\n";
94
95/** Configure the writer wake condition against the same monotonic clock as its deadline. */
96static void initialize_condition(void)
97{
98 pthread_condattr_t attributes;
99 condition_error = pthread_condattr_init(&attributes);
100 if (condition_error != 0) return;
101 condition_error = pthread_condattr_setclock(&attributes, CLOCK_MONOTONIC);
102 if (condition_error == 0) condition_error = pthread_cond_init(&available, &attributes);
103 pthread_condattr_destroy(&attributes);
104}
105
106/** Serialize one validated wire sample and its optional clock mapping as a CSV row.
107 * @param record Queue-owned immutable snapshot.
108 * @return Zero when written, otherwise errno-style I/O or bounds error. */
109static int write_record(const struct pose_record *record)
110{
111 const union usher_pose_sample_union *sample = &record->sample;
112 const union dc_shot_union *shot = &sample->data.shot;
113 char line[1024];
114 int length = snprintf(line, sizeof(line),
115 "%d,%s,%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu32 ",%" PRIu32 ",%" PRIu32
116 ",%" PRId32 ",%" PRId32 ",%" PRId32 ",%" PRId32 ",%" PRId32 ",%" PRId32
117 ",%" PRId32 ",%" PRId32 ",%" PRId32 ",%" PRId32 ",%" PRId32 ",%" PRId32
118 ",%" PRId32 ",%" PRIu32 ",%" PRIu32 ",%" PRIu32 ",%" PRIu32 ",%" PRIu32
119 ",%" PRIu32 ",%" PRIu32 ",%" PRIu32 ",%" PRIu32,
120 POSE_LOG_SCHEMA_VERSION, boot_id, record->received_us, record->ordinal, record->dropped,
121 sample->data.sequence, sample->data.sample_begin_us, sample->data.sample_end_us,
122 shot->data.nr, shot->data.lat, shot->data.lon, shot->data.alt,
123 shot->data.phi, shot->data.theta, shot->data.psi, shot->data.vground,
124 shot->data.course, shot->data.groundalt, sample->data.velocity_north_bfp,
126 sample->data.gps_tow_ms, sample->data.gps_week, sample->data.gps_hacc_cm,
127 sample->data.gps_vacc_cm, sample->data.gps_sacc_cm_s, sample->data.gps_fix,
128 sample->data.gps_num_sv, sample->data.gps_valid_fields, sample->data.flags);
129 if (length < 0 || (size_t)length >= sizeof(line)) return EOVERFLOW;
130 const struct pose_clock_evidence *clock = &record->clock;
131 int extra = snprintf(line + length, sizeof(line) - (size_t)length,
132 ",%" PRIu32 ",%" PRIu32 ",%d,%" PRIu64 ",%" PRIu64 ",%" PRIu64
133 ",%" PRIu64 ",%" PRIu32 ",%" PRIu32 ",%" PRIu64 "\n",
134 clock->token.data.token_low, clock->token.data.token_high, clock->mapped,
136 clock->probe_sent_us, clock->probe_received_us,
138 if (extra < 0 || (size_t)extra >= sizeof(line) - (size_t)length) return EOVERFLOW;
139 length += extra;
140 if (bytes_written > POSE_LOG_MAX_BYTES || (size_t)length > POSE_LOG_MAX_BYTES - bytes_written) return EFBIG;
141 if (fwrite(line, 1, (size_t)length, output) != (size_t)length) return errno ? errno : EIO;
142 bytes_written += (size_t)length;
143 return 0;
144}
145
146/** Drain bounded batches off the UART thread and fdatasync before counting them synced.
147 * @param unused pthread entry-point argument; unused. */
148static void *write_queue(void *unused)
149{
150 (void)unused;
151 for (;;) {
152 struct pose_record batch[POSE_LOG_BATCH];
153 struct timespec deadline;
154 if (clock_gettime(CLOCK_MONOTONIC, &deadline) != 0) {
155 int error = errno;
156 pthread_mutex_lock(&mutex);
158 pthread_mutex_unlock(&mutex);
159 return NULL;
160 }
161 ++deadline.tv_sec;
162 pthread_mutex_lock(&mutex);
163 while (!stopping && count < POSE_LOG_BATCH) {
164 int result = pthread_cond_timedwait(&available, &mutex, &deadline);
165 if (result == ETIMEDOUT) break;
166 if (result != 0) {
168 pthread_mutex_unlock(&mutex);
169 return NULL;
170 }
171 }
172 if (stopping && count == 0) {
173 pthread_mutex_unlock(&mutex);
174 return NULL;
175 }
176 size_t batch_count = 0;
177 while (count > 0 && batch_count < POSE_LOG_BATCH) {
178 batch[batch_count++] = queue[head];
179 head = (head + 1) % POSE_LOG_CAPACITY;
180 --count;
181 }
182 pthread_mutex_unlock(&mutex);
183 if (batch_count == 0) continue;
184 int error = 0;
185 for (size_t index = 0; index < batch_count && error == 0; ++index) error = write_record(&batch[index]);
186 if (error == 0 && (fflush(output) != 0 || fdatasync(fileno(output)) != 0)) error = errno ? errno : EIO;
187 pthread_mutex_lock(&mutex);
188 if (error == 0) synced += batch_count;
189 else log_error = error;
190 pthread_mutex_unlock(&mutex);
191 if (error != 0) return NULL;
192 }
193}
194
195/** Start one durable CSV session; an empty path leaves optional logging disabled.
196 * @return Zero on active/disabled success, otherwise -1 with errno set. */
197int pose_log_start(const char *directory)
198{
199 pthread_mutex_lock(&mutex);
200 if (active) {
201 pthread_mutex_unlock(&mutex);
202 errno = EBUSY;
203 return -1;
204 }
205 head = count = bytes_written = 0;
206 accepted = synced = rejected = 0;
207 atomic_store(&dropped, 0);
208 log_error = 0;
209 stopping = false;
210 if (directory == NULL || directory[0] == '\0') {
211 pthread_mutex_unlock(&mutex);
212 return 0;
213 }
214 int initialization = pthread_once(&condition_once, initialize_condition);
215 if (initialization != 0 || condition_error != 0) {
216 log_error = initialization != 0 ? initialization : condition_error;
217 pthread_mutex_unlock(&mutex);
218 errno = log_error;
219 return -1;
220 }
221 char path[PATH_MAX] = {0};
222 char resolved_dir[PATH_MAX];
223 const char *target_dir = usher_resolve_path(directory, resolved_dir, sizeof(resolved_dir));
224 usher_ensure_directory(target_dir);
225
227 char stamp[32];
228 struct tm utc;
229 time_t now = time(NULL);
230 if (gmtime_r(&now, &utc) == NULL || strftime(stamp, sizeof(stamp), "%Y%m%dT%H%M%SZ", &utc) == 0) {
231 pthread_mutex_unlock(&mutex);
232 errno = EINVAL;
233 return -1;
234 }
235 char temporary_path[PATH_MAX] = {0};
236 int length = snprintf(temporary_path, sizeof(temporary_path), "%s/pose-%s-XXXXXX", target_dir, stamp);
237 if (length < 0 || (size_t)length >= sizeof(temporary_path)) {
238 pthread_mutex_unlock(&mutex);
239 errno = ENAMETOOLONG;
240 return -1;
241 }
242 int directory_fd = open(target_dir, O_RDONLY | O_DIRECTORY | O_CLOEXEC);
243 if (directory_fd < 0) {
244 log_error = errno;
245 pthread_mutex_unlock(&mutex);
246 return -1;
247 }
248 int descriptor = mkstemp(temporary_path);
249 bool renamed = false;
250 int error = descriptor < 0 ? errno : 0;
251 if (error == 0 && fcntl(descriptor, F_SETFD, FD_CLOEXEC) != 0) error = errno;
252 if (error == 0) {
253 length = snprintf(path, sizeof(path), "%s.csv", temporary_path);
254 if (length < 0 || (size_t)length >= sizeof(path)) error = ENAMETOOLONG;
255 else if (rename(temporary_path, path) != 0) error = errno;
256 else renamed = true;
257 }
258 if (error == 0) {
259 output = fdopen(descriptor, "w");
260 if (output == NULL) {
261 error = errno;
262 }
263 }
264 if (error == 0) {
265 bytes_written = sizeof(header) - 1;
267 else if (fwrite(header, 1, bytes_written, output) != bytes_written || fflush(output) != 0
268 || fsync(fileno(output)) != 0 || fsync(directory_fd) != 0) error = errno ? errno : EIO;
269 }
270 close(directory_fd);
271 if (error == 0) error = pthread_create(&writer, NULL, write_queue, NULL);
272 if (error != 0) {
273 if (output != NULL) fclose(output);
274 else if (descriptor >= 0) close(descriptor);
275 output = NULL;
276 if (descriptor >= 0) {
277 unlink(temporary_path);
278 if (renamed) unlink(path);
279 }
281 pthread_mutex_unlock(&mutex);
282 errno = error;
283 return -1;
284 }
285 active = true;
286 pthread_mutex_unlock(&mutex);
287 fprintf(stderr, "USHER POSE: logging to %s\n", path);
288 return 0;
289}
290
291/** Enqueue a raw pose with no mapped-clock evidence.
292 * @param payload Wire-format USHER_POSE_SAMPLE bytes.
293 * @param length Must equal the packed message size.
294 * @param receive_monotonic_us UART receive timestamp from CLOCK_MONOTONIC.
295 * @return 1 queued, 0 best-effort drop, or -1 invalid payload. */
296int pose_log_record(const uint8_t *payload, size_t length, uint64_t receive_monotonic_us)
297{
298 return pose_log_record_clocked(payload, length, receive_monotonic_us, NULL);
299}
300
301/** Validate and enqueue a pose with optional token-bound FC/MORA clock evidence.
302 * @param payload Wire-format USHER_POSE_SAMPLE bytes.
303 * @param length Must equal the packed message size.
304 * @param receive_monotonic_us UART receive timestamp from CLOCK_MONOTONIC.
305 * @param evidence Optional probe and mapped interval copied into this row.
306 * @return 1 queued, 0 best-effort drop, or -1 malformed timing/pose data. */
307int pose_log_record_clocked(const uint8_t *payload, size_t length, uint64_t receive_monotonic_us,
308 const struct pose_clock_evidence *evidence)
309{
310 if (pthread_mutex_trylock(&mutex) != 0) {
311 atomic_fetch_add(&dropped, 1);
312 return 0;
313 }
314 if (!active || stopping) {
315 pthread_mutex_unlock(&mutex);
316 return 0;
317 }
318 union usher_pose_sample_union sample = {0};
319 bool valid = payload != NULL && length == sizeof(sample.bin) && receive_monotonic_us != 0;
320 if (valid) {
321 for (size_t index = 0; index < sizeof(sample.bin); ++index) sample.bin[index] = payload[index];
324 && sample.data.shot.data.lat >= -90 * POSE_LOG_COORDINATE_SCALE
325 && sample.data.shot.data.lat <= 90 * POSE_LOG_COORDINATE_SCALE
326 && sample.data.shot.data.lon >= -180 * POSE_LOG_COORDINATE_SCALE
327 && sample.data.shot.data.lon <= 180 * POSE_LOG_COORDINATE_SCALE;
328 }
329 if (valid && evidence != NULL && evidence->mapped) {
330 valid = (evidence->token.data.token_low != 0 || evidence->token.data.token_high != 0)
331 && evidence->sample_time.earliest_us > 0
332 && evidence->sample_time.earliest_us <= evidence->sample_time.latest_us
333 && evidence->sample_time.latest_us <= receive_monotonic_us
334 && evidence->probe_sent_us > 0 && evidence->probe_sent_us <= evidence->probe_received_us
335 && evidence->probe_received_us <= receive_monotonic_us;
336 }
337 if (!valid) {
338 ++rejected;
339 pthread_mutex_unlock(&mutex);
340 return -1;
341 }
342 if (log_error != 0 || count == POSE_LOG_CAPACITY) {
343 atomic_fetch_add(&dropped, 1);
344 pthread_mutex_unlock(&mutex);
345 return 0;
346 }
348 record->sample = sample;
349 record->received_us = receive_monotonic_us;
350 record->ordinal = ++accepted;
351 record->dropped = atomic_load(&dropped);
352 record->clock = evidence != NULL ? *evidence : (struct pose_clock_evidence) {0};
353 if (!record->clock.mapped) {
354 const union usher_clock_request_union token = record->clock.token;
355 record->clock = (struct pose_clock_evidence) {.token = token};
356 }
357 ++count;
358 pthread_cond_signal(&available);
359 pthread_mutex_unlock(&mutex);
360 return 1;
361}
362
363/** Return a mutex-consistent snapshot of accepted, durable, dropped and rejected records.
364 * @return Statistics from the current logging session. */
366{
367 pthread_mutex_lock(&mutex);
368 struct pose_log_stats status = {accepted, synced, atomic_load(&dropped), rejected, log_error};
369 pthread_mutex_unlock(&mutex);
370 return status;
371}
372
373/** Stop accepting records, drain the queue, join the writer, and close the CSV.
374 * @return Zero after orderly durable shutdown, otherwise -1 with errno set. */
376{
377 pthread_mutex_lock(&mutex);
378 if (!active) {
379 pthread_mutex_unlock(&mutex);
380 return 0;
381 }
382 stopping = true;
383 pthread_cond_signal(&available);
384 pthread_mutex_unlock(&mutex);
385 int error = pthread_join(writer, NULL);
386 if (error != 0) {
387 errno = error;
388 return -1;
389 }
390 if (fclose(output) != 0) error = errno ? errno : EIO;
391 pthread_mutex_lock(&mutex);
392 output = NULL;
393 active = false;
394 if (log_error == 0) log_error = error;
396 pthread_mutex_unlock(&mutex);
397 if (error != 0) errno = error;
398 return error == 0 ? 0 : -1;
399}
Read Linux's boot-unique identifier for pose-log session provenance.
static void usher_boot_id(char result[37])
Fill a 36-character boot UUID or the stable fallback "unknown".
Definition boot_id.h:36
static struct capture_intent active[2 *COVERAGE_CAMERAS]
int descriptor
Definition chdk_pipe.c:142
void(* close)(struct usbfs_handle *)
Definition chdkbridge.c:57
int(* open)(const struct usbfs_device *, struct usbfs_handle *)
Definition chdkbridge.c:56
#define CLOCK_ALIGNMENT_DRIFT_PPM
static int record(const union dc_shot_union *shot)
Retrieve a point and admit a new observation only when its credit is free.
static bool stopping
char evidence[8192]
Definition lwir_image.c:66
struct jpeg_error_mgr error
Definition lwir_image.c:60
Small path and writable-directory helpers shared by USHER backends.
static const char * usher_resolve_path(const char *path, char *buffer, size_t buffer_size)
Expand a leading home-directory shorthand without allocating memory.
Definition path_utils.h:74
#define PATH_MAX
Definition path_utils.h:39
static int usher_ensure_directory(const char *path)
Create missing directory components and verify the final directory is writable.
Definition path_utils.h:97
static pthread_cond_t available
Definition pose_log.c:73
int pose_log_record(const uint8_t *payload, size_t length, uint64_t receive_monotonic_us)
Validate and enqueue a raw pose payload without clock mapping evidence.
Definition pose_log.c:296
static const char header[]
Definition pose_log.c:87
#define POSE_LOG_MAX_SAMPLE_WINDOW_US
Definition pose_log.c:58
static char boot_id[37]
Definition pose_log.c:84
int pose_log_record_clocked(const uint8_t *payload, size_t length, uint64_t receive_monotonic_us, const struct pose_clock_evidence *evidence)
Validate and enqueue a pose payload with optional mapped timing provenance.
Definition pose_log.c:307
static int condition_error
Definition pose_log.c:75
static pthread_mutex_t mutex
Definition pose_log.c:72
static atomic_uint_fast64_t dropped
Definition pose_log.c:82
static uint64_t rejected
Definition pose_log.c:81
int pose_log_start(const char *directory)
Start a durable asynchronous CSV session, or disable logging when directory is empty.
Definition pose_log.c:197
int pose_log_stop(void)
Flush queued records, join the writer, and close the CSV file.
Definition pose_log.c:375
static void initialize_condition(void)
Definition pose_log.c:96
static size_t head
Definition pose_log.c:79
static size_t count
Definition pose_log.c:79
static uint64_t accepted
Definition pose_log.c:81
static int log_error
Definition pose_log.c:83
static pthread_t writer
Definition pose_log.c:76
struct pose_log_stats pose_log_status(void)
Return a synchronized snapshot of logging health and loss counters.
Definition pose_log.c:365
static int write_record(const struct pose_record *record)
Definition pose_log.c:109
#define POSE_LOG_SCHEMA_VERSION
Definition pose_log.c:60
static FILE * output
Definition pose_log.c:77
#define POSE_LOG_MAX_BYTES
Definition pose_log.c:51
static pthread_once_t condition_once
Definition pose_log.c:74
#define POSE_LOG_COORDINATE_SCALE
Definition pose_log.c:56
static struct pose_record queue[POSE_LOG_CAPACITY]
Definition pose_log.c:78
static size_t bytes_written
Definition pose_log.c:79
static uint64_t synced
Definition pose_log.c:81
static void * write_queue(void *unused)
Definition pose_log.c:148
#define POSE_LOG_BATCH
Definition pose_log.c:54
#define POSE_LOG_CAPACITY
Definition pose_log.c:47
Non-blocking pose-evidence CSV logging API.
Optional token-bound evidence mapping a controller pose interval to MORA time.
Definition pose_log.h:29
union usher_clock_request_union token
Definition pose_log.h:30
uint64_t probe_sent_us
Definition pose_log.h:32
uint32_t probe_transmit_fc_us
Definition pose_log.h:35
uint64_t probe_received_us
Definition pose_log.h:33
struct clock_interval sample_time
Definition pose_log.h:31
uint32_t probe_receive_fc_us
Definition pose_log.h:34
Snapshot counters for accepted, flushed, dropped, and rejected pose records.
Definition pose_log.h:40
uint64_t ordinal
Definition pose_log.c:67
union usher_pose_sample_union sample
Definition pose_log.c:65
uint64_t dropped
Definition pose_log.c:68
struct pose_clock_evidence clock
Definition pose_log.c:69
uint64_t received_us
Definition pose_log.c:66
struct dc_shot_union::@351207206061007345226276304225222200157171312155 data
struct usher_clock_request_union::@124322121054031044024143305010352315355331137256 data
uint8_t bin[USHER_POSE_SAMPLE_MSG_SIZE]
union dc_shot_union shot
struct usher_pose_sample_union::@042015206147200076364072004013147106310343341221 data
bool valid
Definition usher.c:537
struct usher_control_result result
Definition usher.c:377
union dc_shot_union shot
Definition usher.c:534
Shared FC/MORA byte-stream UART framing, messages and payload layouts.
static bool usher_pose_flags_valid(uint32_t flags)
Check the structural consistency of pose-sample GPS flags.
#define USHER_POSE_SAMPLE_MSG_SIZE