retain telemetry across transport outages
This commit is contained in:
@@ -66,10 +66,12 @@ overruns. Hardware data-ready interrupts and FIFO acquisition are deferred to th
|
|||||||
later sensor-side acquisition refinement.
|
later sensor-side acquisition refinement.
|
||||||
|
|
||||||
Completed samples enter a 512-record RAM queue, providing 5.12 seconds of
|
Completed samples enter a 512-record RAM queue, providing 5.12 seconds of
|
||||||
transport-stall tolerance at 100 Hz. A lower-priority output task
|
blocked or disconnected transport tolerance at 100 Hz. A failed write retains
|
||||||
batches up to eight records into versioned `TRK1` frames, isolating acquisition
|
and retries its packet while this queue accumulates the backlog. A lower-priority
|
||||||
from brief USB or future BLE stalls. CRC, packet and sample sequences, timestamps,
|
output task batches up to eight records into versioned `TRK1` frames, isolating
|
||||||
and cumulative loss/overrun counters make loss detectable.
|
acquisition from brief USB or future BLE stalls. CRC, packet and sample sequences,
|
||||||
|
timestamps, and cumulative loss/overrun counters make any queue overflow
|
||||||
|
detectable.
|
||||||
|
|
||||||
Measured end-to-end framing overhead is about 2.47 kB/s at 100 Hz, or 8.47
|
Measured end-to-end framing overhead is about 2.47 kB/s at 100 Hz, or 8.47
|
||||||
MiB/hour before BLE link overhead.
|
MiB/hour before BLE link overhead.
|
||||||
@@ -123,7 +125,7 @@ Status bits:
|
|||||||
|
|
||||||
The binary capture tool auto-detects a single `/dev/cu.usbmodem*` device, stores
|
The binary capture tool auto-detects a single `/dev/cu.usbmodem*` device, stores
|
||||||
only CRC-valid frames, renders CSV, and reports packet, sample, timing, status,
|
only CRC-valid frames, renders CSV, and reports packet, sample, timing, status,
|
||||||
drop, and overrun totals:
|
drop, overrun, timestamp-saturation, and trailing-partial-byte totals:
|
||||||
|
|
||||||
```sh
|
```sh
|
||||||
python tools/capture_binary.py
|
python tools/capture_binary.py
|
||||||
|
|||||||
@@ -19,7 +19,7 @@ combined by an underlying byte transport.
|
|||||||
| 10 | 2 | Payload size |
|
| 10 | 2 | Payload size |
|
||||||
| 12 | 4 | Monotonic packet sequence |
|
| 12 | 4 | Monotonic packet sequence |
|
||||||
| 16 | 8 | Base ESP timer timestamp in microseconds |
|
| 16 | 8 | Base ESP timer timestamp in microseconds |
|
||||||
| 24 | 4 | Cumulative samples lost to read failure, queue overflow, or output failure |
|
| 24 | 4 | Cumulative samples lost to sensor read failure or queue overflow |
|
||||||
| 28 | 4 | Cumulative acquisition-loop overruns |
|
| 28 | 4 | Cumulative acquisition-loop overruns |
|
||||||
| 32 | 4 | IEEE CRC-32 |
|
| 32 | 4 | IEEE CRC-32 |
|
||||||
|
|
||||||
@@ -27,7 +27,9 @@ CRC uses polynomial `0xEDB88320`, initial value `0xFFFFFFFF`, and final XOR
|
|||||||
`0xFFFFFFFF`. It covers header bytes 4–31 followed by the complete payload. The
|
`0xFFFFFFFF`. It covers header bytes 4–31 followed by the complete payload. The
|
||||||
magic and stored CRC field are excluded.
|
magic and stored CRC field are excluded.
|
||||||
|
|
||||||
Packet flag bit 0 means at least one sample timestamp delta saturated.
|
Packet flag bit 0 means at least one sample timestamp delta saturated. Both host
|
||||||
|
tools report any frame carrying this flag instead of silently treating its
|
||||||
|
reconstructed timestamps as exact.
|
||||||
|
|
||||||
## Sample record (20 bytes)
|
## Sample record (20 bytes)
|
||||||
|
|
||||||
@@ -45,9 +47,11 @@ Packet flag bit 0 means at least one sample timestamp delta saturated.
|
|||||||
| 19 | 1 | Raw L3G4200D `STATUS_REG` |
|
| 19 | 1 | Raw L3G4200D `STATUS_REG` |
|
||||||
|
|
||||||
The first record has delta zero and uses the frame's base timestamp. Each later
|
The first record has delta zero and uses the frame's base timestamp. Each later
|
||||||
timestamp is reconstructed by cumulatively adding its delta. A delta that cannot
|
timestamp is reconstructed by cumulatively adding its delta. Firmware ends the
|
||||||
fit is stored as `0xFFFF` and sets packet flag bit 0. Sample sequence gaps remain
|
current packet before a delta exceeds the representable 655.35 ms range, making
|
||||||
detectable independently.
|
the next sample the exact base timestamp of a new packet. As a defensive encoder
|
||||||
|
fallback, an unrepresentable delta is stored as `0xFFFF` and sets packet flag bit
|
||||||
|
0. Sample sequence gaps remain detectable independently.
|
||||||
|
|
||||||
Mapped raw counts are authoritative. The original sensor-native axes can be
|
Mapped raw counts are authoritative. The original sensor-native axes can be
|
||||||
reconstructed because the mappings are lossless:
|
reconstructed because the mappings are lossless:
|
||||||
@@ -77,5 +81,12 @@ before that first valid frame separately from CRC failures after synchronization
|
|||||||
Acquisition runs in a dedicated higher-priority task and writes complete samples
|
Acquisition runs in a dedicated higher-priority task and writes complete samples
|
||||||
to a 512-entry RAM queue. The lower-priority output task batches up to eight
|
to a 512-entry RAM queue. The lower-priority output task batches up to eight
|
||||||
records per frame. At 100 Hz this queue represents about 5.12 seconds of
|
records per frame. At 100 Hz this queue represents about 5.12 seconds of
|
||||||
decoupling from a blocked transport. Queue overflow never overwrites an older
|
decoupling from a blocked or disconnected transport. A failed write retains and
|
||||||
sample silently: sequence gaps and the cumulative lost-sample counter expose it.
|
retries the same encoded packet rather than dequeuing more samples, so the queue
|
||||||
|
accumulates the outage backlog. After reconnection, the oldest retained data is
|
||||||
|
sent first. If the queue fills, acquisition drops new samples rather than
|
||||||
|
overwriting older ones; sequence gaps and the cumulative lost-sample counter
|
||||||
|
expose that permanent loss.
|
||||||
|
|
||||||
|
Receivers report bytes left in an incomplete trailing frame when capture ends.
|
||||||
|
Those bytes cannot pass CRC validation and are not silently admitted as samples.
|
||||||
|
|||||||
@@ -11,6 +11,8 @@ data.
|
|||||||
- Host compilation of the production C encoder with `-Wall -Wextra -Werror`
|
- Host compilation of the production C encoder with `-Wall -Wextra -Werror`
|
||||||
- Fragmented C-encoder-to-Python-parser contract test
|
- Fragmented C-encoder-to-Python-parser contract test
|
||||||
- Deliberately corrupted CRC test with stream resynchronization
|
- Deliberately corrupted CRC test with stream resynchronization
|
||||||
|
- Full eight-record, timestamp-saturation, invalid-size/count, and truncated-tail
|
||||||
|
encoder/parser contract cases
|
||||||
- ESP-IDF firmware build and flash on the assembled ESP32-C3 prototype
|
- ESP-IDF firmware build and flash on the assembled ESP32-C3 prototype
|
||||||
- Live USB capture followed by independent offline re-decoding
|
- Live USB capture followed by independent offline re-decoding
|
||||||
|
|
||||||
@@ -55,3 +57,26 @@ at priority 5 and receives samples through a 512-entry queue (about 5.12 seconds
|
|||||||
at 100 Hz). The hardware capture's zero timing anomalies and zero loop overruns
|
at 100 Hz). The hardware capture's zero timing anomalies and zero loop overruns
|
||||||
confirm that packet encoding, CRC, float metadata, and USB output did not disturb
|
confirm that packet encoding, CRC, float metadata, and USB output did not disturb
|
||||||
the acquisition cadence.
|
the acquisition cadence.
|
||||||
|
|
||||||
|
Output failure is transactional: firmware retains and retries the same encoded
|
||||||
|
packet with a scheduler delay instead of discarding it or dequeuing more samples.
|
||||||
|
The queue therefore accumulates a disconnected-transport backlog. If an outage
|
||||||
|
outlasts the queue, acquisition drops and counts new samples while preserving the
|
||||||
|
oldest queued data for ordered delivery after reconnection.
|
||||||
|
|
||||||
|
## Forced transport-outage validation
|
||||||
|
|
||||||
|
A temporary validation build made the packet writer report transport failure for
|
||||||
|
a fixed interval while acquisition continued normally. The failure injection was
|
||||||
|
removed before the production build.
|
||||||
|
|
||||||
|
With a three-second forced outage, all 2,144 observed samples arrived contiguously
|
||||||
|
from sequence 0 through 2,143. Packet gaps, sample gaps, drops, loop overruns, and
|
||||||
|
timestamp-saturation flags were all zero.
|
||||||
|
|
||||||
|
With a seven-second forced outage, the queue preserved samples 0 through 511 and
|
||||||
|
then dropped 138 new samples after reaching capacity. Delivery resumed at sample
|
||||||
|
650. The cumulative drop count and observed sequence gap both equaled 138. The
|
||||||
|
timestamp difference from sample 511 to 650 was exactly 1,390,000 us, matching
|
||||||
|
139 sample intervals, and no saturation flag was emitted. This verifies both the
|
||||||
|
oldest-data retention policy and the new exact timestamp re-anchor after overflow.
|
||||||
|
|||||||
+20
-3
@@ -85,6 +85,18 @@ static void encode_header(uint8_t *output,
|
|||||||
put_u32_le(output + 32, 0);
|
put_u32_le(output + 32, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool trikke_wire_timestamp_delta_fits(
|
||||||
|
int64_t previous_timestamp_us,
|
||||||
|
int64_t timestamp_us)
|
||||||
|
{
|
||||||
|
if (timestamp_us < previous_timestamp_us) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
const uint64_t delta_us =
|
||||||
|
(uint64_t)timestamp_us - (uint64_t)previous_timestamp_us;
|
||||||
|
return delta_us <= TRIKKE_WIRE_MAX_TIMESTAMP_DELTA_US;
|
||||||
|
}
|
||||||
|
|
||||||
size_t trikke_encode_metadata_packet(
|
size_t trikke_encode_metadata_packet(
|
||||||
uint8_t *output,
|
uint8_t *output,
|
||||||
size_t output_size,
|
size_t output_size,
|
||||||
@@ -159,12 +171,17 @@ size_t trikke_encode_sample_packet(
|
|||||||
i * TRIKKE_WIRE_SAMPLE_RECORD_SIZE;
|
i * TRIKKE_WIRE_SAMPLE_RECORD_SIZE;
|
||||||
uint16_t timestamp_delta_10us = 0;
|
uint16_t timestamp_delta_10us = 0;
|
||||||
if (i > 0) {
|
if (i > 0) {
|
||||||
const int64_t delta_us = samples[i].timestamp_us - previous_timestamp_us;
|
if (!trikke_wire_timestamp_delta_fits(
|
||||||
if (delta_us < 0 || delta_us > (int64_t)UINT16_MAX * 10) {
|
previous_timestamp_us, samples[i].timestamp_us)) {
|
||||||
timestamp_delta_10us = UINT16_MAX;
|
timestamp_delta_10us = UINT16_MAX;
|
||||||
packet_flags |= TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED;
|
packet_flags |= TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED;
|
||||||
} else {
|
} else {
|
||||||
timestamp_delta_10us = (uint16_t)((delta_us + 5) / 10);
|
const uint64_t delta_us =
|
||||||
|
(uint64_t)samples[i].timestamp_us -
|
||||||
|
(uint64_t)previous_timestamp_us;
|
||||||
|
timestamp_delta_10us = (uint16_t)(
|
||||||
|
(delta_us + TRIKKE_WIRE_TIMESTAMP_DELTA_UNIT_US / 2) /
|
||||||
|
TRIKKE_WIRE_TIMESTAMP_DELTA_UNIT_US);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
#include <stddef.h>
|
#include <stddef.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
@@ -16,6 +17,9 @@
|
|||||||
#define TRIKKE_PACKET_TYPE_SAMPLES 2
|
#define TRIKKE_PACKET_TYPE_SAMPLES 2
|
||||||
|
|
||||||
#define TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED 0x01
|
#define TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED 0x01
|
||||||
|
#define TRIKKE_WIRE_TIMESTAMP_DELTA_UNIT_US 10
|
||||||
|
#define TRIKKE_WIRE_MAX_TIMESTAMP_DELTA_US \
|
||||||
|
((uint64_t)UINT16_MAX * TRIKKE_WIRE_TIMESTAMP_DELTA_UNIT_US)
|
||||||
|
|
||||||
#define TRIKKE_METADATA_FLAG_ACCEL_Y_NEGX_Z 0x0001
|
#define TRIKKE_METADATA_FLAG_ACCEL_Y_NEGX_Z 0x0001
|
||||||
#define TRIKKE_METADATA_FLAG_GYRO_IDENTITY 0x0002
|
#define TRIKKE_METADATA_FLAG_GYRO_IDENTITY 0x0002
|
||||||
@@ -46,6 +50,10 @@ typedef struct {
|
|||||||
float gyro_mdps_per_lsb;
|
float gyro_mdps_per_lsb;
|
||||||
} trikke_wire_metadata_t;
|
} trikke_wire_metadata_t;
|
||||||
|
|
||||||
|
bool trikke_wire_timestamp_delta_fits(
|
||||||
|
int64_t previous_timestamp_us,
|
||||||
|
int64_t timestamp_us);
|
||||||
|
|
||||||
size_t trikke_encode_metadata_packet(
|
size_t trikke_encode_metadata_packet(
|
||||||
uint8_t *output,
|
uint8_t *output,
|
||||||
size_t output_size,
|
size_t output_size,
|
||||||
|
|||||||
+37
-12
@@ -24,6 +24,7 @@
|
|||||||
#define TRIKKE_SAMPLE_TICKS pdMS_TO_TICKS(1000 / TRIKKE_SAMPLE_RATE_HZ)
|
#define TRIKKE_SAMPLE_TICKS pdMS_TO_TICKS(1000 / TRIKKE_SAMPLE_RATE_HZ)
|
||||||
#define TRIKKE_SAMPLE_QUEUE_DEPTH 512
|
#define TRIKKE_SAMPLE_QUEUE_DEPTH 512
|
||||||
#define TRIKKE_METADATA_INTERVAL_PACKETS 64
|
#define TRIKKE_METADATA_INTERVAL_PACKETS 64
|
||||||
|
#define TRIKKE_TRANSPORT_RETRY_DELAY_MS 10
|
||||||
|
|
||||||
// Software calibration from the 2026-08-17 enclosure six-face capture.
|
// Software calibration from the 2026-08-17 enclosure six-face capture.
|
||||||
// Accelerometer coefficients are measured. Gyroscope scale is nominal; its
|
// Accelerometer coefficients are measured. Gyroscope scale is nominal; its
|
||||||
@@ -158,14 +159,24 @@ static void acquisition_task(void *argument)
|
|||||||
|
|
||||||
static bool write_binary_packet(const uint8_t *packet, size_t packet_size)
|
static bool write_binary_packet(const uint8_t *packet, size_t packet_size)
|
||||||
{
|
{
|
||||||
const bool complete = fwrite(packet, 1, packet_size, stdout) == packet_size;
|
const size_t written = fwrite(packet, 1, packet_size, stdout);
|
||||||
fflush(stdout);
|
const int flush_result = fflush(stdout);
|
||||||
|
const bool complete = written == packet_size && flush_result == 0;
|
||||||
if (!complete) {
|
if (!complete) {
|
||||||
clearerr(stdout);
|
clearerr(stdout);
|
||||||
}
|
}
|
||||||
return complete;
|
return complete;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void write_binary_packet_until_sent(
|
||||||
|
const uint8_t *packet,
|
||||||
|
size_t packet_size)
|
||||||
|
{
|
||||||
|
while (!write_binary_packet(packet, packet_size)) {
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(TRIKKE_TRANSPORT_RETRY_DELAY_MS));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
static void output_task(void *argument)
|
static void output_task(void *argument)
|
||||||
{
|
{
|
||||||
trikke_context_t *context = argument;
|
trikke_context_t *context = argument;
|
||||||
@@ -179,7 +190,7 @@ static void output_task(void *argument)
|
|||||||
packet, sizeof(packet), packet_sequence++, esp_timer_get_time(),
|
packet, sizeof(packet), packet_sequence++, esp_timer_get_time(),
|
||||||
atomic_load(&context->dropped_sample_count),
|
atomic_load(&context->dropped_sample_count),
|
||||||
atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA);
|
atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA);
|
||||||
write_binary_packet(packet, packet_size);
|
write_binary_packet_until_sent(packet, packet_size);
|
||||||
|
|
||||||
while (true) {
|
while (true) {
|
||||||
trikke_wire_sample_t samples[TRIKKE_WIRE_MAX_RECORDS] = {0};
|
trikke_wire_sample_t samples[TRIKKE_WIRE_MAX_RECORDS] = {0};
|
||||||
@@ -190,9 +201,21 @@ static void output_task(void *argument)
|
|||||||
}
|
}
|
||||||
++sample_count;
|
++sample_count;
|
||||||
|
|
||||||
while (sample_count < TRIKKE_WIRE_MAX_RECORDS &&
|
while (sample_count < TRIKKE_WIRE_MAX_RECORDS) {
|
||||||
xQueueReceive(context->sample_queue, &samples[sample_count],
|
trikke_wire_sample_t next_sample = {0};
|
||||||
pdMS_TO_TICKS(15)) == pdPASS) {
|
if (xQueuePeek(context->sample_queue, &next_sample,
|
||||||
|
pdMS_TO_TICKS(15)) != pdPASS) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (!trikke_wire_timestamp_delta_fits(
|
||||||
|
samples[sample_count - 1].timestamp_us,
|
||||||
|
next_sample.timestamp_us)) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (xQueueReceive(context->sample_queue, &samples[sample_count], 0) !=
|
||||||
|
pdPASS) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
++sample_count;
|
++sample_count;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -202,16 +225,14 @@ static void output_task(void *argument)
|
|||||||
packet, sizeof(packet), packet_sequence++, esp_timer_get_time(),
|
packet, sizeof(packet), packet_sequence++, esp_timer_get_time(),
|
||||||
atomic_load(&context->dropped_sample_count),
|
atomic_load(&context->dropped_sample_count),
|
||||||
atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA);
|
atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA);
|
||||||
write_binary_packet(packet, packet_size);
|
write_binary_packet_until_sent(packet, packet_size);
|
||||||
}
|
}
|
||||||
|
|
||||||
packet_size = trikke_encode_sample_packet(
|
packet_size = trikke_encode_sample_packet(
|
||||||
packet, sizeof(packet), packet_sequence++,
|
packet, sizeof(packet), packet_sequence++,
|
||||||
atomic_load(&context->dropped_sample_count),
|
atomic_load(&context->dropped_sample_count),
|
||||||
atomic_load(&context->loop_overrun_count), samples, sample_count);
|
atomic_load(&context->loop_overrun_count), samples, sample_count);
|
||||||
if (!write_binary_packet(packet, packet_size)) {
|
write_binary_packet_until_sent(packet, packet_size);
|
||||||
atomic_fetch_add(&context->dropped_sample_count, sample_count);
|
|
||||||
}
|
|
||||||
++sample_packet_count;
|
++sample_packet_count;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -232,8 +253,12 @@ static esp_err_t init_i2c(i2c_master_bus_handle_t *bus)
|
|||||||
|
|
||||||
void app_main(void)
|
void app_main(void)
|
||||||
{
|
{
|
||||||
// Flush startup text by line; binary frames are flushed explicitly.
|
// Keep the byte stream unbuffered so a write result describes the complete
|
||||||
setvbuf(stdout, NULL, _IOLBF, 0);
|
// packet rather than bytes still retained inside stdio.
|
||||||
|
if (setvbuf(stdout, NULL, _IONBF, 0) != 0) {
|
||||||
|
ESP_LOGE(TAG, "failed to configure unbuffered telemetry output");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
ESP_LOGI(TAG, "Trikke motion telemetry prototype v0");
|
ESP_LOGI(TAG, "Trikke motion telemetry prototype v0");
|
||||||
ESP_LOGI(TAG, "I2C: SDA=GPIO%d, SCL=GPIO%d, clock=%d Hz",
|
ESP_LOGI(TAG, "I2C: SDA=GPIO%d, SCL=GPIO%d, clock=%d Hz",
|
||||||
|
|||||||
@@ -54,5 +54,51 @@ int main(void)
|
|||||||
if (size == 0 || fwrite(packet, 1, size, stdout) != size) {
|
if (size == 0 || fwrite(packet, 1, size, stdout) != size) {
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
trikke_wire_sample_t full_packet[TRIKKE_WIRE_MAX_RECORDS] = {0};
|
||||||
|
for (size_t i = 0; i < TRIKKE_WIRE_MAX_RECORDS; ++i) {
|
||||||
|
full_packet[i].sequence = 2000 + (uint32_t)i;
|
||||||
|
full_packet[i].timestamp_us = 3000000 + (int64_t)i * 10000;
|
||||||
|
full_packet[i].accel_x = (int16_t)i;
|
||||||
|
full_packet[i].gyro_z = -(int16_t)i;
|
||||||
|
full_packet[i].accel_status = 0x80;
|
||||||
|
full_packet[i].gyro_status = 0x08;
|
||||||
|
}
|
||||||
|
size = trikke_encode_sample_packet(
|
||||||
|
packet, sizeof(packet), 43, 2, 3, full_packet,
|
||||||
|
TRIKKE_WIRE_MAX_RECORDS);
|
||||||
|
if (size != TRIKKE_WIRE_MAX_PACKET_SIZE ||
|
||||||
|
fwrite(packet, 1, size, stdout) != size) {
|
||||||
|
return 2;
|
||||||
|
}
|
||||||
|
|
||||||
|
const trikke_wire_sample_t saturated[] = {
|
||||||
|
{.sequence = 3000, .timestamp_us = 4000000},
|
||||||
|
{.sequence = 3001, .timestamp_us = 4700000},
|
||||||
|
};
|
||||||
|
size = trikke_encode_sample_packet(packet, sizeof(packet), 44, 2, 3,
|
||||||
|
saturated, 2);
|
||||||
|
if (size == 0 || fwrite(packet, 1, size, stdout) != size) {
|
||||||
|
return 3;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!trikke_wire_timestamp_delta_fits(0, 655350) ||
|
||||||
|
trikke_wire_timestamp_delta_fits(0, 655351) ||
|
||||||
|
trikke_wire_timestamp_delta_fits(1, 0)) {
|
||||||
|
return 4;
|
||||||
|
}
|
||||||
|
if (trikke_encode_metadata_packet(
|
||||||
|
packet, TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_METADATA_SIZE - 1,
|
||||||
|
0, 0, 0, 0, &metadata) != 0 ||
|
||||||
|
trikke_encode_sample_packet(packet, sizeof(packet), 0, 0, 0,
|
||||||
|
samples, 0) != 0 ||
|
||||||
|
trikke_encode_sample_packet(packet, sizeof(packet), 0, 0, 0,
|
||||||
|
full_packet,
|
||||||
|
TRIKKE_WIRE_MAX_RECORDS + 1) != 0 ||
|
||||||
|
trikke_encode_sample_packet(packet, TRIKKE_WIRE_MAX_PACKET_SIZE - 1,
|
||||||
|
0, 0, 0, full_packet,
|
||||||
|
TRIKKE_WIRE_MAX_RECORDS) != 0) {
|
||||||
|
return 5;
|
||||||
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -9,6 +9,7 @@ ROOT = Path(__file__).resolve().parents[1]
|
|||||||
sys.path.insert(0, str(ROOT / "tools"))
|
sys.path.insert(0, str(ROOT / "tools"))
|
||||||
|
|
||||||
from trikke_protocol import ( # noqa: E402
|
from trikke_protocol import ( # noqa: E402
|
||||||
|
PACKET_FLAG_TIMESTAMP_DELTA_SATURATED,
|
||||||
PACKET_TYPE_METADATA,
|
PACKET_TYPE_METADATA,
|
||||||
PACKET_TYPE_SAMPLES,
|
PACKET_TYPE_SAMPLES,
|
||||||
StreamParser,
|
StreamParser,
|
||||||
@@ -55,8 +56,8 @@ class ProtocolContractTest(unittest.TestCase):
|
|||||||
for offset in range(0, len(stream), 7):
|
for offset in range(0, len(stream), 7):
|
||||||
frames.extend(parser.feed(stream[offset : offset + 7]))
|
frames.extend(parser.feed(stream[offset : offset + 7]))
|
||||||
|
|
||||||
self.assertEqual(2, len(frames))
|
self.assertEqual(4, len(frames))
|
||||||
metadata_frame, sample_frame = frames
|
metadata_frame, sample_frame, full_frame, saturated_frame = frames
|
||||||
self.assertEqual(PACKET_TYPE_METADATA, metadata_frame.packet_type)
|
self.assertEqual(PACKET_TYPE_METADATA, metadata_frame.packet_type)
|
||||||
self.assertEqual(41, metadata_frame.packet_sequence)
|
self.assertEqual(41, metadata_frame.packet_sequence)
|
||||||
self.assertEqual(2, metadata_frame.dropped_sample_count)
|
self.assertEqual(2, metadata_frame.dropped_sample_count)
|
||||||
@@ -75,6 +76,20 @@ class ProtocolContractTest(unittest.TestCase):
|
|||||||
self.assertEqual(0xFF, sample_frame.samples[1].gyro_status)
|
self.assertEqual(0xFF, sample_frame.samples[1].gyro_status)
|
||||||
self.assertEqual(0, parser.crc_errors)
|
self.assertEqual(0, parser.crc_errors)
|
||||||
self.assertEqual(len(b"startup text\r\n"), parser.skipped_bytes)
|
self.assertEqual(len(b"startup text\r\n"), parser.skipped_bytes)
|
||||||
|
self.assertEqual(0, parser.buffered_bytes)
|
||||||
|
|
||||||
|
self.assertEqual(8, len(full_frame.samples))
|
||||||
|
self.assertEqual(43, full_frame.packet_sequence)
|
||||||
|
self.assertEqual(2_007, full_frame.samples[-1].sequence)
|
||||||
|
self.assertEqual(3_070_000, full_frame.samples[-1].timestamp_us)
|
||||||
|
self.assertEqual(0, full_frame.flags)
|
||||||
|
|
||||||
|
self.assertEqual(44, saturated_frame.packet_sequence)
|
||||||
|
self.assertEqual(
|
||||||
|
PACKET_FLAG_TIMESTAMP_DELTA_SATURATED,
|
||||||
|
saturated_frame.flags,
|
||||||
|
)
|
||||||
|
self.assertEqual(4_655_350, saturated_frame.samples[-1].timestamp_us)
|
||||||
|
|
||||||
row = sample_to_csv_row(
|
row = sample_to_csv_row(
|
||||||
sample_frame.samples[0], metadata_frame.metadata, 3
|
sample_frame.samples[0], metadata_frame.metadata, 3
|
||||||
@@ -91,20 +106,31 @@ class ProtocolContractTest(unittest.TestCase):
|
|||||||
frames = parser.feed(bytes(damaged) + self.encoded[first_size:])
|
frames = parser.feed(bytes(damaged) + self.encoded[first_size:])
|
||||||
self.assertEqual(1, parser.startup_crc_errors)
|
self.assertEqual(1, parser.startup_crc_errors)
|
||||||
self.assertEqual(0, parser.crc_errors)
|
self.assertEqual(0, parser.crc_errors)
|
||||||
self.assertEqual(1, len(frames))
|
self.assertEqual(3, len(frames))
|
||||||
self.assertEqual(PACKET_TYPE_SAMPLES, frames[0].packet_type)
|
self.assertEqual(PACKET_TYPE_SAMPLES, frames[0].packet_type)
|
||||||
|
|
||||||
def test_crc_failure_after_sync_is_stream_error(self) -> None:
|
def test_crc_failure_after_sync_is_stream_error(self) -> None:
|
||||||
first_size = 36 + 48
|
first_size = 36 + 48
|
||||||
damaged = bytearray(self.encoded[first_size:])
|
second_size = 36 + 2 * 20
|
||||||
|
damaged = bytearray(self.encoded[first_size : first_size + second_size])
|
||||||
damaged[-1] ^= 0x80
|
damaged[-1] ^= 0x80
|
||||||
parser = StreamParser()
|
parser = StreamParser()
|
||||||
frames = parser.feed(self.encoded[:first_size] + bytes(damaged))
|
frames = parser.feed(
|
||||||
|
self.encoded[:first_size]
|
||||||
|
+ bytes(damaged)
|
||||||
|
+ self.encoded[first_size + second_size :]
|
||||||
|
)
|
||||||
self.assertEqual(0, parser.startup_crc_errors)
|
self.assertEqual(0, parser.startup_crc_errors)
|
||||||
self.assertEqual(1, parser.crc_errors)
|
self.assertEqual(1, parser.crc_errors)
|
||||||
self.assertEqual(1, len(frames))
|
self.assertEqual(3, len(frames))
|
||||||
self.assertEqual(PACKET_TYPE_METADATA, frames[0].packet_type)
|
self.assertEqual(PACKET_TYPE_METADATA, frames[0].packet_type)
|
||||||
|
|
||||||
|
def test_trailing_partial_frame_is_observable(self) -> None:
|
||||||
|
parser = StreamParser()
|
||||||
|
frames = parser.feed(self.encoded[:-5])
|
||||||
|
self.assertEqual(3, len(frames))
|
||||||
|
self.assertEqual(36 + 2 * 20 - 5, parser.buffered_bytes)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
unittest.main()
|
unittest.main()
|
||||||
|
|||||||
+12
-2
@@ -13,6 +13,7 @@ import serial
|
|||||||
|
|
||||||
from trikke_protocol import (
|
from trikke_protocol import (
|
||||||
CSV_COLUMNS,
|
CSV_COLUMNS,
|
||||||
|
PACKET_FLAG_TIMESTAMP_DELTA_SATURATED,
|
||||||
PACKET_TYPE_METADATA,
|
PACKET_TYPE_METADATA,
|
||||||
Frame,
|
Frame,
|
||||||
Metadata,
|
Metadata,
|
||||||
@@ -76,6 +77,7 @@ def main() -> int:
|
|||||||
sample_gap_count = 0
|
sample_gap_count = 0
|
||||||
sample_reset_count = 0
|
sample_reset_count = 0
|
||||||
timing_anomaly_count = 0
|
timing_anomaly_count = 0
|
||||||
|
timestamp_saturation_frame_count = 0
|
||||||
accel_stale_count = 0
|
accel_stale_count = 0
|
||||||
accel_overrun_count = 0
|
accel_overrun_count = 0
|
||||||
gyro_stale_count = 0
|
gyro_stale_count = 0
|
||||||
@@ -85,6 +87,7 @@ def main() -> int:
|
|||||||
previous_timestamp_us = None
|
previous_timestamp_us = None
|
||||||
final_dropped_count = 0
|
final_dropped_count = 0
|
||||||
final_loop_overrun_count = 0
|
final_loop_overrun_count = 0
|
||||||
|
serial_error: serial.SerialException | None = None
|
||||||
|
|
||||||
def render_frame(frame: Frame, writer: csv.writer) -> None:
|
def render_frame(frame: Frame, writer: csv.writer) -> None:
|
||||||
nonlocal sample_count, sample_gap_count, sample_reset_count
|
nonlocal sample_count, sample_gap_count, sample_reset_count
|
||||||
@@ -144,6 +147,8 @@ def main() -> int:
|
|||||||
else:
|
else:
|
||||||
packet_reset_count += 1
|
packet_reset_count += 1
|
||||||
previous_packet_sequence = frame.packet_sequence
|
previous_packet_sequence = frame.packet_sequence
|
||||||
|
if frame.flags & PACKET_FLAG_TIMESTAMP_DELTA_SATURATED:
|
||||||
|
timestamp_saturation_frame_count += 1
|
||||||
if frame.packet_type == PACKET_TYPE_METADATA:
|
if frame.packet_type == PACKET_TYPE_METADATA:
|
||||||
metadata = frame.metadata
|
metadata = frame.metadata
|
||||||
metadata_count += 1
|
metadata_count += 1
|
||||||
@@ -156,16 +161,19 @@ def main() -> int:
|
|||||||
decoded.flush()
|
decoded.flush()
|
||||||
except serial.SerialException as exc:
|
except serial.SerialException as exc:
|
||||||
print(f"Serial error: {exc}", file=sys.stderr)
|
print(f"Serial error: {exc}", file=sys.stderr)
|
||||||
return 1
|
serial_error = exc
|
||||||
|
|
||||||
print(
|
print(
|
||||||
f"Stopped after {sample_count} samples and {metadata_count} metadata frames; "
|
f"Stopped after {sample_count} samples and {metadata_count} metadata frames; "
|
||||||
f"packet_gaps={packet_gap_count}, packet_resets={packet_reset_count}, "
|
f"packet_gaps={packet_gap_count}, packet_resets={packet_reset_count}, "
|
||||||
f"sample_gaps={sample_gap_count}, sample_resets={sample_reset_count}, "
|
f"sample_gaps={sample_gap_count}, sample_resets={sample_reset_count}, "
|
||||||
f"timing_anomalies={timing_anomaly_count}, "
|
f"timing_anomalies={timing_anomaly_count}, "
|
||||||
|
f"timestamp_saturation_frames={timestamp_saturation_frame_count}, "
|
||||||
f"startup_crc_rejects={parser.startup_crc_errors}, "
|
f"startup_crc_rejects={parser.startup_crc_errors}, "
|
||||||
f"stream_crc_errors={parser.crc_errors}, "
|
f"stream_crc_errors={parser.crc_errors}, "
|
||||||
f"header_errors={parser.header_errors}, skipped_nonframe_bytes={parser.skipped_bytes}"
|
f"header_errors={parser.header_errors}, "
|
||||||
|
f"skipped_nonframe_bytes={parser.skipped_bytes}, "
|
||||||
|
f"trailing_partial_bytes={parser.buffered_bytes}"
|
||||||
)
|
)
|
||||||
print(
|
print(
|
||||||
f"Status totals: accel_stale={accel_stale_count}, "
|
f"Status totals: accel_stale={accel_stale_count}, "
|
||||||
@@ -174,6 +182,8 @@ def main() -> int:
|
|||||||
f"acquisition_loop_overruns={final_loop_overrun_count}"
|
f"acquisition_loop_overruns={final_loop_overrun_count}"
|
||||||
)
|
)
|
||||||
print(f"Saved {output} and {csv_output}")
|
print(f"Saved {output} and {csv_output}")
|
||||||
|
if serial_error is not None:
|
||||||
|
return 1
|
||||||
return 0 if metadata is not None else 4
|
return 0 if metadata is not None else 4
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
+11
-2
@@ -7,6 +7,7 @@ from pathlib import Path
|
|||||||
|
|
||||||
from trikke_protocol import (
|
from trikke_protocol import (
|
||||||
CSV_COLUMNS,
|
CSV_COLUMNS,
|
||||||
|
PACKET_FLAG_TIMESTAMP_DELTA_SATURATED,
|
||||||
PACKET_TYPE_METADATA,
|
PACKET_TYPE_METADATA,
|
||||||
Frame,
|
Frame,
|
||||||
StreamParser,
|
StreamParser,
|
||||||
@@ -31,15 +32,21 @@ def main() -> int:
|
|||||||
None,
|
None,
|
||||||
)
|
)
|
||||||
if metadata is None:
|
if metadata is None:
|
||||||
print("No valid metadata frame found")
|
print(
|
||||||
|
"No valid metadata frame found; "
|
||||||
|
f"trailing_partial_bytes={stream.buffered_bytes}"
|
||||||
|
)
|
||||||
return 2
|
return 2
|
||||||
|
|
||||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||||
sample_count = 0
|
sample_count = 0
|
||||||
|
timestamp_saturation_frame_count = 0
|
||||||
with args.output.open("w", encoding="utf-8", newline="") as target:
|
with args.output.open("w", encoding="utf-8", newline="") as target:
|
||||||
writer = csv.writer(target)
|
writer = csv.writer(target)
|
||||||
writer.writerow(CSV_COLUMNS)
|
writer.writerow(CSV_COLUMNS)
|
||||||
for frame in frames:
|
for frame in frames:
|
||||||
|
if frame.flags & PACKET_FLAG_TIMESTAMP_DELTA_SATURATED:
|
||||||
|
timestamp_saturation_frame_count += 1
|
||||||
if frame.packet_type == PACKET_TYPE_METADATA:
|
if frame.packet_type == PACKET_TYPE_METADATA:
|
||||||
if frame.metadata is not None:
|
if frame.metadata is not None:
|
||||||
metadata = frame.metadata
|
metadata = frame.metadata
|
||||||
@@ -54,7 +61,9 @@ def main() -> int:
|
|||||||
f"Decoded {sample_count} samples from {len(frames)} frames; "
|
f"Decoded {sample_count} samples from {len(frames)} frames; "
|
||||||
f"startup_crc_rejects={stream.startup_crc_errors}, "
|
f"startup_crc_rejects={stream.startup_crc_errors}, "
|
||||||
f"stream_crc_errors={stream.crc_errors}, header_errors={stream.header_errors}, "
|
f"stream_crc_errors={stream.crc_errors}, header_errors={stream.header_errors}, "
|
||||||
f"skipped_nonframe_bytes={stream.skipped_bytes}; saved {args.output}"
|
f"skipped_nonframe_bytes={stream.skipped_bytes}, "
|
||||||
|
f"timestamp_saturation_frames={timestamp_saturation_frame_count}, "
|
||||||
|
f"trailing_partial_bytes={stream.buffered_bytes}; saved {args.output}"
|
||||||
)
|
)
|
||||||
return 0
|
return 0
|
||||||
|
|
||||||
|
|||||||
@@ -16,6 +16,7 @@ MAX_RECORDS = 8
|
|||||||
|
|
||||||
PACKET_TYPE_METADATA = 1
|
PACKET_TYPE_METADATA = 1
|
||||||
PACKET_TYPE_SAMPLES = 2
|
PACKET_TYPE_SAMPLES = 2
|
||||||
|
PACKET_FLAG_TIMESTAMP_DELTA_SATURATED = 0x01
|
||||||
|
|
||||||
HEADER = struct.Struct("<4sBBBBBBHIQIII")
|
HEADER = struct.Struct("<4sBBBBBBHIQIII")
|
||||||
METADATA = struct.Struct("<HHHH10f")
|
METADATA = struct.Struct("<HHHH10f")
|
||||||
@@ -127,6 +128,11 @@ class StreamParser:
|
|||||||
self.startup_crc_errors = 0
|
self.startup_crc_errors = 0
|
||||||
self.crc_errors = 0
|
self.crc_errors = 0
|
||||||
|
|
||||||
|
@property
|
||||||
|
def buffered_bytes(self) -> int:
|
||||||
|
"""Bytes retained because they do not yet form a complete frame."""
|
||||||
|
return len(self._buffer)
|
||||||
|
|
||||||
def feed(self, data: bytes) -> list[Frame]:
|
def feed(self, data: bytes) -> list[Frame]:
|
||||||
self._buffer.extend(data)
|
self._buffer.extend(data)
|
||||||
frames: list[Frame] = []
|
frames: list[Frame] = []
|
||||||
|
|||||||
Reference in New Issue
Block a user