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