130 lines
4.7 KiB
C
130 lines
4.7 KiB
C
#include <stdint.h>
|
|
#include <stdio.h>
|
|
|
|
#include "trikke_protocol.h"
|
|
|
|
static int fail(int code, const char *message)
|
|
{
|
|
fprintf(stderr, "protocol fixture failure %d: %s\n", code, message);
|
|
return code;
|
|
}
|
|
|
|
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 fail(1, "metadata encoding or output");
|
|
}
|
|
|
|
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 fail(2, "basic sample encoding or output");
|
|
}
|
|
|
|
trikke_wire_sample_t full_packet[TRIKKE_WIRE_MAX_RECORDS] = {0};
|
|
for (size_t i = 0; i < TRIKKE_WIRE_MAX_RECORDS; ++i) {
|
|
full_packet[i].sequence = 2000 + (uint32_t)i;
|
|
full_packet[i].timestamp_us = 3000000 + (int64_t)i * 10000;
|
|
full_packet[i].accel_x = (int16_t)i;
|
|
full_packet[i].gyro_z = -(int16_t)i;
|
|
full_packet[i].accel_status = 0x80;
|
|
full_packet[i].gyro_status = 0x08;
|
|
}
|
|
size = trikke_encode_sample_packet(
|
|
packet, sizeof(packet), 43, 2, 3, full_packet,
|
|
TRIKKE_WIRE_MAX_RECORDS);
|
|
if (size != TRIKKE_WIRE_MAX_PACKET_SIZE ||
|
|
fwrite(packet, 1, size, stdout) != size) {
|
|
return fail(3, "maximum-size sample encoding or output");
|
|
}
|
|
|
|
const trikke_wire_sample_t saturated[] = {
|
|
{.sequence = 3000, .timestamp_us = 4000000},
|
|
{.sequence = 3001, .timestamp_us = 4700000},
|
|
};
|
|
size = trikke_encode_sample_packet(packet, sizeof(packet), 44, 2, 3,
|
|
saturated, 2);
|
|
if (size == 0 || fwrite(packet, 1, size, stdout) != size) {
|
|
return fail(4, "saturated timestamp encoding or output");
|
|
}
|
|
|
|
const trikke_wire_status_t status = {
|
|
.sensor_read_failure_count = 5,
|
|
.queue_overflow_count = 6,
|
|
.transport_begin_retry_count = 7,
|
|
.transport_disconnect_count = 8,
|
|
.transport_send_failure_count = 9,
|
|
.transport_replay_count = 10,
|
|
.transport_invalid_ack_count = 11,
|
|
};
|
|
size = trikke_encode_status_packet(packet, sizeof(packet), 45, 5000000,
|
|
11, 12, &status);
|
|
if (size != TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_STATUS_SIZE ||
|
|
fwrite(packet, 1, size, stdout) != size) {
|
|
return fail(5, "status encoding or output");
|
|
}
|
|
|
|
if (!trikke_wire_timestamp_delta_fits(0, 655350) ||
|
|
trikke_wire_timestamp_delta_fits(0, 655351) ||
|
|
trikke_wire_timestamp_delta_fits(1, 0)) {
|
|
return fail(6, "timestamp-delta boundary contract");
|
|
}
|
|
if (trikke_encode_metadata_packet(
|
|
packet, TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_METADATA_SIZE - 1,
|
|
0, 0, 0, 0, &metadata) != 0 ||
|
|
trikke_encode_sample_packet(packet, sizeof(packet), 0, 0, 0,
|
|
samples, 0) != 0 ||
|
|
trikke_encode_sample_packet(packet, sizeof(packet), 0, 0, 0,
|
|
full_packet,
|
|
TRIKKE_WIRE_MAX_RECORDS + 1) != 0 ||
|
|
trikke_encode_sample_packet(packet, TRIKKE_WIRE_MAX_PACKET_SIZE - 1,
|
|
0, 0, 0, full_packet,
|
|
TRIKKE_WIRE_MAX_RECORDS) != 0 ||
|
|
trikke_encode_status_packet(
|
|
packet, TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_STATUS_SIZE - 1,
|
|
0, 0, 0, 0, &status) != 0) {
|
|
return fail(7, "invalid argument rejection contract");
|
|
}
|
|
return 0;
|
|
}
|