thejayman77/trikkeSensors
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add framed binary telemetry transport

Browse Source
This commit is contained in:
Jay
2026-08-17 11:13:53 -04:00
parent a5c3087ee4
commit aceaa2b270
13 changed files with 1320 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files
main/trikke_protocol.c
+187
View File
@@ -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;
}