#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); } bool trikke_wire_timestamp_delta_fits( int64_t previous_timestamp_us, int64_t timestamp_us) { if (timestamp_us < previous_timestamp_us) { return false; } const uint64_t delta_us = (uint64_t)timestamp_us - (uint64_t)previous_timestamp_us; return delta_us <= TRIKKE_WIRE_MAX_TIMESTAMP_DELTA_US; } size_t trikke_encode_metadata_packet( 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) { if (!trikke_wire_timestamp_delta_fits( previous_timestamp_us, samples[i].timestamp_us)) { timestamp_delta_10us = UINT16_MAX; packet_flags |= TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED; } else { const uint64_t delta_us = (uint64_t)samples[i].timestamp_us - (uint64_t)previous_timestamp_us; timestamp_delta_10us = (uint16_t)( (delta_us + TRIKKE_WIRE_TIMESTAMP_DELTA_UNIT_US / 2) / TRIKKE_WIRE_TIMESTAMP_DELTA_UNIT_US); } } 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; }