205 lines
6.8 KiB
C
205 lines
6.8 KiB
C
#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);
|
|
}
|
|
|
|
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;
|
|
}
|