Skip to content

Commit

Permalink
Add raw_data IMU handling for _TEST_IMU_DATA
Browse files Browse the repository at this point in the history
  • Loading branch information
@hedgecrw
hedgecrw committed Jul 18, 2024
1 parent 8939599 commit 2e2e690
Show file tree
Hide file tree
Showing 6 changed files with 132 additions and 64 deletions.
16 changes: 14 additions & 2 deletions software/firmware/src/peripherals/include/imu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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);
Expand Down
123 changes: 75 additions & 48 deletions software/firmware/src/peripherals/src/imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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);
}

Expand All @@ -174,61 +169,61 @@ 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);
}

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


Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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
Expand All @@ -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;
}
8 changes: 6 additions & 2 deletions software/firmware/src/tasks/app_task_ranging.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
6 changes: 5 additions & 1 deletion software/firmware/src/tasks/app_tasks.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
24 changes: 19 additions & 5 deletions software/firmware/src/tasks/storage_task.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
19 changes: 13 additions & 6 deletions software/firmware/tests/peripherals/test_imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit 2e2e690

Please sign in to comment.