TEST test_entry_write_multiple(void)
{
uint32_t pos_a = test_logger.flash_write_pos;
cmp_ctx_t *ctx;
ctx = _log_entry_create(&test_logger, "a");
cmp_write_integer(ctx, 42);
_log_entry_write_to_flash(&test_logger);
uint32_t pos_b = test_logger.flash_write_pos;
ASSERT(pos_a < pos_b);
ctx = _log_entry_create(&test_logger, "b");
cmp_write_integer(ctx, 23);
_log_entry_write_to_flash(&test_logger);
cmp_ctx_t reader;
cmp_mem_access_t cma;
cmp_mem_access_ro_init(&reader, &cma, &flash_array[pos_a + LOG_ENTRY_HEADER_LEN], LOG_ENTRY_DATA_LEN);
uint32_t size = 0;
uint64_t timestamp = 0;
int64_t val = 0;
char name[10];
bool ok;
size = sizeof(name);
ok = true;
ok &= cmp_read_array(&reader, &size);
ok &= cmp_read_uinteger(&reader, ×tamp);
ok &= cmp_read_str(&reader, name, &size);
ok &= cmp_read_integer(&reader, &val);
ASSERT(ok);
ASSERT_EQ(val, 42);
size_t pos = cmp_mem_access_get_pos(&cma);
ASSERT_EQ(pos, flash_array[0]);
ASSERT_EQ(pos + LOG_ENTRY_HEADER_LEN, pos_b);
cmp_mem_access_ro_init(&reader, &cma, &flash_array[pos_b + LOG_ENTRY_HEADER_LEN], LOG_ENTRY_DATA_LEN);
size = sizeof(name);
ok = true;
ok &= cmp_read_array(&reader, &size);
ok &= cmp_read_uinteger(&reader, ×tamp);
ok &= cmp_read_str(&reader, name, &size);
ok &= cmp_read_integer(&reader, &val);
ASSERT(ok);
ASSERT_EQ(val, 23);
ASSERT_EQ(flash_array[pos_b], cmp_mem_access_get_pos(&cma));
PASS();
}
void sender_thread(void *arg)
{
chRegSetThreadName("USB sender");
struct io_dev_s *dev = (struct io_dev_s *)arg;
char message_buffer[32];
cmp_ctx_t cmp;
cmp_mem_access_t mem;
cmp_mem_access_init(&cmp, &mem, message_buffer, sizeof(message_buffer));
while (1) {
struct can_rx_frame_s *fp;
bool drop;
fp = can_frame_receive(&drop);
if (drop) {
can_drop_msg_encode(&cmp);
} else {
if (fp == NULL) {
continue;
} else if (fp->error) {
can_error_msg_encode(&cmp, fp->timestamp);
} else {
can_rx_msg_encode(&cmp, &fp->frame, fp->timestamp);
}
can_rx_frame_delete(fp);
}
size_t len = cmp_mem_access_get_pos(&mem);
io_dev_send(message_buffer, len, dev);
cmp_mem_access_set_pos(&mem, 0);
}
}
size_t rpc_transmit(uint8_t *input_buffer, size_t input_buffer_size,
uint8_t *output_buffer, size_t output_buffer_size,
ip_addr_t *addr, uint16_t port)
{
struct netconn *conn;
int err;
cmp_ctx_t ctx; /* For cmp_mem_access. */
cmp_mem_access_t mem;
struct netbuf *buf;
u16_t len;
char *data;
conn = netconn_new(NETCONN_TCP);
if (conn == NULL) {
return -1;
}
err = netconn_connect(conn, addr, port);
if (err != ERR_OK) {
goto fail;
}
serial_datagram_send((void *)input_buffer, input_buffer_size,
netconn_serial_datagram_tx_adapter, (void *)conn);
cmp_mem_access_init(&ctx, &mem, output_buffer, output_buffer_size);
while (1) {
err = netconn_recv(conn, &buf);
/* If connection was closed by server, abort */
if (err != ERR_OK) {
break;
}
do {
netbuf_data(buf, (void **)&data, &len);
/* Append data to buffer. */
ctx.write(&ctx, data, len);
} while (netbuf_next(buf) >= 0);
netbuf_delete(buf);
}
netconn_delete(conn);
return cmp_mem_access_get_pos(&mem);
fail:
netconn_delete(conn);
return -1;
}
bool can_bridge_frame_write(struct can_frame *frame, uint8_t *outbuf, size_t *len)
{
cmp_mem_access_t cma;
cmp_ctx_t ctx;
cmp_mem_access_init(&ctx, &cma, outbuf, *len);
// encode with MessagePack
if (!can_frame_cmp_write(&ctx, frame)) {
return false;
}
*len = cmp_mem_access_get_pos(&cma);
return true;
}
TEST test_entry_crc_and_length(void)
{
cmp_ctx_t *ctx = _log_entry_create(&test_logger, "entry");
cmp_write_integer(ctx, 42);
_log_entry_write_to_flash(&test_logger);
size_t entry_length = cmp_mem_access_get_pos(&test_logger.cma);
uint8_t len = flash_array[0];
uint8_t crc = flash_array[1];
if (crc8(0, &flash_array[2], len) != crc) {
FAILm("CRC missmatch");
}
if (entry_length != len) {
FAILm("entry length missmatch");
}
PASS();
}
static void stream_thread(void *p)
{
chRegSetThreadName("stream");
static uint8_t buffer[64];
static char topic_name[TOPIC_NAME_LEN];
cmp_ctx_t ctx;
cmp_mem_access_t mem;
ip_addr_t server;
(void) p;
STREAM_HOST(&server);
while (1) {
motor_driver_t *drv_list;
uint16_t drv_list_len;
motor_manager_get_list(&motor_manager, &drv_list, &drv_list_len);
int i;
for (i = 0; i < drv_list_len; i++) {
if (motor_driver_get_stream_change_status(&drv_list[i]) != 0) {
if (motor_driver_get_stream_change_status(&drv_list[i])
& (1 << MOTOR_STREAM_CURRENT_SETPT)) {
strncpy(topic_name, "actuator/", TOPIC_NAME_LEN);
strncat(topic_name, motor_driver_get_id(&drv_list[i]), TOPIC_NAME_LEN);
strncat(topic_name, "/current_setp", TOPIC_NAME_LEN);
message_write_header(&ctx, &mem, buffer, sizeof(buffer), topic_name);
cmp_write_float(&ctx, motor_driver_get_and_clear_stream_value(&drv_list[i], MOTOR_STREAM_CURRENT_SETPT));
message_transmit(buffer, cmp_mem_access_get_pos(&mem), &server, STREAM_PORT);
}
if (motor_driver_get_stream_change_status(&drv_list[i])
& (1 << MOTOR_STREAM_CURRENT)) {
strncpy(topic_name, "actuator/", TOPIC_NAME_LEN);
strncat(topic_name, motor_driver_get_id(&drv_list[i]), TOPIC_NAME_LEN);
strncat(topic_name, "/current", TOPIC_NAME_LEN);
message_write_header(&ctx, &mem, buffer, sizeof(buffer), topic_name);
cmp_write_float(&ctx, motor_driver_get_and_clear_stream_value(&drv_list[i], MOTOR_STREAM_CURRENT));
message_transmit(buffer, cmp_mem_access_get_pos(&mem), &server, STREAM_PORT);
}
if (motor_driver_get_stream_change_status(&drv_list[i])
& (1 << MOTOR_STREAM_MOTOR_VOLTAGE)) {
strncpy(topic_name, "actuator/", TOPIC_NAME_LEN);
strncat(topic_name, motor_driver_get_id(&drv_list[i]), TOPIC_NAME_LEN);
strncat(topic_name, "/voltage", TOPIC_NAME_LEN);
message_write_header(&ctx, &mem, buffer, sizeof(buffer), topic_name);
cmp_write_float(&ctx, motor_driver_get_and_clear_stream_value(&drv_list[i], MOTOR_STREAM_MOTOR_VOLTAGE));
message_transmit(buffer, cmp_mem_access_get_pos(&mem), &server, STREAM_PORT);
}
if (motor_driver_get_stream_change_status(&drv_list[i])
& (1 << MOTOR_STREAM_VELOCITY_SETPT)) {
strncpy(topic_name, "actuator/", TOPIC_NAME_LEN);
strncat(topic_name, motor_driver_get_id(&drv_list[i]), TOPIC_NAME_LEN);
strncat(topic_name, "/velocity_setp", TOPIC_NAME_LEN);
message_write_header(&ctx, &mem, buffer, sizeof(buffer), topic_name);
cmp_write_float(&ctx, motor_driver_get_and_clear_stream_value(&drv_list[i], MOTOR_STREAM_VELOCITY_SETPT));
message_transmit(buffer, cmp_mem_access_get_pos(&mem), &server, STREAM_PORT);
}
if (motor_driver_get_stream_change_status(&drv_list[i])
& (1 << MOTOR_STREAM_VELOCITY)) {
strncpy(topic_name, "actuator/", TOPIC_NAME_LEN);
strncat(topic_name, motor_driver_get_id(&drv_list[i]), TOPIC_NAME_LEN);
strncat(topic_name, "/velocity", TOPIC_NAME_LEN);
message_write_header(&ctx, &mem, buffer, sizeof(buffer), topic_name);
cmp_write_float(&ctx, motor_driver_get_and_clear_stream_value(&drv_list[i], MOTOR_STREAM_VELOCITY));
message_transmit(buffer, cmp_mem_access_get_pos(&mem), &server, STREAM_PORT);
}
if (motor_driver_get_stream_change_status(&drv_list[i])
& (1 << MOTOR_STREAM_POSITION_SETPT)) {
strncpy(topic_name, "actuator/", TOPIC_NAME_LEN);
strncat(topic_name, motor_driver_get_id(&drv_list[i]), TOPIC_NAME_LEN);
strncat(topic_name, "/position_setp", TOPIC_NAME_LEN);
message_write_header(&ctx, &mem, buffer, sizeof(buffer), topic_name);
cmp_write_float(&ctx, motor_driver_get_and_clear_stream_value(&drv_list[i], MOTOR_STREAM_POSITION_SETPT));
message_transmit(buffer, cmp_mem_access_get_pos(&mem), &server, STREAM_PORT);
}
if (motor_driver_get_stream_change_status(&drv_list[i])
& (1 << MOTOR_STREAM_POSITION)) {
strncpy(topic_name, "actuator/", TOPIC_NAME_LEN);
strncat(topic_name, motor_driver_get_id(&drv_list[i]), TOPIC_NAME_LEN);
strncat(topic_name, "/position", TOPIC_NAME_LEN);
message_write_header(&ctx, &mem, buffer, sizeof(buffer), topic_name);
cmp_write_float(&ctx, motor_driver_get_and_clear_stream_value(&drv_list[i], MOTOR_STREAM_POSITION));
message_transmit(buffer, cmp_mem_access_get_pos(&mem), &server, STREAM_PORT);
}
if (motor_driver_get_stream_change_status(&drv_list[i])
& (1 << MOTOR_STREAM_INDEX)) {
strncpy(topic_name, "actuator/", TOPIC_NAME_LEN);
strncat(topic_name, motor_driver_get_id(&drv_list[i]), TOPIC_NAME_LEN);
strncat(topic_name, "/index", TOPIC_NAME_LEN);
message_write_header(&ctx, &mem, buffer, sizeof(buffer), topic_name);
cmp_write_array(&ctx, 2);
cmp_write_float(&ctx, motor_driver_get_and_clear_stream_value(&drv_list[i], MOTOR_STREAM_INDEX));
cmp_write_uint(&ctx, drv_list[i].stream.value_stream_index_update_count);
message_transmit(buffer, cmp_mem_access_get_pos(&mem), &server, STREAM_PORT);
}
if (motor_driver_get_stream_change_status(&drv_list[i])
& (1 << MOTOR_STREAM_MOTOR_ENCODER)) {
strncpy(topic_name, "actuator/", TOPIC_NAME_LEN);
strncat(topic_name, motor_driver_get_id(&drv_list[i]), TOPIC_NAME_LEN);
strncat(topic_name, "/encoder", TOPIC_NAME_LEN);
message_write_header(&ctx, &mem, buffer, sizeof(buffer), topic_name);
cmp_write_float(&ctx, motor_driver_get_and_clear_stream_value(&drv_list[i], MOTOR_STREAM_MOTOR_ENCODER));
message_transmit(buffer, cmp_mem_access_get_pos(&mem), &server, STREAM_PORT);
}
if (motor_driver_get_stream_change_status(&drv_list[i])
& (1 << MOTOR_STREAM_MOTOR_TORQUE)) {
strncpy(topic_name, "actuator/", TOPIC_NAME_LEN);
strncat(topic_name, motor_driver_get_id(&drv_list[i]), TOPIC_NAME_LEN);
strncat(topic_name, "/torque", TOPIC_NAME_LEN);
message_write_header(&ctx, &mem, buffer, sizeof(buffer), topic_name);
cmp_write_float(&ctx, motor_driver_get_and_clear_stream_value(&drv_list[i], MOTOR_STREAM_MOTOR_TORQUE));
message_transmit(buffer, cmp_mem_access_get_pos(&mem), &server, STREAM_PORT);
}
}
}
chThdSleepMilliseconds(STREAM_TIMESTEP_MS);
}
}
static THD_FUNCTION(stream, arg)
{
(void)arg;
chRegSetThreadName("stream");
static event_listener_t sensor_event_listener;
chEvtRegisterMaskWithFlags(&sensor_events, &sensor_event_listener,
(eventmask_t)ONBOARDSENSOR_EVENT,
(eventflags_t)SENSOR_EVENT_MPU6000 | SENSOR_EVENT_H3LIS331DL | SENSOR_EVENT_HMC5883L | SENSOR_EVENT_MS5611);
static char dtgrm[100];
static cmp_mem_access_t mem;
static cmp_ctx_t cmp;
while (1) {
eventmask_t events = chEvtWaitAny(ONBOARDSENSOR_EVENT);
if (events & ONBOARDSENSOR_EVENT) {
eventflags_t event_flags = chEvtGetAndClearFlags(&sensor_event_listener);
if (event_flags & SENSOR_EVENT_MPU6000) {
rate_gyro_sample_t gyro;
accelerometer_sample_t acc;
onboard_sensor_get_mpu6000_gyro_sample(&gyro);
onboard_sensor_get_mpu6000_acc_sample(&acc);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "imu");
err = err || !cmp_write_map(&cmp, 3);
err = err || !CMP_WRITE_C_STRING(cmp, "gyro");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, gyro.rate[0]);
err = err || !cmp_write_float(&cmp, gyro.rate[1]);
err = err || !cmp_write_float(&cmp, gyro.rate[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "acc");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, acc.acceleration[0]);
err = err || !cmp_write_float(&cmp, acc.acceleration[1]);
err = err || !cmp_write_float(&cmp, acc.acceleration[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, gyro.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
err = false;
float att[4];
attitude_determination_get_attitude(att);
err = err || !msg_header_write(&cmp, "att");
err = err || !cmp_write_array(&cmp, 4);
err = err || !cmp_write_float(&cmp, att[0]);
err = err || !cmp_write_float(&cmp, att[1]);
err = err || !cmp_write_float(&cmp, att[2]);
err = err || !cmp_write_float(&cmp, att[3]);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
if (event_flags & SENSOR_EVENT_HMC5883L) {
magnetometer_sample_t magnetometer;
onboard_sensor_get_hmc5883l_mag_sample(&magnetometer);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "mag");
err = err || !cmp_write_map(&cmp, 2);
err = err || !CMP_WRITE_C_STRING(cmp, "field");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, magnetometer.magnetic_field[0]);
err = err || !cmp_write_float(&cmp, magnetometer.magnetic_field[1]);
err = err || !cmp_write_float(&cmp, magnetometer.magnetic_field[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, magnetometer.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
if (event_flags & SENSOR_EVENT_H3LIS331DL) {
accelerometer_sample_t acc;
onboard_sensor_get_h3lis331dl_acc_sample(&acc);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "hi_acc");
err = err || !cmp_write_map(&cmp, 2);
err = err || !CMP_WRITE_C_STRING(cmp, "acc");
err = err || !cmp_write_array(&cmp, 3);
err = err || !cmp_write_float(&cmp, acc.acceleration[0]);
err = err || !cmp_write_float(&cmp, acc.acceleration[1]);
err = err || !cmp_write_float(&cmp, acc.acceleration[2]);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, acc.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
if (event_flags & SENSOR_EVENT_MS5611) {
barometer_sample_t baro;
onboard_sensor_get_ms5511_baro_sample(&baro);
cmp_mem_access_init(&cmp, &mem, dtgrm, sizeof(dtgrm));
bool err = false;
err = err || !msg_header_write(&cmp, "baro");
err = err || !cmp_write_map(&cmp, 3);
err = err || !CMP_WRITE_C_STRING(cmp, "static_press");
err = err || !cmp_write_float(&cmp, baro.pressure);
err = err || !CMP_WRITE_C_STRING(cmp, "air_temp");
err = err || !cmp_write_float(&cmp, baro.temperature);
err = err || !CMP_WRITE_C_STRING(cmp, "time");
err = err || !cmp_write_uint(&cmp, baro.timestamp);
if (!err) {
datagram_message_send(dtgrm, cmp_mem_access_get_pos(&mem));
}
}
} // ONBOARDSENSOR_EVENT
}
}
