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;
}
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
}
}
