diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h index 829254fc..0562ce1a 100644 --- a/software/firmware/src/peripherals/include/imu.h +++ b/software/firmware/src/peripherals/include/imu.h @@ -12,15 +12,15 @@ #define BNO055_ID 0xA0 +#define ACC_DATA_LEN 6 #define GYRO_DATA_LEN 6 -#define ACC_DATA_LEN 6 #define LACC_DATA_LEN 6 #define GACC_DATA_LEN 6 #define QUAT_DATA_LEN 8 #define STAT_DATA_LEN 1 typedef void (*motion_change_callback_t)(bool in_motion); -typedef void (*data_ready_callback_t)(int16_t *gyro_data, int16_t *linear_accel_data, int16_t *gravity_data, int16_t *quaternion_data, uint8_t *calib_data, uint8_t *raw_data, uint32_t raw_data_length); +typedef void (*data_ready_callback_t)(uint8_t *calib_data, int16_t *linear_accel_data, uint8_t *raw_data, uint32_t raw_data_length); // TODO: Get rid of raw_data stuff after Wenshan updates BLE Live IMU functions to directly accept relevant data items typedef enum @@ -186,6 +186,16 @@ typedef enum } bno055_reg_t; +typedef enum +{ + GYRO_DATA, + ACC_DATA, + LACC_DATA, + GACC_DATA, + QUAT_DATA, + STAT_DATA, +} bno055_data_type_t; + typedef enum { OPERATION_MODE_CONFIG = 0X00, @@ -310,6 +320,8 @@ void imu_read_axis_remap(bno055_axis_remap_t *remap); bool imu_set_axis_remap(bno055_axis_remap_t remap); void imu_read_euler_data(bno055_euler_t *euler); bool imu_read_in_motion(void); +uint8_t imu_pick_data_from_raw(uint8_t **const picked, uint8_t *raw_data, bno055_data_type_t data_type); +uint8_t imu_copy_data_from_raw(uint8_t *picked, uint8_t *raw_data, bno055_data_type_t data_type); // Math utilities void quaternion_to_euler(bno055_quaternion_t quaternion, bno055_euler_t *euler); diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c index a889b792..8ccea974 100644 --- a/software/firmware/src/peripherals/src/imu.c +++ b/software/firmware/src/peripherals/src/imu.c @@ -30,10 +30,10 @@ static data_ready_callback_t data_ready_callback; // Private Helper Functions -------------------------------------------------------------------------------------------- -static void i2c_write8(uint8_t reg_number, uint8_t reg_value) +static void i2c_write8(uint8_t reg_number, uint8_t reg_value, bool non_blocking) { // Repeat the transfer until it succeeds or requires a device reset - uint32_t body_buffer = reg_value, retries_remaining = 5; + uint32_t body_buffer = reg_value; am_hal_iom_transfer_t write_transaction = { .uPeerInfo.ui32I2CDevAddr = IMU_I2C_ADDRESS, .ui32InstrLen = 1, @@ -48,7 +48,13 @@ static void i2c_write8(uint8_t reg_number, uint8_t reg_value) .ui32PauseCondition = 0, .ui32StatusSetClr = 0 }; - while (retries_remaining-- && (am_hal_iom_blocking_transfer(i2c_handle, &write_transaction) != AM_HAL_STATUS_SUCCESS)); + if (non_blocking) + am_hal_iom_nonblocking_transfer(i2c_handle, &write_transaction, NULL, NULL); + else + { + uint32_t retries_remaining = 5; + while (retries_remaining-- && (am_hal_iom_blocking_transfer(i2c_handle, &write_transaction) != AM_HAL_STATUS_SUCCESS)); + } } static void i2c_read_complete(void *pCallbackCtxt, uint32_t transactionStatus) @@ -67,24 +73,13 @@ static void i2c_read_complete(void *pCallbackCtxt, uint32_t transactionStatus) // Read the data-ready status and trigger the registered callback if (data_ready_callback)// TODO: Only call if interrupt-bit set: && (interrupt_status & (ACC_BSX_DRDY | MAG_DRDY | GYR_DRDY))) { - int16_t* gyro_data = (int16_t*)(raw_data + BNO055_GYRO_DATA_X_LSB_ADDR - BURST_READ_BASE_ADDR); - int16_t* linear_accel_data = (int16_t*)(raw_data + BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR - BURST_READ_BASE_ADDR); - int16_t* gravity_data = (int16_t*)(raw_data + BNO055_GRAVITY_DATA_X_LSB_ADDR - BURST_READ_BASE_ADDR); - int16_t* quaternion_data = (int16_t*)(raw_data + BNO055_QUATERNION_DATA_W_LSB_ADDR - BURST_READ_BASE_ADDR); uint8_t* calib_data = raw_data + BNO055_CALIB_STAT_ADDR - BURST_READ_BASE_ADDR; - // Fix quaternion bit-flipping issue - for (uint8_t i = 0; i < 4; ++i) - { - if (quaternion_data[i] > 16384) - quaternion_data[i] -= 32768; - else if (quaternion_data[i] < -16384) - quaternion_data[i] += 32768; - } - data_ready_callback(gyro_data, linear_accel_data, gravity_data, quaternion_data, calib_data, raw_data, BURST_READ_LEN); + int16_t* linear_accel_data = (int16_t*)(raw_data + BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR - BURST_READ_BASE_ADDR); + data_ready_callback(calib_data, linear_accel_data, raw_data, BURST_READ_LEN); } // Reset the interrupt trigger bits - i2c_write8(BNO055_SYS_TRIGGER_ADDR, 0xC0); + i2c_write8(BNO055_SYS_TRIGGER_ADDR, 0xC0, true); } static uint8_t i2c_read8(uint8_t reg_number) @@ -162,7 +157,7 @@ void imu_iom_isr(void) static void imu_set_op_mode(bno055_opmode_t op_mode) { // Set the indicated mode and delay to allow it to take effect - i2c_write8(BNO055_OPR_MODE_ADDR, op_mode); + i2c_write8(BNO055_OPR_MODE_ADDR, op_mode, false); am_util_delay_ms(30); } @@ -174,7 +169,7 @@ static bno055_opmode_t imu_get_op_mode(void) static void set_use_external_crystal(void) { // Set the IMU to use an external crystal clock source - i2c_write8(BNO055_SYS_TRIGGER_ADDR, 0xC0); + i2c_write8(BNO055_SYS_TRIGGER_ADDR, 0xC0, false); am_util_delay_ms(1000); } @@ -182,53 +177,53 @@ static void enter_suspend_mode(void) { // Switch to configuration mode and suspend imu_set_op_mode(OPERATION_MODE_CONFIG); - i2c_write8(BNO055_PWR_MODE_ADDR, 0x02); + i2c_write8(BNO055_PWR_MODE_ADDR, 0x02, false); } static void enable_motion_interrupts(void) { // Set up interrupts for motion and non-motion events - i2c_write8(BNO055_PAGE_ID_ADDR, 1); - i2c_write8(ACC_CONFIG, 0b00000000); //operation mode/bandwidth/g range - i2c_write8(ACC_AM_THRE, 0b00001010); - i2c_write8(ACC_NM_THRE, 0b00001010); - i2c_write8(ACC_NM_SET, 0b00001001); - i2c_write8(ACC_INT_SET, 0b00011111); //axis and duration for triggering motion interrupt - i2c_write8(INT_MSK, ACC_NM | ACC_AM); - i2c_write8(INT_EN, ACC_NM | ACC_AM); - i2c_write8(BNO055_PAGE_ID_ADDR, 0); + i2c_write8(BNO055_PAGE_ID_ADDR, 1, false); + i2c_write8(ACC_CONFIG, 0b00000000, false); //operation mode/bandwidth/g range + i2c_write8(ACC_AM_THRE, 0b00001010, false); + i2c_write8(ACC_NM_THRE, 0b00001010, false); + i2c_write8(ACC_NM_SET, 0b00001001, false); + i2c_write8(ACC_INT_SET, 0b00011111, false); //axis and duration for triggering motion interrupt + i2c_write8(INT_MSK, ACC_NM | ACC_AM, false); + i2c_write8(INT_EN, ACC_NM | ACC_AM, false); + i2c_write8(BNO055_PAGE_ID_ADDR, 0, false); } static void disable_motion_interrupts(void) { // Disable interrupts for motion and non-motion events - i2c_write8(BNO055_PAGE_ID_ADDR, 1); - i2c_write8(ACC_INT_SET, 0); + i2c_write8(BNO055_PAGE_ID_ADDR, 1, false); + i2c_write8(ACC_INT_SET, 0, false); uint8_t int_msk = i2c_read8(INT_MSK) & ~(ACC_NM | ACC_AM); uint8_t int_en = i2c_read8(INT_EN) & ~(ACC_NM | ACC_AM); - i2c_write8(INT_MSK, int_msk); - i2c_write8(INT_EN, int_en); - i2c_write8(BNO055_PAGE_ID_ADDR, 0); + i2c_write8(INT_MSK, int_msk, false); + i2c_write8(INT_EN, int_en, false); + i2c_write8(BNO055_PAGE_ID_ADDR, 0, false); } static void enable_data_ready_interrupts(void) { - i2c_write8(BNO055_PAGE_ID_ADDR, 1); + i2c_write8(BNO055_PAGE_ID_ADDR, 1, false); uint8_t int_msk = i2c_read8(INT_MSK) | (ACC_BSX_DRDY | MAG_DRDY | GYR_DRDY); uint8_t int_en = i2c_read8(INT_EN) | (ACC_BSX_DRDY | MAG_DRDY | GYR_DRDY); - i2c_write8(INT_MSK, int_msk); - i2c_write8(INT_EN, int_en); - i2c_write8(BNO055_PAGE_ID_ADDR, 0); + i2c_write8(INT_MSK, int_msk, false); + i2c_write8(INT_EN, int_en, false); + i2c_write8(BNO055_PAGE_ID_ADDR, 0, false); } static void disable_data_ready_interrupts(void) { - i2c_write8(BNO055_PAGE_ID_ADDR, 1); + i2c_write8(BNO055_PAGE_ID_ADDR, 1, false); uint8_t int_msk = i2c_read8(INT_MSK) & ~(ACC_BSX_DRDY | MAG_DRDY | GYR_DRDY); uint8_t int_en = i2c_read8(INT_EN) & ~(ACC_BSX_DRDY | MAG_DRDY | GYR_DRDY); - i2c_write8(INT_MSK, int_msk); - i2c_write8(INT_EN, int_en); - i2c_write8(BNO055_PAGE_ID_ADDR, 0); + i2c_write8(INT_MSK, int_msk, false); + i2c_write8(INT_EN, int_en, false); + i2c_write8(BNO055_PAGE_ID_ADDR, 0, false); } @@ -287,15 +282,15 @@ void imu_init(void) // Ensure that the device is accessible and enter configuration mode while (i2c_read8(BNO055_CHIP_ID_ADDR) != BNO055_ID) am_util_delay_ms(100); - i2c_write8(BNO055_PAGE_ID_ADDR, 0); + i2c_write8(BNO055_PAGE_ID_ADDR, 0, false); imu_set_op_mode(OPERATION_MODE_CONFIG); // Set up an external crystal and the sensor output units set_use_external_crystal(); - i2c_write8(BNO055_UNIT_SEL_ADDR, 0); + i2c_write8(BNO055_UNIT_SEL_ADDR, 0, false); // Set device to use the low-power mode - i2c_write8(BNO055_PWR_MODE_ADDR, POWER_MODE_LOWPOWER); + i2c_write8(BNO055_PWR_MODE_ADDR, POWER_MODE_LOWPOWER, false); am_util_delay_ms(30); // Set up incoming interrupts from the IMU @@ -353,7 +348,7 @@ void imu_set_power_mode(bno055_powermode_t power_mode) { bno055_opmode_t saved_mode = imu_get_op_mode(); imu_set_op_mode(OPERATION_MODE_CONFIG); - i2c_write8(BNO055_PWR_MODE_ADDR, power_mode); + i2c_write8(BNO055_PWR_MODE_ADDR, power_mode, false); imu_set_op_mode(saved_mode); } @@ -490,8 +485,8 @@ bool imu_set_axis_remap(bno055_axis_remap_t remap) bno055_opmode_t saved_mode = imu_get_op_mode(); imu_set_op_mode(OPERATION_MODE_CONFIG); - i2c_write8(BNO055_AXIS_MAP_CONFIG_ADDR, reg_axis_map_config); - i2c_write8(BNO055_AXIS_MAP_SIGN_ADDR, reg_axis_map_sign); + i2c_write8(BNO055_AXIS_MAP_CONFIG_ADDR, reg_axis_map_config, false); + i2c_write8(BNO055_AXIS_MAP_SIGN_ADDR, reg_axis_map_sign, false); imu_set_op_mode(saved_mode); //test whether the set is successful @@ -505,3 +500,35 @@ bool imu_read_in_motion(void) { return previously_in_motion; } + +uint8_t imu_pick_data_from_raw(uint8_t **const picked, uint8_t *raw_data, bno055_data_type_t data_type) +{ + switch (data_type) { + case GYRO_DATA: + *picked = raw_data + BNO055_GYRO_DATA_X_LSB_ADDR - BURST_READ_BASE_ADDR; + return GYRO_DATA_LEN; + case LACC_DATA: + *picked = raw_data + BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR - BURST_READ_BASE_ADDR; + return LACC_DATA_LEN; + case GACC_DATA: + *picked = raw_data + BNO055_GRAVITY_DATA_X_LSB_ADDR - BURST_READ_BASE_ADDR; + return GACC_DATA_LEN; + case QUAT_DATA: + *picked = raw_data + BNO055_QUATERNION_DATA_W_LSB_ADDR - BURST_READ_BASE_ADDR; + return QUAT_DATA_LEN; + case STAT_DATA: + *picked = raw_data + BNO055_CALIB_STAT_ADDR - BURST_READ_BASE_ADDR; + return STAT_DATA_LEN; + default: + break; + } + return 0; +} + +uint8_t imu_copy_data_from_raw(uint8_t *picked, uint8_t *raw_data, bno055_data_type_t data_type) +{ + uint8_t *data_ptr; + uint8_t data_len = imu_pick_data_from_raw(&data_ptr, raw_data, data_type); + memcpy(picked, data_ptr, data_type); + return data_len; +} diff --git a/software/firmware/src/tasks/app_task_ranging.c b/software/firmware/src/tasks/app_task_ranging.c index 0ca02500..e2ab7891 100644 --- a/software/firmware/src/tasks/app_task_ranging.c +++ b/software/firmware/src/tasks/app_task_ranging.c @@ -219,7 +219,7 @@ static void motion_change_handler(bool in_motion) } #ifdef _TEST_IMU_DATA -static void data_ready_handler(int16_t *gyro_data, int16_t *linear_accel_data, int16_t *gravity_data, int16_t *quaternion_data, uint8_t *calib_data, uint8_t *raw_data, uint32_t raw_data_length) +static void data_ready_handler(uint8_t *calib_data, int16_t *linear_accel_data, uint8_t *raw_data, uint32_t raw_data_length) { //TODO #ifdef _LIVE_IMU_DATA @@ -228,7 +228,11 @@ static void data_ready_handler(int16_t *gyro_data, int16_t *linear_accel_data, i // Store relevant IMU data #ifndef _TEST_NO_STORAGE - storage_write_imu_data(linear_accel_data); +#ifdef _TEST_IMU_DATA + storage_write_imu_data(raw_data, raw_data_length); +#else + storage_write_imu_data(calib_data, linear_accel_data); +#endif #endif } #endif diff --git a/software/firmware/src/tasks/app_tasks.h b/software/firmware/src/tasks/app_tasks.h index 93295e53..13cd7418 100644 --- a/software/firmware/src/tasks/app_tasks.h +++ b/software/firmware/src/tasks/app_tasks.h @@ -49,7 +49,11 @@ void storage_flush_and_shutdown(void); void storage_write_battery_level(uint32_t battery_voltage_mV); void storage_write_motion_status(bool in_motion); void storage_write_ranging_data(uint32_t timestamp, const uint8_t *ranging_data, uint32_t ranging_data_len, int32_t timestamp_offset); -void storage_write_imu_data(const int16_t *accel_data); +#ifdef _TEST_IMU_DATA +void storage_write_imu_data(const uint8_t *data, uint32_t data_len); +#else +void storage_write_imu_data(const uint8_t *calib_data, const int16_t *accel_data); +#endif // Main Task Functions void AppTaskRanging(void *uid); diff --git a/software/firmware/src/tasks/storage_task.c b/software/firmware/src/tasks/storage_task.c index 36fbbb62..ebe982e7 100644 --- a/software/firmware/src/tasks/storage_task.c +++ b/software/firmware/src/tasks/storage_task.c @@ -98,16 +98,30 @@ void storage_write_ranging_data(uint32_t timestamp, const uint8_t *ranging_data, xQueueSendToBack(storage_queue, &storage_item, 0); } -void storage_write_imu_data(const int16_t *accel_data) +#ifdef _TEST_IMU_DATA +void storage_write_imu_data(const uint8_t *data, uint32_t data_len) { static uint32_t imu_data_index = 0; - const uint32_t rounded_timestamp = 500 * (app_get_experiment_time(ranging_timestamp_offset) / 500); - const storage_item_t storage_item = { .timestamp = rounded_timestamp, .value = imu_data_index, .type = STORAGE_TYPE_IMU }; - memcpy(imu_data[imu_data_index].data, accel_data, 3 * sizeof(int16_t)); - imu_data[imu_data_index].length = 3 * sizeof(int16_t); + const storage_item_t storage_item = { .timestamp = app_get_experiment_time(ranging_timestamp_offset), .value = imu_data_index, .type = STORAGE_TYPE_IMU }; + memcpy(imu_data[imu_data_index].data, data, data_len); + imu_data[imu_data_index].length = data_len; + imu_data_index = (imu_data_index + 1) % STORAGE_QUEUE_MAX_NUM_ITEMS; + xQueueSendToBack(storage_queue, &storage_item, 0); +} +#else +void storage_write_imu_data(const uint8_t *calib_data, const int16_t *accel_data) +{ + static uint32_t imu_data_index = 0; + const storage_item_t storage_item = { .timestamp = app_get_experiment_time(ranging_timestamp_offset), .value = imu_data_index, .type = STORAGE_TYPE_IMU }; + imu_data[imu_data_index].length = 0; + memcpy(imu_data[imu_data_index].data + imu_data[imu_data_index].length, calib_data, sizeof(uint8_t)); + imu_data[imu_data_index].length += sizeof(uint8_t); + memcpy(imu_data[imu_data_index].data + imu_data[imu_data_index].length, accel_data, 3 * sizeof(int16_t)); + imu_data[imu_data_index].length += 3 * sizeof(int16_t); imu_data_index = (imu_data_index + 1) % STORAGE_QUEUE_MAX_NUM_ITEMS; xQueueSendToBack(storage_queue, &storage_item, 0); } +#endif // #ifdef _TEST_IMU_DATA #else diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c index 89fe5309..e24321b1 100644 --- a/software/firmware/tests/peripherals/test_imu.c +++ b/software/firmware/tests/peripherals/test_imu.c @@ -7,14 +7,21 @@ static void motion_changed(bool in_motion) print("Device is %s\n", in_motion ? "IN MOTION" : "STATIONARY"); } -static void data_ready(int16_t *gyro_data, int16_t *linear_accel_data, int16_t *gravity_data, int16_t *quaternion_data, uint8_t *calib_data, uint8_t *raw_data, uint32_t raw_data_length) +static void data_ready(uint8_t *calib_data, int16_t *linear_accel_data, uint8_t *raw_data, uint32_t raw_data_length) { - // Print out calibration status + // Pick the relevant data types from the raw data buffer + uint8_t *calib, *lacc, *quat, *gyro; + imu_pick_data_from_raw(&calib, raw_data, STAT_DATA); + imu_pick_data_from_raw(&lacc, raw_data, LACC_DATA); + imu_pick_data_from_raw(&quat, raw_data, QUAT_DATA); + imu_pick_data_from_raw(&gyro, raw_data, GYRO_DATA); + + // Print a summary of the IMU data print("IMU Data:\n"); - print(" Calibration status: sys %u, gyro %u, accel %u, mag %u\n", calib_data[0] >> 6, (calib_data[0] >> 4) & 0x03, (calib_data[0] >> 2) & 0x03, calib_data[0] & 0x03); - print(" Linear Accel: X = %d, Y = %d, Z = %d\n", linear_accel_data[0], linear_accel_data[1], linear_accel_data[2]); - print(" Quaternion: qw = %d, qx = %d, qy = %d, qz = %d\n", quaternion_data[0], quaternion_data[1], quaternion_data[2], quaternion_data[3]); - print(" Gyroscope: gx = %d, gy = %d, gz = %d\n", gyro_data[0], gyro_data[1], gyro_data[2]); + print(" Calibration status: sys %u, gyro %u, accel %u, mag %u\n", calib[0] >> 6, (calib[0] >> 4) & 0x03, (calib[0] >> 2) & 0x03, calib[0] & 0x03); + print(" Linear Accel: X = %d, Y = %d, Z = %d\n", lacc[0], lacc[1], lacc[2]); + print(" Quaternion: qw = %d, qx = %d, qy = %d, qz = %d\n", quat[0], quat[1], quat[2], quat[3]); + print(" Gyroscope: gx = %d, gy = %d, gz = %d\n", gyro[0], gyro[1], gyro[2]); } int main(void)