add framed binary telemetry transport
This commit is contained in:
@@ -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.
|
||||
|
||||
@@ -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.
|
||||
@@ -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.
|
||||
@@ -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.
|
||||
|
||||
+2
-1
@@ -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
|
||||
)
|
||||
|
||||
@@ -0,0 +1,187 @@
|
||||
#include "trikke_protocol.h"
|
||||
|
||||
#include <limits.h>
|
||||
#include <string.h>
|
||||
|
||||
_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;
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
#pragma once
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#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);
|
||||
+196
-87
@@ -1,16 +1,19 @@
|
||||
#include <inttypes.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <stdatomic.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,58 @@
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
@@ -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()
|
||||
@@ -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())
|
||||
@@ -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())
|
||||
@@ -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("<HHHH10f")
|
||||
SAMPLE = struct.Struct("<IHhhhhhhBB")
|
||||
|
||||
CSV_COLUMNS = [
|
||||
"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",
|
||||
]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Metadata:
|
||||
sample_rate_hz: int
|
||||
accel_range_g: int
|
||||
gyro_range_dps: int
|
||||
flags: int
|
||||
accel_offset_counts: tuple[float, float, float]
|
||||
accel_counts_per_g: tuple[float, float, float]
|
||||
gyro_bias_counts: tuple[float, float, float]
|
||||
gyro_mdps_per_lsb: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Sample:
|
||||
sequence: int
|
||||
timestamp_us: int
|
||||
accel: tuple[int, int, int]
|
||||
gyro: tuple[int, int, int]
|
||||
accel_status: int
|
||||
gyro_status: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Frame:
|
||||
packet_type: int
|
||||
flags: int
|
||||
packet_sequence: int
|
||||
base_timestamp_us: int
|
||||
dropped_sample_count: int
|
||||
loop_overrun_count: int
|
||||
metadata: Metadata | None
|
||||
samples: tuple[Sample, ...]
|
||||
raw: bytes
|
||||
|
||||
|
||||
def _lround(value: float) -> 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
|
||||
Reference in New Issue
Block a user