diff --git a/README.md b/README.md index ea3bca7..6a81207 100644 --- a/README.md +++ b/README.md @@ -7,9 +7,9 @@ This milestone does four things: 1. Detects and verifies both sensors by their identification registers. 2. Configures each sensor for a nominal 100 Hz raw output rate. -3. Emits timestamped, sensor-native raw readings over the XIAO USB connection. -4. Maps both sensors into a shared enclosure coordinate frame and emits both raw - and calibrated readings. +3. Emits framed, timestamped binary readings over the XIAO USB connection. +4. Maps both sensors into a shared enclosure frame and carries the metadata + needed to derive calibrated readings without replacing raw data. BLE transport and phone-side storage come after the wired sensor path is proven. @@ -49,17 +49,27 @@ enclosure Y = -native X enclosure Z = native Z ``` -Software calibration is applied after enclosure-axis mapping. Accelerometer -offset and per-axis scale were measured with a six-face enclosure test. Gyroscope -zero-rate bias and polarity were measured; its 17.5 mdps/LSB scale remains the -nominal datasheet value. Sensor-native and mapped raw counts remain in every -record for diagnostics. No software filtering or sensor fusion is performed yet. +Host-side software calibration is applied after enclosure-axis mapping. +Accelerometer offset and per-axis scale were measured with a six-face enclosure +test. Gyroscope zero-rate bias and polarity were measured; its 17.5 mdps/LSB scale +remains the nominal datasheet value. Mapped raw counts are stored directly, and +sensor-native counts are reconstructed losslessly from the documented mapping. +No software filtering or sensor fusion is performed yet. -The ESP32-C3 polls at exactly 100 Hz, but each sensor has an independent internal +The ESP32-C3 polls at exactly 100 Hz in a dedicated acquisition task, but each +sensor has an independent internal sample clock. The status registers are read immediately before each XYZ read so a consumer can distinguish a fresh sample from a repeated poll and identify gyro overruns. Hardware data-ready interrupts and FIFO acquisition are deferred to the -buffering milestone. +later sensor-side acquisition refinement. + +Completed samples enter a 128-record RAM queue. A lower-priority output task +batches up to eight records into versioned `TRK1` frames, isolating acquisition +from brief USB or future BLE stalls. CRC, packet and sample sequences, timestamps, +and cumulative loss/overrun counters make loss detectable. + +Measured end-to-end framing overhead is about 2.47 kB/s at 100 Hz, or 8.47 +MiB/hour before BLE link overhead. ## Build and flash @@ -74,20 +84,24 @@ For each new terminal: ```sh source /Users/jay/.espressif/v6.0.2/esp-idf/export.sh idf.py build -idf.py -p /dev/cu.usbmodem1134101 flash monitor +idf.py -p /dev/cu.usbmodem1134101 flash ``` -Exit the serial monitor with `Ctrl-]`. - ## USB output -After startup metadata, records use CSV: +After readable startup metadata, the device emits framed binary. Each sample is a +20-byte record containing mapped raw sensor counts, timing, sequence, and the two +raw status bytes. See [the complete wire-format specification](docs/binary-record-v1.md). + +Binary is the authoritative capture format. The host tools render it back to the +same diagnostic CSV schema used during calibration: ```text sequence,poll_timestamp_us,accel_x_raw,accel_y_raw,accel_z_raw,gyro_x_raw,gyro_y_raw,gyro_z_raw,accel_x_mg,accel_y_mg,accel_z_mg,gyro_x_mdps,gyro_y_mdps,gyro_z_mdps,accel_native_x_raw,accel_native_y_raw,accel_native_z_raw,gyro_native_x_raw,gyro_native_y_raw,gyro_native_z_raw,accel_int_source,gyro_status,loop_overrun_count ``` -`poll_timestamp_us` is the ESP32-C3 monotonic time immediately before the status +`poll_timestamp_us` is reconstructed from each frame's base timestamp and 10 us +record deltas. It represents the ESP32-C3 monotonic time immediately before status and data reads. It is not the sensors' physical sample time. The axes in the first six sample columns use the enclosure frame above. Calibrated acceleration is in integer milligravity (`mg`), and bias-corrected angular rate is in integer @@ -104,9 +118,19 @@ Status bits: - `loop_overrun_count`: cumulative acquisition deadlines missed; the loop resynchronizes after a miss instead of issuing catch-up bursts. -The capture tool auto-detects a single `/dev/cu.usbmodem*` device, writes only -validated numeric records to a real CSV, and reports sequence or timing problems: +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, +drop, and overrun totals: ```sh -python tools/capture_serial.py +python tools/capture_binary.py ``` + +An existing `.trk` stream can be decoded again without hardware: + +```sh +python tools/decode_binary.py captures/session.trk captures/session.csv +``` + +`tools/capture_serial.py` remains available only for decoding captures from the +older CSV-v3 firmware snapshots. diff --git a/docs/binary-record-v1.md b/docs/binary-record-v1.md new file mode 100644 index 0000000..8dd09b3 --- /dev/null +++ b/docs/binary-record-v1.md @@ -0,0 +1,81 @@ +# TRK1 Binary Telemetry — Version 1 + +The binary stream is the shared transport and storage representation for USB, +BLE, and any later nonvolatile buffer. All multibyte integers and IEEE-754 +float32 values are little-endian. Frames are self-identifying and may be split or +combined by an underlying byte transport. + +## Frame header (36 bytes) + +| Offset | Size | Field | +| ---: | ---: | --- | +| 0 | 4 | ASCII magic `TRK1` | +| 4 | 1 | Wire version (`1`) | +| 5 | 1 | Packet type: metadata `1`, samples `2` | +| 6 | 1 | Header size (`36`) | +| 7 | 1 | Record size (`0` or `20`) | +| 8 | 1 | Record count (`0` or 1–8) | +| 9 | 1 | Packet flags | +| 10 | 2 | Payload size | +| 12 | 4 | Monotonic packet sequence | +| 16 | 8 | Base ESP timer timestamp in microseconds | +| 24 | 4 | Cumulative samples lost to read failure, queue overflow, or output failure | +| 28 | 4 | Cumulative acquisition-loop overruns | +| 32 | 4 | IEEE CRC-32 | + +CRC uses polynomial `0xEDB88320`, initial value `0xFFFFFFFF`, and final XOR +`0xFFFFFFFF`. It covers header bytes 4–31 followed by the complete payload. The +magic and stored CRC field are excluded. + +Packet flag bit 0 means at least one sample timestamp delta saturated. + +## Sample record (20 bytes) + +| Offset | Size | Field | +| ---: | ---: | --- | +| 0 | 4 | Sample sequence | +| 4 | 2 | Timestamp delta from the preceding record, in 10 us units | +| 6 | 2 | Enclosure accel X raw (`int16`) | +| 8 | 2 | Enclosure accel Y raw (`int16`) | +| 10 | 2 | Enclosure accel Z raw (`int16`) | +| 12 | 2 | Enclosure gyro X raw (`int16`) | +| 14 | 2 | Enclosure gyro Y raw (`int16`) | +| 16 | 2 | Enclosure gyro Z raw (`int16`) | +| 18 | 1 | Raw ADXL345 `INT_SOURCE` | +| 19 | 1 | Raw L3G4200D `STATUS_REG` | + +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 +fit 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 +reconstructed because the mappings are lossless: + +```text +accel native = (-enclosure_y, enclosure_x, enclosure_z) +gyro native = ( enclosure_x, enclosure_y, enclosure_z) +``` + +## Metadata payload (48 bytes) + +Metadata frames repeat approximately every five seconds so a receiver may attach +midstream. The payload contains: + +- Sample rate, accelerometer range, gyroscope range, and mapping/calibration flags +- Three accel offset float32 values +- Three accel counts/g float32 values +- Three gyro bias float32 values +- Nominal gyro mdps/LSB float32 value + +A byte-stream receiver may begin inside an incomplete frame. It discards bytes +until a magic/header/CRC combination validates. Host tools report any rejection +before that first valid frame separately from CRC failures after synchronization. + +## Buffering + +Acquisition runs in a dedicated higher-priority task and writes complete samples +to a 128-entry RAM queue. The lower-priority output task batches up to eight +records per frame. At 100 Hz this queue represents about 1.28 seconds of +decoupling from a blocked transport. Queue overflow never overwrites an older +sample silently: sequence gaps and the cumulative lost-sample counter expose it. diff --git a/docs/binary-transport-validation-2026-08-17.md b/docs/binary-transport-validation-2026-08-17.md new file mode 100644 index 0000000..f3ccd4e --- /dev/null +++ b/docs/binary-transport-validation-2026-08-17.md @@ -0,0 +1,57 @@ +# Binary Transport Validation — 2026-08-17 + +This milestone replaced high-volume device-side CSV with the versioned `TRK1` +binary stream shared by future USB, BLE, and storage paths. Mapped raw sensor +counts remain authoritative. Calibration metadata travels as float32 values, and +the host reconstructs the prior 23-column diagnostic CSV without discarding raw +data. + +## Verification layers + +- Host compilation of the production C encoder with `-Wall -Wextra -Werror` +- Fragmented C-encoder-to-Python-parser contract test +- Deliberately corrupted CRC test with stream resynchronization +- ESP-IDF firmware build and flash on the assembled ESP32-C3 prototype +- Live USB capture followed by independent offline re-decoding + +## USB text-conversion finding + +The first live capture exposed that the USB console's default CRLF mode inserted +a carriage return whenever a binary byte equaled LF (`0x0A`). CRC rejected every +affected frame and the parser resynchronized at the next `TRK1` magic. No invalid +sample entered decoded CSV. + +Before binary output begins, firmware now changes the USB Serial/JTAG VFS transmit +mode to `ESP_LINE_ENDINGS_LF`, which means no byte modification. Startup logs and +readable metadata are flushed first. + +## Final hardware capture + +`captures/binary_v1_smoke2.trk` and its decoded CSV contain: + +- 7,184 samples over 71.830 seconds +- 912 total frames, including 14 repeated metadata frames +- Packet gaps and resets: 0 +- Sample gaps and resets: 0 +- Timestamp anomalies: 0 +- CRC and header failures: 0 +- Queue/read/output drops: 0 +- Acquisition-loop overruns: 0 +- ADXL345 overruns: 0 +- L3G4200D data-ready clear: 0 +- L3G4200D overruns: 192 + +Offline decoding of the saved `.trk` file produced CSV byte-for-byte identical to +the CSV rendered during live capture. + +The validated stream occupied 177,184 bytes, or 2,466.7 bytes/s including frame +headers and repeated metadata. That is 8.47 MiB/hour and about 19.7 kbit/s before +BLE link overhead, far below the previous CSV stream. + +## Task separation and buffer + +The 100 Hz I2C acquisition runs at FreeRTOS priority 10. USB encoding/output runs +at priority 5 and receives samples through a 128-entry queue (about 1.28 seconds +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 +the acquisition cadence. diff --git a/docs/calibration-2026-08-17.md b/docs/calibration-2026-08-17.md index 8557f75..ea53078 100644 --- a/docs/calibration-2026-08-17.md +++ b/docs/calibration-2026-08-17.md @@ -31,6 +31,11 @@ coefficients: | Y | -4.400892 | 259.825135 | 3.848742 | | Z | +11.776132 | 245.755573 | 4.069084 | +The retained digits make the firmware calculation reproducible; they are not a +claim of sub-count measurement accuracy. Independent face selection changed the +derived values by up to about 0.03 counts, and repositioning changed a face mean +by about one count. + Firmware converts a mapped raw value to integer milligravity with: ```text @@ -52,14 +57,25 @@ The central five thousand samples of the flat stationary interval produced: | Z | -7.0238 | 10.4259 | The instructed positive motion was top/USB-edge lift for +X, left-edge lift for -+Y, and counterclockwise rotation viewed from the cover for +Z. Every outward -stroke was positive on its intended gyro channel, and every return stroke was -negative. Axis assignment and polarity are therefore confirmed. ++Y, and counterclockwise rotation viewed from the cover for +Z. Accelerometer +tilt kinematics independently confirmed X and Y polarity. Rotation around Z while +flat is rotation around gravity, so the accelerometer cannot independently check +that sign. Z polarity follows from the right-handed enclosure frame, the +right-handed L3G4200D frame, the identity gyro mapping, and the verified X/Y +polarities. Axis assignment and polarity are therefore confirmed. Firmware subtracts the measured zero-rate bias and converts with the nominal L3G4200D +/-500 dps scale of 17.5 mdps/LSB. Gyro scale itself was not measured because no controlled angular-rate reference was available. +The compile-time bias is only an initial correction. Twelve quiet stretches in +this session showed real zero-rate wander of roughly 50 mdps on X/Z and 72 mdps +on Y; the separate smoke capture differed by up to 63 mdps. That is enough to +accumulate several degrees per minute if integrated. Runtime re-zeroing during a +verified stationary interval is required before fusion, heading integration, or +turn counting. Raw gyro counts therefore remain authoritative in the binary +record. + ## Freshness under motion Using gyro magnitude greater than 500 counts from stationary bias as a @@ -86,4 +102,10 @@ and checked with a 3,814-record flat smoke capture: - Mean bias-corrected gyro: (+25.4, -62.9, -54.8) mdps The small residual horizontal acceleration is consistent with the enclosure not -being perfectly level. The largest residual gyro mean is 0.063 dps. +being perfectly level. The largest residual gyro mean is 0.063 dps, which is +evidence of the fixed-bias limitation described above rather than a long-term +heading guarantee. + +The two floating-point startup metadata lines were subsequently observed after a +live device reset. They rendered the intended float32 calibration values before +the binary stream began. diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index a969aab..5b72e01 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,5 +1,6 @@ idf_component_register( - SRCS "trikke_sensor_main.c" + SRCS "trikke_sensor_main.c" "trikke_protocol.c" INCLUDE_DIRS "." REQUIRES adxl345 l3g4200d esp_timer esp_driver_gpio esp_driver_i2c + esp_driver_usb_serial_jtag vfs ) diff --git a/main/trikke_protocol.c b/main/trikke_protocol.c new file mode 100644 index 0000000..4eca27d --- /dev/null +++ b/main/trikke_protocol.c @@ -0,0 +1,187 @@ +#include "trikke_protocol.h" + +#include +#include + +_Static_assert(sizeof(float) == 4, "TRK1 metadata requires 32-bit float"); + +static const uint8_t TRIKKE_MAGIC[4] = {'T', 'R', 'K', '1'}; + +static void put_u16_le(uint8_t *output, uint16_t value) +{ + output[0] = (uint8_t)value; + output[1] = (uint8_t)(value >> 8); +} + +static void put_u32_le(uint8_t *output, uint32_t value) +{ + output[0] = (uint8_t)value; + output[1] = (uint8_t)(value >> 8); + output[2] = (uint8_t)(value >> 16); + output[3] = (uint8_t)(value >> 24); +} + +static void put_u64_le(uint8_t *output, uint64_t value) +{ + put_u32_le(output, (uint32_t)value); + put_u32_le(output + 4, (uint32_t)(value >> 32)); +} + +static void put_i16_le(uint8_t *output, int16_t value) +{ + put_u16_le(output, (uint16_t)value); +} + +static void put_float_le(uint8_t *output, float value) +{ + uint32_t bits = 0; + memcpy(&bits, &value, sizeof(bits)); + put_u32_le(output, bits); +} + +static uint32_t crc32_update(uint32_t crc, const uint8_t *data, size_t size) +{ + for (size_t i = 0; i < size; ++i) { + crc ^= data[i]; + for (unsigned bit = 0; bit < 8; ++bit) { + const uint32_t mask = -(crc & 1U); + crc = (crc >> 1) ^ (0xEDB88320U & mask); + } + } + return crc; +} + +static uint32_t packet_crc32(const uint8_t *packet, size_t payload_size) +{ + uint32_t crc = UINT32_MAX; + crc = crc32_update(crc, packet + 4, 28); + crc = crc32_update(crc, packet + TRIKKE_WIRE_HEADER_SIZE, payload_size); + return ~crc; +} + +static void encode_header(uint8_t *output, + uint8_t packet_type, + uint8_t record_size, + uint8_t record_count, + uint8_t flags, + uint16_t payload_size, + uint32_t packet_sequence, + int64_t base_timestamp_us, + uint32_t dropped_sample_count, + uint32_t loop_overrun_count) +{ + memcpy(output, TRIKKE_MAGIC, sizeof(TRIKKE_MAGIC)); + output[4] = TRIKKE_WIRE_VERSION; + output[5] = packet_type; + output[6] = TRIKKE_WIRE_HEADER_SIZE; + output[7] = record_size; + output[8] = record_count; + output[9] = flags; + put_u16_le(output + 10, payload_size); + put_u32_le(output + 12, packet_sequence); + put_u64_le(output + 16, (uint64_t)base_timestamp_us); + put_u32_le(output + 24, dropped_sample_count); + put_u32_le(output + 28, loop_overrun_count); + put_u32_le(output + 32, 0); +} + +size_t trikke_encode_metadata_packet( + uint8_t *output, + size_t output_size, + uint32_t packet_sequence, + int64_t timestamp_us, + uint32_t dropped_sample_count, + uint32_t loop_overrun_count, + const trikke_wire_metadata_t *metadata) +{ + const size_t packet_size = + TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_METADATA_SIZE; + if (output == NULL || metadata == NULL || output_size < packet_size) { + return 0; + } + + encode_header(output, TRIKKE_PACKET_TYPE_METADATA, 0, 0, 0, + TRIKKE_WIRE_METADATA_SIZE, packet_sequence, timestamp_us, + dropped_sample_count, loop_overrun_count); + + uint8_t *payload = output + TRIKKE_WIRE_HEADER_SIZE; + put_u16_le(payload, metadata->sample_rate_hz); + put_u16_le(payload + 2, metadata->accel_range_g); + put_u16_le(payload + 4, metadata->gyro_range_dps); + put_u16_le(payload + 6, metadata->flags); + + size_t offset = 8; + for (size_t axis = 0; axis < 3; ++axis, offset += sizeof(float)) { + put_float_le(payload + offset, metadata->accel_offset_counts[axis]); + } + for (size_t axis = 0; axis < 3; ++axis, offset += sizeof(float)) { + put_float_le(payload + offset, metadata->accel_counts_per_g[axis]); + } + for (size_t axis = 0; axis < 3; ++axis, offset += sizeof(float)) { + put_float_le(payload + offset, metadata->gyro_bias_counts[axis]); + } + put_float_le(payload + offset, metadata->gyro_mdps_per_lsb); + + put_u32_le(output + 32, packet_crc32(output, TRIKKE_WIRE_METADATA_SIZE)); + return packet_size; +} + +size_t trikke_encode_sample_packet( + uint8_t *output, + size_t output_size, + uint32_t packet_sequence, + uint32_t dropped_sample_count, + uint32_t loop_overrun_count, + const trikke_wire_sample_t *samples, + size_t sample_count) +{ + if (output == NULL || samples == NULL || sample_count == 0 || + sample_count > TRIKKE_WIRE_MAX_RECORDS) { + return 0; + } + + const size_t payload_size = sample_count * TRIKKE_WIRE_SAMPLE_RECORD_SIZE; + const size_t packet_size = TRIKKE_WIRE_HEADER_SIZE + payload_size; + if (output_size < packet_size) { + return 0; + } + + uint8_t packet_flags = 0; + encode_header(output, TRIKKE_PACKET_TYPE_SAMPLES, + TRIKKE_WIRE_SAMPLE_RECORD_SIZE, (uint8_t)sample_count, 0, + (uint16_t)payload_size, packet_sequence, + samples[0].timestamp_us, dropped_sample_count, + loop_overrun_count); + + int64_t previous_timestamp_us = samples[0].timestamp_us; + for (size_t i = 0; i < sample_count; ++i) { + uint8_t *record = output + TRIKKE_WIRE_HEADER_SIZE + + i * TRIKKE_WIRE_SAMPLE_RECORD_SIZE; + uint16_t timestamp_delta_10us = 0; + if (i > 0) { + const int64_t delta_us = samples[i].timestamp_us - previous_timestamp_us; + if (delta_us < 0 || delta_us > (int64_t)UINT16_MAX * 10) { + timestamp_delta_10us = UINT16_MAX; + packet_flags |= TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED; + } else { + timestamp_delta_10us = (uint16_t)((delta_us + 5) / 10); + } + } + + put_u32_le(record, samples[i].sequence); + put_u16_le(record + 4, timestamp_delta_10us); + put_i16_le(record + 6, samples[i].accel_x); + put_i16_le(record + 8, samples[i].accel_y); + put_i16_le(record + 10, samples[i].accel_z); + put_i16_le(record + 12, samples[i].gyro_x); + put_i16_le(record + 14, samples[i].gyro_y); + put_i16_le(record + 16, samples[i].gyro_z); + record[18] = samples[i].accel_status; + record[19] = samples[i].gyro_status; + previous_timestamp_us = samples[i].timestamp_us; + } + + output[9] = packet_flags; + put_u32_le(output + 32, packet_crc32(output, payload_size)); + return packet_size; +} diff --git a/main/trikke_protocol.h b/main/trikke_protocol.h new file mode 100644 index 0000000..3dba3e3 --- /dev/null +++ b/main/trikke_protocol.h @@ -0,0 +1,65 @@ +#pragma once + +#include +#include + +#define TRIKKE_WIRE_VERSION 1 +#define TRIKKE_WIRE_HEADER_SIZE 36 +#define TRIKKE_WIRE_SAMPLE_RECORD_SIZE 20 +#define TRIKKE_WIRE_METADATA_SIZE 48 +#define TRIKKE_WIRE_MAX_RECORDS 8 +#define TRIKKE_WIRE_MAX_PACKET_SIZE \ + (TRIKKE_WIRE_HEADER_SIZE + \ + TRIKKE_WIRE_SAMPLE_RECORD_SIZE * TRIKKE_WIRE_MAX_RECORDS) + +#define TRIKKE_PACKET_TYPE_METADATA 1 +#define TRIKKE_PACKET_TYPE_SAMPLES 2 + +#define TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED 0x01 + +#define TRIKKE_METADATA_FLAG_ACCEL_Y_NEGX_Z 0x0001 +#define TRIKKE_METADATA_FLAG_GYRO_IDENTITY 0x0002 +#define TRIKKE_METADATA_FLAG_GYRO_POLARITY 0x0004 +#define TRIKKE_METADATA_FLAG_GYRO_SCALE_NOMINAL 0x0008 + +typedef struct { + uint32_t sequence; + int64_t timestamp_us; + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + uint8_t accel_status; + uint8_t gyro_status; +} trikke_wire_sample_t; + +typedef struct { + uint16_t sample_rate_hz; + uint16_t accel_range_g; + uint16_t gyro_range_dps; + uint16_t flags; + float accel_offset_counts[3]; + float accel_counts_per_g[3]; + float gyro_bias_counts[3]; + float gyro_mdps_per_lsb; +} trikke_wire_metadata_t; + +size_t trikke_encode_metadata_packet( + uint8_t *output, + size_t output_size, + uint32_t packet_sequence, + int64_t timestamp_us, + uint32_t dropped_sample_count, + uint32_t loop_overrun_count, + const trikke_wire_metadata_t *metadata); + +size_t trikke_encode_sample_packet( + uint8_t *output, + size_t output_size, + uint32_t packet_sequence, + uint32_t dropped_sample_count, + uint32_t loop_overrun_count, + const trikke_wire_sample_t *samples, + size_t sample_count); diff --git a/main/trikke_sensor_main.c b/main/trikke_sensor_main.c index 2f87844..f7809cc 100644 --- a/main/trikke_sensor_main.c +++ b/main/trikke_sensor_main.c @@ -1,16 +1,19 @@ -#include -#include +#include #include +#include #include "adxl345.h" #include "driver/gpio.h" #include "driver/i2c_master.h" +#include "driver/usb_serial_jtag_vfs.h" #include "esp_err.h" #include "esp_log.h" #include "esp_timer.h" #include "freertos/FreeRTOS.h" +#include "freertos/queue.h" #include "freertos/task.h" #include "l3g4200d.h" +#include "trikke_protocol.h" // Seeed Studio XIAO ESP32-C3: D4/SDA = GPIO6, D5/SCL = GPIO7. #define TRIKKE_I2C_PORT I2C_NUM_0 @@ -19,6 +22,8 @@ #define TRIKKE_I2C_FREQ_HZ 400000 #define TRIKKE_SAMPLE_RATE_HZ 100 #define TRIKKE_SAMPLE_TICKS pdMS_TO_TICKS(1000 / TRIKKE_SAMPLE_RATE_HZ) +#define TRIKKE_SAMPLE_QUEUE_DEPTH 128 +#define TRIKKE_METADATA_INTERVAL_PACKETS 64 // Software calibration from the 2026-08-17 enclosure six-face capture. // Accelerometer coefficients are measured. Gyroscope scale is nominal; its @@ -42,6 +47,42 @@ typedef struct { int32_t z; } trikke_axes_sample_t; +typedef struct { + adxl345_t accelerometer; + l3g4200d_t gyroscope; + QueueHandle_t sample_queue; + atomic_uint_least32_t dropped_sample_count; + atomic_uint_least32_t loop_overrun_count; +} trikke_context_t; + +static trikke_context_t s_context; + +static const trikke_wire_metadata_t TRIKKE_METADATA = { + .sample_rate_hz = TRIKKE_SAMPLE_RATE_HZ, + .accel_range_g = 8, + .gyro_range_dps = 500, + .flags = TRIKKE_METADATA_FLAG_ACCEL_Y_NEGX_Z | + TRIKKE_METADATA_FLAG_GYRO_IDENTITY | + TRIKKE_METADATA_FLAG_GYRO_POLARITY | + TRIKKE_METADATA_FLAG_GYRO_SCALE_NOMINAL, + .accel_offset_counts = { + TRIKKE_ACCEL_X_OFFSET_COUNTS, + TRIKKE_ACCEL_Y_OFFSET_COUNTS, + TRIKKE_ACCEL_Z_OFFSET_COUNTS, + }, + .accel_counts_per_g = { + TRIKKE_ACCEL_X_COUNTS_PER_G, + TRIKKE_ACCEL_Y_COUNTS_PER_G, + TRIKKE_ACCEL_Z_COUNTS_PER_G, + }, + .gyro_bias_counts = { + TRIKKE_GYRO_X_BIAS_COUNTS, + TRIKKE_GYRO_Y_BIAS_COUNTS, + TRIKKE_GYRO_Z_BIAS_COUNTS, + }, + .gyro_mdps_per_lsb = TRIKKE_GYRO_MDPS_PER_LSB, +}; + static trikke_axes_sample_t map_accel_to_enclosure(const adxl345_sample_t *native) { // Enclosure frame: +X right, +Y toward the top, +Z toward the cover. @@ -63,28 +104,116 @@ static trikke_axes_sample_t map_gyro_to_enclosure(const l3g4200d_sample_t *nativ }; } -static trikke_axes_sample_t calibrate_accel_mg(const trikke_axes_sample_t *raw) +static void acquisition_task(void *argument) { - return (trikke_axes_sample_t) { - .x = lroundf(((float)raw->x - TRIKKE_ACCEL_X_OFFSET_COUNTS) * - 1000.0f / TRIKKE_ACCEL_X_COUNTS_PER_G), - .y = lroundf(((float)raw->y - TRIKKE_ACCEL_Y_OFFSET_COUNTS) * - 1000.0f / TRIKKE_ACCEL_Y_COUNTS_PER_G), - .z = lroundf(((float)raw->z - TRIKKE_ACCEL_Z_OFFSET_COUNTS) * - 1000.0f / TRIKKE_ACCEL_Z_COUNTS_PER_G), - }; + trikke_context_t *context = argument; + ulTaskNotifyTake(pdTRUE, portMAX_DELAY); + + uint32_t sequence = 0; + TickType_t last_wake = xTaskGetTickCount(); + while (true) { + adxl345_sample_t accel = {0}; + l3g4200d_sample_t gyro = {0}; + uint8_t accel_status = 0; + uint8_t gyro_status = 0; + const int64_t timestamp_us = esp_timer_get_time(); + + const esp_err_t accel_err = + adxl345_read_raw(&context->accelerometer, &accel, &accel_status); + const esp_err_t gyro_err = + l3g4200d_read_raw(&context->gyroscope, &gyro, &gyro_status); + + if (accel_err == ESP_OK && gyro_err == ESP_OK) { + const trikke_axes_sample_t enclosure_accel = + map_accel_to_enclosure(&accel); + const trikke_axes_sample_t enclosure_gyro = + map_gyro_to_enclosure(&gyro); + const trikke_wire_sample_t sample = { + .sequence = sequence, + .timestamp_us = timestamp_us, + .accel_x = (int16_t)enclosure_accel.x, + .accel_y = (int16_t)enclosure_accel.y, + .accel_z = (int16_t)enclosure_accel.z, + .gyro_x = (int16_t)enclosure_gyro.x, + .gyro_y = (int16_t)enclosure_gyro.y, + .gyro_z = (int16_t)enclosure_gyro.z, + .accel_status = accel_status, + .gyro_status = gyro_status, + }; + if (xQueueSend(context->sample_queue, &sample, 0) != pdPASS) { + atomic_fetch_add(&context->dropped_sample_count, 1); + } + } else { + atomic_fetch_add(&context->dropped_sample_count, 1); + } + + ++sequence; + if (xTaskDelayUntil(&last_wake, TRIKKE_SAMPLE_TICKS) == pdFALSE) { + atomic_fetch_add(&context->loop_overrun_count, 1); + last_wake = xTaskGetTickCount(); + vTaskDelay(1); + } + } } -static trikke_axes_sample_t calibrate_gyro_mdps(const trikke_axes_sample_t *raw) +static bool write_binary_packet(const uint8_t *packet, size_t packet_size) { - return (trikke_axes_sample_t) { - .x = lroundf(((float)raw->x - TRIKKE_GYRO_X_BIAS_COUNTS) * - TRIKKE_GYRO_MDPS_PER_LSB), - .y = lroundf(((float)raw->y - TRIKKE_GYRO_Y_BIAS_COUNTS) * - TRIKKE_GYRO_MDPS_PER_LSB), - .z = lroundf(((float)raw->z - TRIKKE_GYRO_Z_BIAS_COUNTS) * - TRIKKE_GYRO_MDPS_PER_LSB), - }; + const bool complete = fwrite(packet, 1, packet_size, stdout) == packet_size; + fflush(stdout); + if (!complete) { + clearerr(stdout); + } + return complete; +} + +static void output_task(void *argument) +{ + trikke_context_t *context = argument; + ulTaskNotifyTake(pdTRUE, portMAX_DELAY); + + uint8_t packet[TRIKKE_WIRE_MAX_PACKET_SIZE] = {0}; + uint32_t packet_sequence = 0; + uint32_t sample_packet_count = 0; + + size_t packet_size = trikke_encode_metadata_packet( + packet, sizeof(packet), packet_sequence++, esp_timer_get_time(), + atomic_load(&context->dropped_sample_count), + atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA); + write_binary_packet(packet, packet_size); + + while (true) { + trikke_wire_sample_t samples[TRIKKE_WIRE_MAX_RECORDS] = {0}; + size_t sample_count = 0; + if (xQueueReceive(context->sample_queue, &samples[sample_count], + portMAX_DELAY) != pdPASS) { + continue; + } + ++sample_count; + + while (sample_count < TRIKKE_WIRE_MAX_RECORDS && + xQueueReceive(context->sample_queue, &samples[sample_count], + pdMS_TO_TICKS(15)) == pdPASS) { + ++sample_count; + } + + if (sample_packet_count > 0 && + sample_packet_count % TRIKKE_METADATA_INTERVAL_PACKETS == 0) { + packet_size = trikke_encode_metadata_packet( + packet, sizeof(packet), packet_sequence++, esp_timer_get_time(), + atomic_load(&context->dropped_sample_count), + atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA); + write_binary_packet(packet, packet_size); + } + + packet_size = trikke_encode_sample_packet( + packet, sizeof(packet), packet_sequence++, + atomic_load(&context->dropped_sample_count), + atomic_load(&context->loop_overrun_count), samples, sample_count); + if (!write_binary_packet(packet, packet_size)) { + atomic_fetch_add(&context->dropped_sample_count, sample_count); + } + ++sample_packet_count; + } } static esp_err_t init_i2c(i2c_master_bus_handle_t *bus) @@ -103,7 +232,7 @@ static esp_err_t init_i2c(i2c_master_bus_handle_t *bus) void app_main(void) { - // Emit each CSV record immediately while testing over USB. + // Flush startup text by line; binary frames are flushed explicitly. setvbuf(stdout, NULL, _IOLBF, 0); ESP_LOGI(TAG, "Trikke motion telemetry prototype v0"); @@ -117,8 +246,7 @@ void app_main(void) return; } - adxl345_t accelerometer = {0}; - err = adxl345_init(&accelerometer, bus, TRIKKE_I2C_FREQ_HZ); + err = adxl345_init(&s_context.accelerometer, bus, TRIKKE_I2C_FREQ_HZ); if (err != ESP_OK) { ESP_LOGE(TAG, "ADXL345 not found at 0x53 or 0x1D (expected DEVID 0xE5): %s", @@ -127,20 +255,19 @@ void app_main(void) return; } ESP_LOGI(TAG, "ADXL345 detected at 0x%02X; 100 Hz, +/-8 g, full resolution", - adxl345_address(&accelerometer)); + adxl345_address(&s_context.accelerometer)); - l3g4200d_t gyroscope = {0}; - err = l3g4200d_init(&gyroscope, bus, TRIKKE_I2C_FREQ_HZ); + err = l3g4200d_init(&s_context.gyroscope, bus, TRIKKE_I2C_FREQ_HZ); if (err != ESP_OK) { ESP_LOGE(TAG, "L3G4200D not found at 0x69 or 0x68 (expected WHO_AM_I 0xD3): %s", esp_err_to_name(err)); - adxl345_deinit(&accelerometer); + adxl345_deinit(&s_context.accelerometer); i2c_del_master_bus(bus); return; } ESP_LOGI(TAG, "L3G4200D detected at 0x%02X; 100 Hz, 25 Hz BW, +/-500 dps", - l3g4200d_address(&gyroscope)); + l3g4200d_address(&s_context.gyroscope)); // Discard the gyroscope's visible startup transient before beginning the stream. vTaskDelay(pdMS_TO_TICKS(500)); @@ -160,66 +287,48 @@ void app_main(void) "gyro(x,y,z)=(native_x,native_y,native_z)\n"); printf("# status_masks=accel_data_ready:0x80,accel_overrun:0x01," "gyro_data_ready:0x08,gyro_overrun:0x80\n"); - printf("sequence,poll_timestamp_us,accel_x_raw,accel_y_raw,accel_z_raw," - "gyro_x_raw,gyro_y_raw,gyro_z_raw," - "accel_x_mg,accel_y_mg,accel_z_mg," - "gyro_x_mdps,gyro_y_mdps,gyro_z_mdps," - "accel_native_x_raw,accel_native_y_raw,accel_native_z_raw," - "gyro_native_x_raw,gyro_native_y_raw,gyro_native_z_raw," - "accel_int_source,gyro_status,loop_overrun_count\n"); + printf("# binary_stream=TRK1,wire_version=%d,record_size=%d," + "records_per_packet=%d\n", + TRIKKE_WIRE_VERSION, TRIKKE_WIRE_SAMPLE_RECORD_SIZE, + TRIKKE_WIRE_MAX_RECORDS); + fflush(stdout); - uint32_t sequence = 0; - uint32_t read_error_count = 0; - uint32_t loop_overrun_count = 0; - TickType_t last_wake = xTaskGetTickCount(); + // The console defaults to CRLF conversion, which would insert bytes into + // binary frames whenever a payload byte equals LF. + usb_serial_jtag_vfs_set_tx_line_endings(ESP_LINE_ENDINGS_LF); - while (true) { - adxl345_sample_t accel = {0}; - l3g4200d_sample_t gyro = {0}; - uint8_t accel_int_source = 0; - uint8_t gyro_status = 0; - const int64_t timestamp_us = esp_timer_get_time(); - - const esp_err_t accel_err = adxl345_read_raw(&accelerometer, &accel, - &accel_int_source); - const esp_err_t gyro_err = l3g4200d_read_raw(&gyroscope, &gyro, &gyro_status); - - if (accel_err == ESP_OK && gyro_err == ESP_OK) { - const trikke_axes_sample_t enclosure_accel = map_accel_to_enclosure(&accel); - const trikke_axes_sample_t enclosure_gyro = map_gyro_to_enclosure(&gyro); - const trikke_axes_sample_t calibrated_accel = - calibrate_accel_mg(&enclosure_accel); - const trikke_axes_sample_t calibrated_gyro = - calibrate_gyro_mdps(&enclosure_gyro); - - printf("%" PRIu32 ",%" PRId64 ",%" PRId32 ",%" PRId32 ",%" PRId32 - ",%" PRId32 ",%" PRId32 ",%" PRId32 - ",%" PRId32 ",%" PRId32 ",%" PRId32 - ",%" PRId32 ",%" PRId32 ",%" PRId32 - ",%" PRId16 ",%" PRId16 ",%" PRId16 - ",%" PRId16 ",%" PRId16 ",%" PRId16 - ",%" PRIu8 ",%" PRIu8 ",%" PRIu32 "\n", - sequence, timestamp_us, - enclosure_accel.x, enclosure_accel.y, enclosure_accel.z, - enclosure_gyro.x, enclosure_gyro.y, enclosure_gyro.z, - calibrated_accel.x, calibrated_accel.y, calibrated_accel.z, - calibrated_gyro.x, calibrated_gyro.y, calibrated_gyro.z, - accel.x, accel.y, accel.z, - gyro.x, gyro.y, gyro.z, - accel_int_source, gyro_status, loop_overrun_count); - } else { - ++read_error_count; - ESP_LOGE(TAG, - "sample %" PRIu32 " read failed (accel=%s, gyro=%s, total_errors=%" PRIu32 ")", - sequence, esp_err_to_name(accel_err), esp_err_to_name(gyro_err), - read_error_count); - } - - ++sequence; - if (xTaskDelayUntil(&last_wake, TRIKKE_SAMPLE_TICKS) == pdFALSE) { - // Do not issue a burst of back-to-back samples after a stalled output path. - ++loop_overrun_count; - last_wake = xTaskGetTickCount(); - } + s_context.sample_queue = + xQueueCreate(TRIKKE_SAMPLE_QUEUE_DEPTH, sizeof(trikke_wire_sample_t)); + if (s_context.sample_queue == NULL) { + ESP_LOGE(TAG, "sample queue allocation failed"); + l3g4200d_deinit(&s_context.gyroscope); + adxl345_deinit(&s_context.accelerometer); + i2c_del_master_bus(bus); + return; } + + TaskHandle_t output_task_handle = NULL; + TaskHandle_t acquisition_task_handle = NULL; + if (xTaskCreate(output_task, "trikke_output", 4096, &s_context, 5, + &output_task_handle) != pdPASS || + xTaskCreate(acquisition_task, "trikke_acquire", 4096, &s_context, 10, + &acquisition_task_handle) != pdPASS) { + if (output_task_handle != NULL) { + vTaskDelete(output_task_handle); + } + if (acquisition_task_handle != NULL) { + vTaskDelete(acquisition_task_handle); + } + vQueueDelete(s_context.sample_queue); + ESP_LOGE(TAG, "telemetry task creation failed"); + l3g4200d_deinit(&s_context.gyroscope); + adxl345_deinit(&s_context.accelerometer); + i2c_del_master_bus(bus); + return; + } + + // No text may share the byte stream once framed binary output begins. + esp_log_level_set("*", ESP_LOG_NONE); + xTaskNotifyGive(output_task_handle); + xTaskNotifyGive(acquisition_task_handle); } diff --git a/tests/protocol_fixture.c b/tests/protocol_fixture.c new file mode 100644 index 0000000..400fe17 --- /dev/null +++ b/tests/protocol_fixture.c @@ -0,0 +1,58 @@ +#include +#include + +#include "trikke_protocol.h" + +int main(void) +{ + uint8_t packet[TRIKKE_WIRE_MAX_PACKET_SIZE] = {0}; + const trikke_wire_metadata_t metadata = { + .sample_rate_hz = 100, + .accel_range_g = 8, + .gyro_range_dps = 500, + .flags = TRIKKE_METADATA_FLAG_ACCEL_Y_NEGX_Z | + TRIKKE_METADATA_FLAG_GYRO_IDENTITY, + .accel_offset_counts = {-1.5f, -4.5f, 12.0f}, + .accel_counts_per_g = {259.0f, 260.0f, 246.0f}, + .gyro_bias_counts = {9.0f, -154.0f, -7.0f}, + .gyro_mdps_per_lsb = 17.5f, + }; + size_t size = trikke_encode_metadata_packet( + packet, sizeof(packet), 41, 1234567, 2, 3, &metadata); + if (size == 0 || fwrite(packet, 1, size, stdout) != size) { + return 1; + } + + const trikke_wire_sample_t samples[] = { + { + .sequence = 1000, + .timestamp_us = 2000000, + .accel_x = 1, + .accel_y = -2, + .accel_z = 258, + .gyro_x = -10, + .gyro_y = 20, + .gyro_z = -30, + .accel_status = 0x82, + .gyro_status = 0x0F, + }, + { + .sequence = 1001, + .timestamp_us = 2010000, + .accel_x = 4, + .accel_y = -5, + .accel_z = 257, + .gyro_x = 40, + .gyro_y = -50, + .gyro_z = 60, + .accel_status = 0x02, + .gyro_status = 0xFF, + }, + }; + size = trikke_encode_sample_packet(packet, sizeof(packet), 42, 2, 3, + samples, 2); + if (size == 0 || fwrite(packet, 1, size, stdout) != size) { + return 1; + } + return 0; +} diff --git a/tests/test_trikke_protocol.py b/tests/test_trikke_protocol.py new file mode 100644 index 0000000..aee58a8 --- /dev/null +++ b/tests/test_trikke_protocol.py @@ -0,0 +1,110 @@ +import shutil +import subprocess +import sys +import tempfile +import unittest +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(ROOT / "tools")) + +from trikke_protocol import ( # noqa: E402 + PACKET_TYPE_METADATA, + PACKET_TYPE_SAMPLES, + StreamParser, + sample_to_csv_row, +) + + +class ProtocolContractTest(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + compiler = shutil.which("cc") + if compiler is None: + raise unittest.SkipTest("host C compiler is unavailable") + cls.tempdir = tempfile.TemporaryDirectory() + executable = Path(cls.tempdir.name) / "protocol_fixture" + subprocess.run( + [ + compiler, + "-std=c11", + "-Wall", + "-Wextra", + "-Werror", + "-I", + str(ROOT / "main"), + str(ROOT / "main" / "trikke_protocol.c"), + str(ROOT / "tests" / "protocol_fixture.c"), + "-o", + str(executable), + ], + check=True, + ) + cls.encoded = subprocess.run( + [str(executable)], check=True, capture_output=True + ).stdout + + @classmethod + def tearDownClass(cls) -> None: + cls.tempdir.cleanup() + + def test_c_encoder_to_python_parser_contract(self) -> None: + parser = StreamParser() + frames = [] + stream = b"startup text\r\n" + self.encoded + for offset in range(0, len(stream), 7): + frames.extend(parser.feed(stream[offset : offset + 7])) + + self.assertEqual(2, len(frames)) + metadata_frame, sample_frame = frames + self.assertEqual(PACKET_TYPE_METADATA, metadata_frame.packet_type) + self.assertEqual(41, metadata_frame.packet_sequence) + self.assertEqual(2, metadata_frame.dropped_sample_count) + self.assertEqual(3, metadata_frame.loop_overrun_count) + self.assertEqual(100, metadata_frame.metadata.sample_rate_hz) + self.assertEqual((-1.5, -4.5, 12.0), metadata_frame.metadata.accel_offset_counts) + + self.assertEqual(PACKET_TYPE_SAMPLES, sample_frame.packet_type) + self.assertEqual(42, sample_frame.packet_sequence) + self.assertEqual(2, len(sample_frame.samples)) + self.assertEqual(1000, sample_frame.samples[0].sequence) + self.assertEqual(2_000_000, sample_frame.samples[0].timestamp_us) + self.assertEqual((1, -2, 258), sample_frame.samples[0].accel) + self.assertEqual(2_010_000, sample_frame.samples[1].timestamp_us) + self.assertEqual((40, -50, 60), sample_frame.samples[1].gyro) + self.assertEqual(0xFF, sample_frame.samples[1].gyro_status) + self.assertEqual(0, parser.crc_errors) + self.assertEqual(len(b"startup text\r\n"), parser.skipped_bytes) + + row = sample_to_csv_row( + sample_frame.samples[0], metadata_frame.metadata, 3 + ) + self.assertEqual(23, len(row)) + self.assertEqual((2, 1, 258), tuple(row[14:17])) + self.assertEqual(3, row[-1]) + + def test_crc_failure_resynchronizes_to_next_frame(self) -> None: + first_size = 36 + 48 + damaged = bytearray(self.encoded[:first_size]) + damaged[-1] ^= 0x80 + parser = StreamParser() + frames = parser.feed(bytes(damaged) + self.encoded[first_size:]) + self.assertEqual(1, parser.startup_crc_errors) + self.assertEqual(0, parser.crc_errors) + self.assertEqual(1, len(frames)) + self.assertEqual(PACKET_TYPE_SAMPLES, frames[0].packet_type) + + def test_crc_failure_after_sync_is_stream_error(self) -> None: + first_size = 36 + 48 + damaged = bytearray(self.encoded[first_size:]) + damaged[-1] ^= 0x80 + parser = StreamParser() + frames = parser.feed(self.encoded[:first_size] + bytes(damaged)) + self.assertEqual(0, parser.startup_crc_errors) + self.assertEqual(1, parser.crc_errors) + self.assertEqual(1, len(frames)) + self.assertEqual(PACKET_TYPE_METADATA, frames[0].packet_type) + + +if __name__ == "__main__": + unittest.main() diff --git a/tools/capture_binary.py b/tools/capture_binary.py new file mode 100644 index 0000000..508b3da --- /dev/null +++ b/tools/capture_binary.py @@ -0,0 +1,181 @@ +#!/usr/bin/env python3 +"""Capture validated TRK1 frames and render their samples to CSV.""" + +import argparse +import csv +import glob +import signal +import sys +from datetime import datetime +from pathlib import Path + +import serial + +from trikke_protocol import ( + CSV_COLUMNS, + PACKET_TYPE_METADATA, + Frame, + Metadata, + StreamParser, + sample_to_csv_row, +) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser() + parser.add_argument("--port", help="serial port; auto-detected when omitted") + parser.add_argument("--baud", type=int, default=115200) + parser.add_argument("--output", type=Path, help="validated binary .trk output") + parser.add_argument("--csv", type=Path, help="decoded CSV output") + return parser.parse_args() + + +def resolve_port(requested_port: str | None) -> str: + if requested_port: + return requested_port + candidates = sorted(glob.glob("/dev/cu.usbmodem*")) + if not candidates: + raise RuntimeError("no /dev/cu.usbmodem* serial device found") + if len(candidates) > 1: + raise RuntimeError( + f"multiple serial devices found ({', '.join(candidates)}); specify --port" + ) + return candidates[0] + + +def main() -> int: + args = parse_args() + try: + port = resolve_port(args.port) + except RuntimeError as exc: + print(f"Port error: {exc}", file=sys.stderr) + return 2 + + stem = datetime.now().strftime("binary_%Y%m%d_%H%M%S") + output = args.output or Path("captures") / f"{stem}.trk" + csv_output = args.csv or output.with_suffix(".csv") + output.parent.mkdir(parents=True, exist_ok=True) + csv_output.parent.mkdir(parents=True, exist_ok=True) + + stop_requested = False + + def request_stop(_signum: int, _frame: object) -> None: + nonlocal stop_requested + stop_requested = True + + signal.signal(signal.SIGINT, request_stop) + signal.signal(signal.SIGTERM, request_stop) + + parser = StreamParser() + metadata: Metadata | None = None + pending_frames: list[Frame] = [] + sample_count = 0 + metadata_count = 0 + packet_gap_count = 0 + packet_reset_count = 0 + sample_gap_count = 0 + sample_reset_count = 0 + timing_anomaly_count = 0 + accel_stale_count = 0 + accel_overrun_count = 0 + gyro_stale_count = 0 + gyro_overrun_count = 0 + previous_packet_sequence = None + previous_sample_sequence = None + previous_timestamp_us = None + final_dropped_count = 0 + final_loop_overrun_count = 0 + + def render_frame(frame: Frame, writer: csv.writer) -> None: + nonlocal sample_count, sample_gap_count, sample_reset_count + nonlocal timing_anomaly_count + nonlocal accel_stale_count, accel_overrun_count + nonlocal gyro_stale_count, gyro_overrun_count + nonlocal previous_sample_sequence, previous_timestamp_us + if metadata is None: + pending_frames.append(frame) + return + for sample in frame.samples: + if previous_sample_sequence is not None: + expected = (previous_sample_sequence + 1) & 0xFFFFFFFF + if sample.sequence != expected: + if sample.sequence > expected: + sample_gap_count += sample.sequence - expected + else: + sample_reset_count += 1 + if previous_timestamp_us is not None: + if sample.timestamp_us - previous_timestamp_us != 10_000: + timing_anomaly_count += 1 + if not sample.accel_status & 0x80: + accel_stale_count += 1 + if sample.accel_status & 0x01: + accel_overrun_count += 1 + if not sample.gyro_status & 0x08: + gyro_stale_count += 1 + if sample.gyro_status & 0x80: + gyro_overrun_count += 1 + writer.writerow(sample_to_csv_row(sample, metadata, frame.loop_overrun_count)) + previous_sample_sequence = sample.sequence + previous_timestamp_us = sample.timestamp_us + sample_count += 1 + if sample_count % 500 == 0: + print(f" {sample_count} samples captured", flush=True) + + print(f"Recording {port} to {output} and {csv_output}; press Ctrl-C to stop") + try: + with serial.Serial(port, args.baud, timeout=0.25) as sensor, output.open( + "wb" + ) as raw_capture, csv_output.open("w", encoding="utf-8", newline="") as decoded: + writer = csv.writer(decoded) + writer.writerow(CSV_COLUMNS) + while not stop_requested: + chunk = sensor.read(4096) + if not chunk: + continue + for frame in parser.feed(chunk): + raw_capture.write(frame.raw) + final_dropped_count = frame.dropped_sample_count + final_loop_overrun_count = frame.loop_overrun_count + if previous_packet_sequence is not None: + expected = (previous_packet_sequence + 1) & 0xFFFFFFFF + if frame.packet_sequence != expected: + if frame.packet_sequence > expected: + packet_gap_count += frame.packet_sequence - expected + else: + packet_reset_count += 1 + previous_packet_sequence = frame.packet_sequence + if frame.packet_type == PACKET_TYPE_METADATA: + metadata = frame.metadata + metadata_count += 1 + for pending in pending_frames: + render_frame(pending, writer) + pending_frames.clear() + else: + render_frame(frame, writer) + raw_capture.flush() + decoded.flush() + except serial.SerialException as exc: + print(f"Serial error: {exc}", file=sys.stderr) + return 1 + + print( + f"Stopped after {sample_count} samples and {metadata_count} metadata frames; " + f"packet_gaps={packet_gap_count}, packet_resets={packet_reset_count}, " + f"sample_gaps={sample_gap_count}, sample_resets={sample_reset_count}, " + f"timing_anomalies={timing_anomaly_count}, " + f"startup_crc_rejects={parser.startup_crc_errors}, " + f"stream_crc_errors={parser.crc_errors}, " + f"header_errors={parser.header_errors}, skipped_nonframe_bytes={parser.skipped_bytes}" + ) + print( + f"Status totals: accel_stale={accel_stale_count}, " + f"accel_overrun={accel_overrun_count}, gyro_stale={gyro_stale_count}, " + f"gyro_overrun={gyro_overrun_count}, dropped={final_dropped_count}, " + f"acquisition_loop_overruns={final_loop_overrun_count}" + ) + print(f"Saved {output} and {csv_output}") + return 0 if metadata is not None else 4 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/decode_binary.py b/tools/decode_binary.py new file mode 100644 index 0000000..a97bfa3 --- /dev/null +++ b/tools/decode_binary.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +"""Decode a validated or raw TRK1 byte stream into CSV.""" + +import argparse +import csv +from pathlib import Path + +from trikke_protocol import ( + CSV_COLUMNS, + PACKET_TYPE_METADATA, + Frame, + StreamParser, + sample_to_csv_row, +) + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("input", type=Path) + parser.add_argument("output", type=Path) + args = parser.parse_args() + + stream = StreamParser() + frames: list[Frame] = [] + with args.input.open("rb") as source: + while chunk := source.read(64 * 1024): + frames.extend(stream.feed(chunk)) + + metadata = next( + (frame.metadata for frame in frames if frame.packet_type == PACKET_TYPE_METADATA), + None, + ) + if metadata is None: + print("No valid metadata frame found") + return 2 + + args.output.parent.mkdir(parents=True, exist_ok=True) + sample_count = 0 + with args.output.open("w", encoding="utf-8", newline="") as target: + writer = csv.writer(target) + writer.writerow(CSV_COLUMNS) + for frame in frames: + if frame.packet_type == PACKET_TYPE_METADATA: + if frame.metadata is not None: + metadata = frame.metadata + continue + for sample in frame.samples: + writer.writerow( + sample_to_csv_row(sample, metadata, frame.loop_overrun_count) + ) + sample_count += 1 + + print( + f"Decoded {sample_count} samples from {len(frames)} frames; " + f"startup_crc_rejects={stream.startup_crc_errors}, " + f"stream_crc_errors={stream.crc_errors}, header_errors={stream.header_errors}, " + f"skipped_nonframe_bytes={stream.skipped_bytes}; saved {args.output}" + ) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/trikke_protocol.py b/tools/trikke_protocol.py new file mode 100644 index 0000000..8a2b138 --- /dev/null +++ b/tools/trikke_protocol.py @@ -0,0 +1,252 @@ +"""Parser and CSV renderer for the Trikke TRK1 binary telemetry stream.""" + +from __future__ import annotations + +import math +import struct +import zlib +from dataclasses import dataclass + +MAGIC = b"TRK1" +VERSION = 1 +HEADER_SIZE = 36 +SAMPLE_RECORD_SIZE = 20 +METADATA_SIZE = 48 +MAX_RECORDS = 8 + +PACKET_TYPE_METADATA = 1 +PACKET_TYPE_SAMPLES = 2 + +HEADER = struct.Struct("<4sBBBBBBHIQIII") +METADATA = struct.Struct(" int: + """Match C lroundf: nearest integer, halfway cases away from zero.""" + return math.floor(value + 0.5) if value >= 0 else math.ceil(value - 0.5) + + +def sample_to_csv_row(sample: Sample, metadata: Metadata, loop_overruns: int) -> list[int]: + accel_mg = [ + _lround((sample.accel[i] - metadata.accel_offset_counts[i]) * 1000.0 / + metadata.accel_counts_per_g[i]) + for i in range(3) + ] + gyro_mdps = [ + _lround((sample.gyro[i] - metadata.gyro_bias_counts[i]) * + metadata.gyro_mdps_per_lsb) + for i in range(3) + ] + + # Inverse of enclosure accel(x,y,z)=(native_y,-native_x,native_z). + accel_native = (-sample.accel[1], sample.accel[0], sample.accel[2]) + gyro_native = sample.gyro + return [ + sample.sequence, + sample.timestamp_us, + *sample.accel, + *sample.gyro, + *accel_mg, + *gyro_mdps, + *accel_native, + *gyro_native, + sample.accel_status, + sample.gyro_status, + loop_overruns, + ] + + +class StreamParser: + def __init__(self) -> None: + self._buffer = bytearray() + self.synchronized = False + self.skipped_bytes = 0 + self.header_errors = 0 + self.startup_crc_errors = 0 + self.crc_errors = 0 + + def feed(self, data: bytes) -> list[Frame]: + self._buffer.extend(data) + frames: list[Frame] = [] + while True: + magic_at = self._buffer.find(MAGIC) + if magic_at < 0: + keep = min(len(self._buffer), len(MAGIC) - 1) + self.skipped_bytes += len(self._buffer) - keep + if keep: + del self._buffer[:-keep] + else: + self._buffer.clear() + break + if magic_at: + self.skipped_bytes += magic_at + del self._buffer[:magic_at] + if len(self._buffer) < HEADER_SIZE: + break + + fields = HEADER.unpack_from(self._buffer) + ( + _magic, + version, + packet_type, + header_size, + record_size, + record_count, + flags, + payload_size, + packet_sequence, + base_timestamp_us, + dropped_sample_count, + loop_overrun_count, + expected_crc, + ) = fields + + valid_shape = ( + version == VERSION + and header_size == HEADER_SIZE + and packet_type in (PACKET_TYPE_METADATA, PACKET_TYPE_SAMPLES) + and payload_size <= max(METADATA_SIZE, SAMPLE_RECORD_SIZE * MAX_RECORDS) + ) + if packet_type == PACKET_TYPE_METADATA: + valid_shape = valid_shape and ( + record_size == 0 and record_count == 0 and payload_size == METADATA_SIZE + ) + elif packet_type == PACKET_TYPE_SAMPLES: + valid_shape = valid_shape and ( + record_size == SAMPLE_RECORD_SIZE + and 1 <= record_count <= MAX_RECORDS + and payload_size == record_size * record_count + ) + if not valid_shape: + self.header_errors += 1 + self.skipped_bytes += 1 + del self._buffer[0] + continue + + frame_size = HEADER_SIZE + payload_size + if len(self._buffer) < frame_size: + break + raw = bytes(self._buffer[:frame_size]) + actual_crc = zlib.crc32(raw[4:32]) + actual_crc = zlib.crc32(raw[HEADER_SIZE:], actual_crc) + if actual_crc != expected_crc: + if self.synchronized: + self.crc_errors += 1 + else: + self.startup_crc_errors += 1 + self.skipped_bytes += 1 + del self._buffer[0] + continue + + metadata = None + samples: tuple[Sample, ...] = () + payload = raw[HEADER_SIZE:] + if packet_type == PACKET_TYPE_METADATA: + values = METADATA.unpack(payload) + metadata = Metadata( + sample_rate_hz=values[0], + accel_range_g=values[1], + gyro_range_dps=values[2], + flags=values[3], + accel_offset_counts=values[4:7], + accel_counts_per_g=values[7:10], + gyro_bias_counts=values[10:13], + gyro_mdps_per_lsb=values[13], + ) + else: + decoded: list[Sample] = [] + timestamp_us = base_timestamp_us + for index in range(record_count): + values = SAMPLE.unpack_from(payload, index * SAMPLE_RECORD_SIZE) + if index: + timestamp_us += values[1] * 10 + decoded.append( + Sample( + sequence=values[0], + timestamp_us=timestamp_us, + accel=values[2:5], + gyro=values[5:8], + accel_status=values[8], + gyro_status=values[9], + ) + ) + samples = tuple(decoded) + + frames.append( + Frame( + packet_type=packet_type, + flags=flags, + packet_sequence=packet_sequence, + base_timestamp_us=base_timestamp_us, + dropped_sample_count=dropped_sample_count, + loop_overrun_count=loop_overrun_count, + metadata=metadata, + samples=samples, + raw=raw, + ) + ) + self.synchronized = True + del self._buffer[:frame_size] + return frames