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