From 8939599aa09bf8536e17acd8e58c205f85946c40 Mon Sep 17 00:00:00 2001 From: Will Hedgecock Date: Wed, 17 Jul 2024 17:24:25 -0500 Subject: [PATCH] Lots of IMU cleanup, merged block/nonblocking code --- software/firmware/src/app/app_config.h | 3 +- .../firmware/src/peripherals/include/imu.h | 34 +- software/firmware/src/peripherals/src/imu.c | 299 +++++++----------- .../firmware/src/tasks/app_task_ranging.c | 26 +- software/firmware/src/tasks/app_tasks.h | 2 +- software/firmware/src/tasks/storage_task.c | 17 +- software/firmware/tests/Makefile | 7 +- .../firmware/tests/peripherals/test_imu.c | 87 +---- 8 files changed, 156 insertions(+), 319 deletions(-) diff --git a/software/firmware/src/app/app_config.h b/software/firmware/src/app/app_config.h index a3355535..51111427 100644 --- a/software/firmware/src/app/app_config.h +++ b/software/firmware/src/app/app_config.h @@ -37,8 +37,7 @@ #define MAX_COMPRESSED_RANGE_DATA_LENGTH (1 + (COMPRESSED_RANGE_DATUM_LENGTH * MAX_NUM_RANGING_DEVICES)) #define MAX_IMU_DATA_LENGTH 40 -#define STORAGE_QUEUE_MAX_NUM_ITEMS 24 -#define STORAGE_IMU_BUFFER_NUM_ITEMS 50 +#define STORAGE_QUEUE_MAX_NUM_ITEMS 25 #define BATTERY_CHECK_INTERVAL_S 300 diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h index 3c7b04d8..829254fc 100644 --- a/software/firmware/src/peripherals/include/imu.h +++ b/software/firmware/src/peripherals/include/imu.h @@ -1,7 +1,6 @@ #ifndef __IMU_HEADER_H__ #define __IMU_HEADER_H__ -#define RAD_TO_DEG(radian) (radian * 180.0 / 3.14159265358979f) // Header Inclusions --------------------------------------------------------------------------------------------------- #include "app_config.h" @@ -9,11 +8,9 @@ // Peripheral Type Definitions ----------------------------------------------------------------------------------------- -#define BNO055_ID 0xA0 +#define RAD_TO_DEG(radian) (radian * 180.0 / 3.14159265358979f) -// For burst data transfer -#define BURST_READ_BASE_ADDR BNO055_GYRO_DATA_X_LSB_ADDR -#define BURST_READ_LEN 40 +#define BNO055_ID 0xA0 #define GYRO_DATA_LEN 6 #define ACC_DATA_LEN 6 @@ -23,11 +20,8 @@ #define STAT_DATA_LEN 1 typedef void (*motion_change_callback_t)(bool in_motion); -#if NONBLOCKING -typedef void (*data_ready_callback_t)(uint8_t *localBuffer); -#else -typedef void (*data_ready_callback_t)(uint8_t interrupt_status); -#endif +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); +// TODO: Get rid of raw_data stuff after Wenshan updates BLE Live IMU functions to directly accept relevant data items typedef enum { @@ -192,16 +186,6 @@ 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, @@ -229,7 +213,7 @@ typedef enum GYRO_AM = 0b00000100, MAG_DRDY = 0b00000010, ACC_BSX_DRDY = 0b00000001 -}bno055_intmsk_t; +} bno055_intmsk_t; typedef enum { @@ -273,7 +257,7 @@ typedef struct uint8_t x_remap_sign; uint8_t y_remap_sign; uint8_t z_remap_sign; -}bno055_axis_remap_t; +} bno055_axis_remap_t; typedef struct { @@ -288,7 +272,7 @@ typedef struct double yaw; double pitch; double roll; -}bno055_euler_t; +} bno055_euler_t; typedef struct { @@ -302,7 +286,7 @@ typedef struct int16_t x; int16_t y; int16_t z; -}bno055_gyro_t; +} bno055_gyro_t; // Public API Functions ------------------------------------------------------------------------------------------------ @@ -326,8 +310,6 @@ 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); -void imu_read_burst_buffer(uint8_t *destBuffer); -uint8_t imu_pick_data_from_burst_buffer(uint8_t *picked, uint8_t *full, 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 74d366a2..a889b792 100644 --- a/software/firmware/src/peripherals/src/imu.c +++ b/software/firmware/src/peripherals/src/imu.c @@ -6,61 +6,41 @@ #include "logging.h" +// IMU Definitions ----------------------------------------------------------------------------------------------------- + +// ISR Definitions +#define imu_iom_isr am_iom_isr1(IMU_I2C_NUMBER) +#define am_iom_isr1(n) am_iom_isr(n) +#define am_iom_isr(n) am_iomaster ## n ## _isr + +// Burst data transfer definitions +#define BURST_READ_BASE_ADDR BNO055_GYRO_DATA_X_LSB_ADDR +#define BURST_READ_LAST_ADDR BNO055_INTR_STAT_ADDR +#define BURST_READ_LEN (BURST_READ_LAST_ADDR - BURST_READ_BASE_ADDR + 1) + + // Static Global Variables --------------------------------------------------------------------------------------------- static void *i2c_handle; static volatile bool previously_in_motion; +static uint32_t imu_buffer[BURST_READ_LEN]; static motion_change_callback_t motion_change_callback; static data_ready_callback_t data_ready_callback; -#if NONBLOCKING -static uint32_t imuBuffer[2048]; -static uint8_t gReadBuffer[BURST_READ_LEN] = {0}; - -#define imu_iom_isr \ - am_iom_isr1(IMU_I2C_NUMBER) -#define am_iom_isr1(n) \ - am_iom_isr(n) -#define am_iom_isr(n) \ - am_iomaster ## n ## _isr -#define IMU_IOM_IRQn ((IRQn_Type)(IOMSTR0_IRQn + IMU_I2C_NUMBER)) - -void imu_iom_isr(void) -{ - uint32_t ui32Status; - - if (!am_hal_iom_interrupt_status_get(i2c_handle, true, &ui32Status)) - { - if ( ui32Status ) - { - am_hal_iom_interrupt_clear(i2c_handle, ui32Status); - am_hal_iom_interrupt_service(i2c_handle, ui32Status); - } - } -} - -static void nonblocking_read_complete(void *pCallbackCtxt, uint32_t transactionStatus) -{ - uint8_t saved_buffer[BURST_READ_LEN] = {0}; - imu_read_burst_buffer(saved_buffer); - data_ready_callback(saved_buffer); -} -#endif // Private Helper Functions -------------------------------------------------------------------------------------------- - static void i2c_write8(uint8_t reg_number, uint8_t reg_value) { // Repeat the transfer until it succeeds or requires a device reset - uint32_t bodyBuffer = reg_value, retries_remaining = 5; + uint32_t body_buffer = reg_value, retries_remaining = 5; am_hal_iom_transfer_t write_transaction = { .uPeerInfo.ui32I2CDevAddr = IMU_I2C_ADDRESS, .ui32InstrLen = 1, .ui64Instr = reg_number, .eDirection = AM_HAL_IOM_TX, .ui32NumBytes = 1, - .pui32TxBuffer = &bodyBuffer, + .pui32TxBuffer = &body_buffer, .pui32RxBuffer = NULL, .bContinue = false, .ui8RepeatCount = 0, @@ -71,41 +51,55 @@ static void i2c_write8(uint8_t reg_number, uint8_t reg_value) while (retries_remaining-- && (am_hal_iom_blocking_transfer(i2c_handle, &write_transaction) != AM_HAL_STATUS_SUCCESS)); } -static uint8_t i2c_read8(uint8_t reg_number) +static void i2c_read_complete(void *pCallbackCtxt, uint32_t transactionStatus) { - // Repeat the transfer until it succeeds or requires a device reset - static uint32_t readBuffer; - uint32_t retries_remaining = 5; - am_hal_iom_transfer_t read_transaction = { - .uPeerInfo.ui32I2CDevAddr = IMU_I2C_ADDRESS, - .ui32InstrLen = 1, - .ui64Instr = reg_number, - .eDirection = AM_HAL_IOM_RX, - .ui32NumBytes = 1, - .pui32TxBuffer = NULL, - .pui32RxBuffer = &readBuffer, - .bContinue = false, - .ui8RepeatCount = 0, - .ui8Priority = 1, - .ui32PauseCondition = 0, - .ui32StatusSetClr = 0 - }; - while (retries_remaining-- && (am_hal_iom_blocking_transfer(i2c_handle, &read_transaction) != AM_HAL_STATUS_SUCCESS)); - return ((uint8_t*)&readBuffer)[0]; + // Read the device motion status and trigger the registered callback + uint8_t *raw_data = (uint8_t*)imu_buffer; + const uint8_t interrupt_status = *(raw_data + BNO055_INTR_STAT_ADDR - BURST_READ_BASE_ADDR); + const bool in_motion_fired = interrupt_status & ACC_AM, no_motion_fired = interrupt_status & ACC_NM; + if ((in_motion_fired && !previously_in_motion) || (no_motion_fired && previously_in_motion)) + { + previously_in_motion = !previously_in_motion; + if (motion_change_callback) + motion_change_callback(previously_in_motion); + } + + // 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); + } + + // Reset the interrupt trigger bits + i2c_write8(BNO055_SYS_TRIGGER_ADDR, 0xC0); } -static void i2c_read(uint8_t reg_number, uint8_t *read_buffer, uint32_t buffer_length) +static uint8_t i2c_read8(uint8_t reg_number) { // Repeat the transfer until it succeeds or requires a device reset + static uint32_t read_buffer; uint32_t retries_remaining = 5; am_hal_iom_transfer_t read_transaction = { .uPeerInfo.ui32I2CDevAddr = IMU_I2C_ADDRESS, .ui32InstrLen = 1, .ui64Instr = reg_number, .eDirection = AM_HAL_IOM_RX, - .ui32NumBytes = buffer_length, + .ui32NumBytes = 1, .pui32TxBuffer = NULL, - .pui32RxBuffer = (uint32_t*)read_buffer, + .pui32RxBuffer = &read_buffer, .bContinue = false, .ui8RepeatCount = 0, .ui8Priority = 1, @@ -113,11 +107,12 @@ static void i2c_read(uint8_t reg_number, uint8_t *read_buffer, uint32_t buffer_l .ui32StatusSetClr = 0 }; while (retries_remaining-- && (am_hal_iom_blocking_transfer(i2c_handle, &read_transaction) != AM_HAL_STATUS_SUCCESS)); + return ((uint8_t*)&read_buffer)[0]; } -#if NONBLOCKING -static uint8_t i2c_nonblocking_read(uint8_t reg_number, uint8_t *read_buffer, uint32_t buffer_length) +static void i2c_read(uint8_t reg_number, uint8_t *read_buffer, uint32_t buffer_length, bool non_blocking) { + // Repeat the transfer until it succeeds or requires a device reset am_hal_iom_transfer_t read_transaction = { .uPeerInfo.ui32I2CDevAddr = IMU_I2C_ADDRESS, .ui32InstrLen = 1, @@ -132,51 +127,36 @@ static uint8_t i2c_nonblocking_read(uint8_t reg_number, uint8_t *read_buffer, ui .ui32PauseCondition = 0, .ui32StatusSetClr = 0 }; - uint32_t status = am_hal_iom_nonblocking_transfer(i2c_handle, &read_transaction, nonblocking_read_complete, NULL); - if (status!=AM_HAL_STATUS_SUCCESS) + if (non_blocking) + am_hal_iom_nonblocking_transfer(i2c_handle, &read_transaction, i2c_read_complete, NULL); + else { - return status; + uint32_t retries_remaining = 5; + while (retries_remaining-- && (am_hal_iom_blocking_transfer(i2c_handle, &read_transaction) != AM_HAL_STATUS_SUCCESS)); } - return AM_HAL_STATUS_SUCCESS; } -#endif + + +// Interrupt Service Routines ------------------------------------------------------------------------------------------ static void imu_isr(void *args) { -#if NONBLOCKING - //read useful data in one go - i2c_nonblocking_read(BURST_READ_BASE_ADDR, gReadBuffer, BURST_READ_LEN); -#else - // Read the device motion status and trigger the registered callback - const uint8_t interrupt_status = i2c_read8(BNO055_INTR_STAT_ADDR); - const bool in_motion_fired = interrupt_status & ACC_AM; - const bool no_motion_fired = interrupt_status & ACC_NM; - //print("interrupt status (blocking)%u\n",interrupt_status); - if ((in_motion_fired && !previously_in_motion) || (no_motion_fired && previously_in_motion)) - { - previously_in_motion = !previously_in_motion; - if (motion_change_callback!=NULL) - motion_change_callback(previously_in_motion); - } - if (interrupt_status & (ACC_BSX_DRDY | MAG_DRDY | GYR_DRDY)) - { - if (data_ready_callback!=NULL) - data_ready_callback(interrupt_status); - } - i2c_write8(BNO055_SYS_TRIGGER_ADDR, 0xC0);//reset all interrupt status bits, and INT output -#endif + // Initiate an IMU burst read + i2c_read(BURST_READ_BASE_ADDR, (uint8_t*)imu_buffer, BURST_READ_LEN, true); } -static void read_int16_vector(uint8_t reg_number, int16_t *read_buffer, uint32_t byte_count){ - static uint8_t byte_array[22]; - memset(byte_array, 0, 22); - memset(read_buffer, 0, byte_count); - i2c_read(reg_number, byte_array, byte_count); - for (uint32_t i = 0; i < byte_count/2; i++){ - read_buffer[i] = ((int16_t)byte_array[i*2]) | (((int16_t)byte_array[i*2+1]) << 8); - } +void imu_iom_isr(void) +{ + // Handle an IMU read interrupt + static uint32_t status; + AM_CRITICAL_BEGIN + am_hal_iom_interrupt_status_get(i2c_handle, false, &status); + am_hal_iom_interrupt_clear(i2c_handle, status); + AM_CRITICAL_END + am_hal_iom_interrupt_service(i2c_handle, status); } + // IMU Chip-Specific API Functions ------------------------------------------------------------------------------------- static void imu_set_op_mode(bno055_opmode_t op_mode) @@ -205,16 +185,6 @@ static void enter_suspend_mode(void) i2c_write8(BNO055_PWR_MODE_ADDR, 0x02); } -static void disable_motion_interrupts(void) -{ - // Disable all interrupts - i2c_write8(BNO055_PAGE_ID_ADDR, 1); - i2c_write8(ACC_INT_SET, 0); - i2c_write8(INT_MSK, 0); - i2c_write8(INT_EN, 0); - i2c_write8(BNO055_PAGE_ID_ADDR, 0); -} - static void enable_motion_interrupts(void) { // Set up interrupts for motion and non-motion events @@ -224,16 +194,28 @@ static void enable_motion_interrupts(void) 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(INT_MSK, ACC_NM | ACC_AM); + i2c_write8(INT_EN, ACC_NM | ACC_AM); + i2c_write8(BNO055_PAGE_ID_ADDR, 0); +} + +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); + 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); } static void enable_data_ready_interrupts(void) { i2c_write8(BNO055_PAGE_ID_ADDR, 1); - 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); + 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); @@ -249,7 +231,9 @@ static void disable_data_ready_interrupts(void) i2c_write8(BNO055_PAGE_ID_ADDR, 0); } -// Math helper functions ----------------------------------------------------------------------------------------------- + +// Math Helper Functions ----------------------------------------------------------------------------------------------- + void quaternion_to_euler(bno055_quaternion_t quaternion, bno055_euler_t *euler) { int32_t sqw = quaternion.w * quaternion.w; @@ -262,6 +246,7 @@ void quaternion_to_euler(bno055_quaternion_t quaternion, bno055_euler_t *euler) euler->roll = atan2(2.0*(quaternion.y*quaternion.z + quaternion.x*quaternion.w),(-sqx - sqy + sqz + sqw)); } + // Public API Functions ------------------------------------------------------------------------------------------------ void imu_init(void) @@ -272,18 +257,14 @@ void imu_init(void) data_ready_callback = NULL; // Create an I2C configuration structure + static uint32_t command_queue_buffer[128]; const am_hal_iom_config_t i2c_config = { .eInterfaceMode = AM_HAL_IOM_I2C_MODE, .ui32ClockFreq = AM_HAL_IOM_100KHZ, .eSpiMode = 0, -#if NONBLOCKING - .pNBTxnBuf = &imuBuffer[0], - .ui32NBTxnBufLength = sizeof(imuBuffer) / 4 -#else - .pNBTxnBuf = NULL, - .ui32NBTxnBufLength = 0 -#endif + .pNBTxnBuf = command_queue_buffer, + .ui32NBTxnBufLength = sizeof(command_queue_buffer) / sizeof(uint32_t) }; // Configure and assert the RESET pin @@ -325,12 +306,9 @@ void imu_init(void) configASSERT0(am_hal_gpio_interrupt_control(AM_HAL_GPIO_INT_CHANNEL_0, AM_HAL_GPIO_INT_CTRL_INDV_ENABLE, &imu_interrupt_pin)); configASSERT0(am_hal_gpio_interrupt_register(AM_HAL_GPIO_INT_CHANNEL_0, PIN_IMU_INTERRUPT, imu_isr, NULL)); NVIC_SetPriority(GPIO0_001F_IRQn + GPIO_NUM2IDX(PIN_IMU_INTERRUPT), NVIC_configKERNEL_INTERRUPT_PRIORITY - 1); + NVIC_SetPriority(IOMSTR0_IRQn + IMU_I2C_NUMBER, NVIC_configKERNEL_INTERRUPT_PRIORITY - 2); NVIC_EnableIRQ(GPIO0_001F_IRQn + GPIO_NUM2IDX(PIN_IMU_INTERRUPT)); - -#if NONBLOCKING - NVIC_SetPriority(IMU_IOM_IRQn, NVIC_configKERNEL_INTERRUPT_PRIORITY - 2); - NVIC_EnableIRQ(IMU_IOM_IRQn); -#endif + NVIC_EnableIRQ(IOMSTR0_IRQn + IMU_I2C_NUMBER); } void imu_deinit(void) @@ -382,7 +360,7 @@ void imu_set_power_mode(bno055_powermode_t power_mode) void imu_read_accel_data(bno055_acc_t *acc) { static int16_t accel_data[3]; - read_int16_vector(BNO055_ACCEL_DATA_X_LSB_ADDR, accel_data, sizeof(accel_data)); + i2c_read(BNO055_ACCEL_DATA_X_LSB_ADDR, (uint8_t*)accel_data, sizeof(accel_data), false); acc->x = accel_data[0]; acc->y = accel_data[1]; acc->z = accel_data[2]; @@ -391,7 +369,7 @@ void imu_read_accel_data(bno055_acc_t *acc) void imu_read_linear_accel_data(bno055_acc_t *acc) { static int16_t accel_data[3]; - read_int16_vector(BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, accel_data, sizeof(accel_data)); + i2c_read(BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, (uint8_t*)accel_data, sizeof(accel_data), false); acc->x = accel_data[0]; acc->y = accel_data[1]; acc->z = accel_data[2]; @@ -400,7 +378,7 @@ void imu_read_linear_accel_data(bno055_acc_t *acc) void imu_read_gravity_accel_data(bno055_acc_t *acc) { static int16_t accel_data[3]; - read_int16_vector(BNO055_GRAVITY_DATA_X_LSB_ADDR, accel_data, sizeof(accel_data)); + i2c_read(BNO055_GRAVITY_DATA_X_LSB_ADDR, (uint8_t*)accel_data, sizeof(accel_data), false); acc->x = accel_data[0]; acc->y = accel_data[1]; acc->z = accel_data[2]; @@ -409,7 +387,7 @@ void imu_read_gravity_accel_data(bno055_acc_t *acc) void imu_read_euler_data(bno055_euler_t *euler) { static int16_t euler_data[3]; - read_int16_vector(BNO055_EULER_H_LSB_ADDR, euler_data, sizeof(euler_data)); + i2c_read(BNO055_EULER_H_LSB_ADDR, (uint8_t*)euler_data, sizeof(euler_data), false); euler->yaw = (euler_data[0]/16.0); euler->roll = (euler_data[1]/16.0); euler->pitch = (euler_data[2]/16.0); @@ -418,16 +396,14 @@ void imu_read_euler_data(bno055_euler_t *euler) void imu_read_quaternion_data(bno055_quaternion_t *quaternion) { static int16_t quaternion_data[4]; - read_int16_vector(BNO055_QUATERNION_DATA_W_LSB_ADDR, quaternion_data, sizeof(quaternion_data)); + i2c_read(BNO055_QUATERNION_DATA_W_LSB_ADDR, (uint8_t*)quaternion_data, sizeof(quaternion_data), false); //temporary fix of MSB sign bit flipping problem for (uint8_t i = 0; i < 4; i++) { - if (quaternion_data[i]>16384){ - quaternion_data[i] = quaternion_data[i] - 32768; - } - else if (quaternion_data[i]<-16384){ - quaternion_data[i] = quaternion_data[i] + 32768; - } + if (quaternion_data[i] > 16384) + quaternion_data[i] -= 32768; + else if (quaternion_data[i] < -16384) + quaternion_data[i] += 32768; } quaternion->w = quaternion_data[0]; quaternion->x = quaternion_data[1]; @@ -438,7 +414,7 @@ void imu_read_quaternion_data(bno055_quaternion_t *quaternion) void imu_read_gyro_data(bno055_gyro_t *gyro) { static int16_t gyro_data[3]; - read_int16_vector(BNO055_GYRO_DATA_X_LSB_ADDR, gyro_data, sizeof(gyro_data)); + i2c_read(BNO055_GYRO_DATA_X_LSB_ADDR, (uint8_t*)gyro_data, sizeof(gyro_data), false); gyro->x = gyro_data[0]; gyro->y = gyro_data[1]; gyro->z = gyro_data[2]; @@ -447,7 +423,7 @@ void imu_read_gyro_data(bno055_gyro_t *gyro) void imu_read_temp(int8_t *temp) { static int8_t temp_data; - i2c_read(BNO055_TEMP_ADDR, (uint8_t*)&temp_data, 1); + i2c_read(BNO055_TEMP_ADDR, (uint8_t*)&temp_data, 1, false); *temp = (int8_t)temp_data; } @@ -474,7 +450,7 @@ void imu_read_calibration_offsets(bno055_calib_offsets_t *offsets) //calibration values are only availble in config mode imu_set_op_mode(OPERATION_MODE_CONFIG); //read the 11 offset values - read_int16_vector(ACCEL_OFFSET_X_LSB_ADDR, calib_data, sizeof(calib_data)); + i2c_read(ACCEL_OFFSET_X_LSB_ADDR, (uint8_t*)calib_data, sizeof(calib_data), false); //revert to the previous mode imu_set_op_mode(saved_mode); @@ -521,56 +497,11 @@ bool imu_set_axis_remap(bno055_axis_remap_t remap) //test whether the set is successful bno055_axis_remap_t remap_verification = {0}; imu_read_axis_remap(&remap_verification); - if ((remap_verification.x_remap_val == remap.x_remap_val) && (remap_verification.y_remap_val == remap.y_remap_val) && (remap_verification.z_remap_val == remap.z_remap_val) && - (remap_verification.x_remap_sign == remap.x_remap_sign) && (remap_verification.y_remap_sign == remap.y_remap_sign) && (remap_verification.z_remap_sign == remap.z_remap_sign)) - { - return 1; - } - return 0; + return ((remap_verification.x_remap_val == remap.x_remap_val) && (remap_verification.y_remap_val == remap.y_remap_val) && (remap_verification.z_remap_val == remap.z_remap_val) && + (remap_verification.x_remap_sign == remap.x_remap_sign) && (remap_verification.y_remap_sign == remap.y_remap_sign) && (remap_verification.z_remap_sign == remap.z_remap_sign)); } bool imu_read_in_motion(void) { return previously_in_motion; } - -uint8_t imu_pick_data_from_burst_buffer(uint8_t *picked, uint8_t *full, bno055_data_type_t data_type) -{ - //pick data from the continuous read data, and copy it at the start of the picked buffer - if (data_type == GYRO_DATA) - { - memcpy(picked, full+BNO055_GYRO_DATA_X_LSB_ADDR-BURST_READ_BASE_ADDR, GYRO_DATA_LEN); - return GYRO_DATA_LEN; - } - else if (data_type == LACC_DATA) - { - memcpy(picked, full+BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR-BURST_READ_BASE_ADDR, LACC_DATA_LEN); - return LACC_DATA_LEN; - } - else if (data_type == GACC_DATA) - { - memcpy(picked, full+BNO055_GRAVITY_DATA_X_LSB_ADDR-BURST_READ_BASE_ADDR, LACC_DATA_LEN); - return GACC_DATA_LEN; - } - else if (data_type == QUAT_DATA) - { - memcpy(picked, full+BNO055_QUATERNION_DATA_W_LSB_ADDR-BURST_READ_BASE_ADDR, QUAT_DATA_LEN); - return QUAT_DATA_LEN; - } - else if (data_type == STAT_DATA) - { - memcpy(picked, full+BNO055_CALIB_STAT_ADDR-BURST_READ_BASE_ADDR, STAT_DATA_LEN); - return STAT_DATA_LEN; - } - return 0; -} - -#if NONBLOCKING -void imu_read_burst_buffer(uint8_t *destBuffer) -{ - AM_CRITICAL_BEGIN - memcpy(destBuffer, gReadBuffer, BURST_READ_LEN); - memset(gReadBuffer, 0, BURST_READ_LEN); - AM_CRITICAL_END -} -#endif diff --git a/software/firmware/src/tasks/app_task_ranging.c b/software/firmware/src/tasks/app_task_ranging.c index e2fe4dcd..0ca02500 100644 --- a/software/firmware/src/tasks/app_task_ranging.c +++ b/software/firmware/src/tasks/app_task_ranging.c @@ -218,30 +218,20 @@ static void motion_change_handler(bool in_motion) app_notify(APP_NOTIFY_MOTION_EVENT, true); } -static void imu_burst_data_handler(uint8_t *burst_data_buffer) +#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) { //TODO #ifdef _LIVE_IMU_DATA - bluetooth_write_imu_data(burst_data_buffer, BURST_READ_LEN); + bluetooth_write_imu_data(raw_data, raw_data_length); #endif - uint8_t useful_imu_data[BURST_READ_LEN] = {0}; - - //types of imu data to be saved - const bno055_data_type_t data_types[] = {STAT_DATA,LACC_DATA,GYRO_DATA}; - uint8_t index = 0; - uint8_t len = 0; - + // Store relevant IMU data #ifndef _TEST_NO_STORAGE - for (uint8_t i = 0; i < sizeof(data_types)/sizeof(data_types[0]); i+=1) - { - len = imu_pick_data_from_burst_buffer(useful_imu_data+index, burst_data_buffer, data_types[i]); - index+= len; - } - storage_write_imu_data(app_get_experiment_time(0), useful_imu_data, index); - //storage_write_imu_data(app_get_experiment_time(0), burst_data_buffer, 38); + storage_write_imu_data(linear_accel_data); #endif } +#endif static void ble_discovery_handler(const uint8_t ble_address[EUI_LEN], uint8_t ranging_role) { @@ -383,8 +373,8 @@ void AppTaskRanging(void *uid) #endif #ifdef _TEST_IMU_DATA - //imu_register_motion_change_callback(motion_change_handler); - imu_register_data_ready_callback(imu_burst_data_handler); + imu_register_motion_change_callback(motion_change_handler); + imu_register_data_ready_callback(data_ready_handler); imu_set_power_mode(POWER_MODE_NORMAL); //imu_set_power_mode(POWER_MODE_LOWPOWER); imu_set_fusion_mode(OPERATION_MODE_NDOF); diff --git a/software/firmware/src/tasks/app_tasks.h b/software/firmware/src/tasks/app_tasks.h index 0ffe54ad..93295e53 100644 --- a/software/firmware/src/tasks/app_tasks.h +++ b/software/firmware/src/tasks/app_tasks.h @@ -49,7 +49,7 @@ 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(uint32_t timestamp, const uint8_t *imu_data, uint32_t imu_data_len); +void storage_write_imu_data(const int16_t *accel_data); // 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 da4c567a..36fbbb62 100644 --- a/software/firmware/src/tasks/storage_task.c +++ b/software/firmware/src/tasks/storage_task.c @@ -8,14 +8,14 @@ // Storage Task and Notification Types --------------------------------------------------------------------------------- typedef struct storage_item_t { uint32_t timestamp, value; uint8_t type; } storage_item_t; -typedef struct ranging_data_t { uint8_t data[MAX_COMPRESSED_RANGE_DATA_LENGTH]; uint32_t length; } ranging_data_t; typedef struct imu_data_t { uint8_t data[MAX_IMU_DATA_LENGTH]; uint32_t length; } imu_data_t; +typedef struct ranging_data_t { uint8_t data[MAX_COMPRESSED_RANGE_DATA_LENGTH]; uint32_t length; } ranging_data_t; // Static Global Variables --------------------------------------------------------------------------------------------- +static imu_data_t imu_data[STORAGE_QUEUE_MAX_NUM_ITEMS]; static ranging_data_t range_data[STORAGE_QUEUE_MAX_NUM_ITEMS]; -static imu_data_t imu_buffer[STORAGE_IMU_BUFFER_NUM_ITEMS]; static uint8_t ucQueueStorage[STORAGE_QUEUE_MAX_NUM_ITEMS * sizeof(storage_item_t)]; static int32_t ranging_timestamp_offset; static StaticQueue_t xQueueBuffer; @@ -98,13 +98,14 @@ void storage_write_ranging_data(uint32_t timestamp, const uint8_t *ranging_data, xQueueSendToBack(storage_queue, &storage_item, 0); } -void storage_write_imu_data(uint32_t timestamp, const uint8_t *imu_data, uint32_t imu_data_len) +void storage_write_imu_data(const int16_t *accel_data) { static uint32_t imu_data_index = 0; - const storage_item_t storage_item = { .timestamp = timestamp, .value = imu_data_index, .type = STORAGE_TYPE_IMU}; - memcpy(imu_buffer[imu_data_index].data, imu_data, imu_data_len); - imu_buffer[imu_data_index].length = imu_data_len; - imu_data_index = (imu_data_index + 1) % STORAGE_IMU_BUFFER_NUM_ITEMS; + 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); + imu_data_index = (imu_data_index + 1) % STORAGE_QUEUE_MAX_NUM_ITEMS; xQueueSendToBack(storage_queue, &storage_item, 0); } @@ -153,7 +154,7 @@ void StorageTask(void *params) store_ranges(item.timestamp, range_data[item.value].data, range_data[item.value].length); break; case STORAGE_TYPE_IMU: - store_imu_data(item.timestamp, imu_buffer[item.value].data, imu_buffer[item.value].length); + store_imu_data(item.timestamp, imu_data[item.value].data, imu_data[item.value].length); break; default: break; diff --git a/software/firmware/tests/Makefile b/software/firmware/tests/Makefile index c568b347..773c71e1 100644 --- a/software/firmware/tests/Makefile +++ b/software/firmware/tests/Makefile @@ -8,8 +8,6 @@ else REVISION := M endif -NONBLOCKING ?= 0 - ifeq ($(REVISION), I) BSP = apollo4_blue PART = apollo4b @@ -282,7 +280,7 @@ ble_reset: $(CONFIG) $(CONFIG)/test_ble_reset.o $(CONFIG)/$$(TARGET).bin program ble_range_imu: TARGET = TestBleRangingIMU ble_range_imu: SRC += test_ble_range_imu.c -ble_range_imu: CFLAGS += -D_TEST_NO_STORAGE -D_TEST_NO_EXP_DETAILS -D_TEST_NO_BATTERY_CALLBACK -DNONBLOCKING=1 -D_TEST_IMU_DATA -D__USE_FREERTOS__ -D_LIVE_IMU_DATA +ble_range_imu: CFLAGS += -D_TEST_NO_STORAGE -D_TEST_NO_EXP_DETAILS -D_TEST_NO_BATTERY_CALLBACK -D_TEST_IMU_DATA -D__USE_FREERTOS__ -D_LIVE_IMU_DATA ble_range_imu: $(CONFIG) $(CONFIG)/test_ble_range_imu.o $(CONFIG)/$$(TARGET).bin program bluetooth: TARGET = TestBluetooth @@ -310,12 +308,11 @@ full_segger: $(CONFIG) $(CONFIG)/main.o $(CONFIG)/SEGGER_RTT.o $(CONFIG)/$$(TARG full_exp: TARGET = TestFullExp full_exp: SRC += main.c SEGGER_RTT.c -full_exp: CFLAGS += -D__USE_FREERTOS__ -D__USE_SEGGER__ -DNONBLOCKING=1 -D_TEST_IMU_DATA -D_REMOTE_MODE_SWITCH_ENABLED -D_TEST_NO_EXP_DETAILS -D_USE_DEFAULT_EXP_DETAILS -D_LIVE_IMU_DATA +full_exp: CFLAGS += -D__USE_FREERTOS__ -D__USE_SEGGER__ -D_TEST_IMU_DATA -D_REMOTE_MODE_SWITCH_ENABLED -D_TEST_NO_EXP_DETAILS -D_USE_DEFAULT_EXP_DETAILS -D_LIVE_IMU_DATA full_exp: $(CONFIG) $(CONFIG)/main.o $(CONFIG)/SEGGER_RTT.o $(CONFIG)/$$(TARGET).bin program imu: TARGET = TestIMU imu: SRC += test_imu.c -imu: CFLAGS += -DNONBLOCKING=$(NONBLOCKING) imu: $(CONFIG) $(CONFIG)/test_imu.o $(CONFIG)/$$(TARGET).bin program led: TARGET = TestLED diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c index 24e56eeb..89fe5309 100644 --- a/software/firmware/tests/peripherals/test_imu.c +++ b/software/firmware/tests/peripherals/test_imu.c @@ -2,75 +2,19 @@ #include "logging.h" #include "system.h" -static void motion_interrupt(bool in_motion) +static void motion_changed(bool in_motion) { print("Device is %s\n", in_motion ? "IN MOTION" : "STATIONARY"); } -static void read_data(uint8_t interrupt_status) +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) { - const bool acc_fusion_data_ready = interrupt_status & 0x01;//ACC_BSX_DRDY - //const bool mag_data_ready = interrupt_status & 0x02;//MAG_DRDY - //const bool gyro_data_ready = interrupt_status & 0x10;//GYR_DRDY - if (acc_fusion_data_ready) - { - //int8_t temp; - bno55_calib_status_t status = {0}; - //bno055_calib_offsets_t offsets = {0}; - //bno055_axis_remap_t remap = {0}; - bno055_quaternion_t quaternion = {0}; - bno055_euler_t euler = {0}; - //bno055_euler_t device_calculated_euler = {0}; - bno055_acc_t acc = {0}; - bno055_gyro_t gyro = {0}; - imu_read_calibration_status(&status); - //imu_read_calibration_offsets(&offsets); - imu_read_linear_accel_data(&acc); - imu_read_quaternion_data(&quaternion); - quaternion_to_euler(quaternion, &euler); - imu_read_gyro_data(&gyro); - //imu_read_temp(&temp); - //print("temp:%d\n", (int32_t)temp); - print("Calibration status: sys %u, gyro %u, accel %u, mag %u\n",status.sys, status.gyro, status.accel, status.mag); - //print("Calibration offsets: %d, %d, %d \n", offsets.gyro_offset_x, offsets.gyro_offset_y, offsets.gyro_offset_z); - print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d, gx = %d, gy = %d, gz = %d\n", acc.x, acc.y, acc.z, quaternion.w, quaternion.x, quaternion.y, quaternion.z, gyro.x, gyro.y,gyro.z); - } -} - -static void handle_burst_data(uint8_t *localBuffer){ - int16_t read_buffer[BURST_READ_LEN/2] = {0}; - - for (uint32_t i = 0; i < BURST_READ_LEN/2; i++) - { - read_buffer[i] = ((int16_t)localBuffer[i*2]) | (((int16_t)localBuffer[i*2+1]) << 8); - } - - uint8_t reg_value = localBuffer[BNO055_CALIB_STAT_ADDR-BURST_READ_BASE_ADDR]; - bno55_calib_status_t status = {0}; - status.mag = reg_value & 0x03; - status.accel = (reg_value >> 2) & 0x03; - status.gyro = (reg_value >> 4) & 0x03; - status.sys = (reg_value >> 6) & 0x03; - print("Calibration status: sys %u, gyro %u, accel %u, mag %u\n",status.sys, status.gyro, status.accel, status.mag); - - int16_t quaternion_data[4]; - memcpy(quaternion_data, read_buffer + (BNO055_QUATERNION_DATA_W_LSB_ADDR-BURST_READ_BASE_ADDR)/2, 4 * sizeof(int16_t)); - //temporary fix of MSB sign bit flipping problem - for (uint8_t i = 0; i < 4; i++) - { - if (quaternion_data[i]>16384){ - quaternion_data[i] = quaternion_data[i] - 32768; - } - else if (quaternion_data[i]<-16384){ - quaternion_data[i] = quaternion_data[i] + 32768; - } - } - bno055_acc_t lacc = {0}; - memcpy(&lacc, read_buffer + (BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR-BURST_READ_BASE_ADDR)/2, 3 * sizeof(int16_t)); - bno055_gyro_t gyro = {0}; - memcpy(&gyro, read_buffer + (BNO055_GYRO_DATA_X_LSB_ADDR-BURST_READ_BASE_ADDR)/2, 3 * sizeof(int16_t)); - - print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d, gx = %d, gy = %d, gz = %d\n",lacc.x,lacc.y,lacc.z,quaternion_data[0],quaternion_data[1],quaternion_data[2],quaternion_data[3],gyro.x,gyro.y,gyro.z); + // Print out calibration status + 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]); } int main(void) @@ -84,16 +28,11 @@ int main(void) //bno055_axis_remap_t remap = {.x_remap_val = 1, .y_remap_val = 0, .z_remap_val = 2}; //bno055_axis_remap_t remap = {0}; imu_read_fw_version(&rev_msb, &rev_lsb); - print("BNO055 firmware version:%x.%x\n",rev_msb, rev_lsb); + print("BNO055 firmware version: %x.%x\n", rev_msb, rev_lsb); - imu_register_motion_change_callback(motion_interrupt); + imu_register_motion_change_callback(motion_changed); + imu_register_data_ready_callback(data_ready); -#if NONBLOCKING - imu_register_data_ready_callback(handle_burst_data); -#else - imu_register_data_ready_callback(read_data); -#endif - //imu_register_data_ready_callback(NULL); imu_set_power_mode(POWER_MODE_NORMAL); imu_set_fusion_mode(OPERATION_MODE_NDOF); @@ -102,9 +41,7 @@ int main(void) //if (imu_set_axis_remap(remap)){print("remap success!\n");} //print("BNO055 X mapping:%u, Y mapping:%u, Z mapping:%u, X sign:%u, Y sign:%u, Z sign:%u\n", remap.x_remap_val, remap.y_remap_val, remap.z_remap_val, remap.x_remap_sign, remap.y_remap_sign, remap.z_remap_sign); - while (true){ - ; - } + while (true); // Should never reach this point return 0;