From 235919c8862372256be24eee27dc52fff5c6a1ce Mon Sep 17 00:00:00 2001 From: corruptbear Date: Tue, 14 Nov 2023 14:51:32 -0800 Subject: [PATCH] Use structure for quaternion values --- software/firmware/src/peripherals/include/imu.h | 9 ++++++++- software/firmware/src/peripherals/src/imu.c | 12 ++++++------ software/firmware/tests/peripherals/test_imu.c | 17 +++++++++-------- 3 files changed, 23 insertions(+), 15 deletions(-) diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h index e4815990..82279c26 100644 --- a/software/firmware/src/peripherals/include/imu.h +++ b/software/firmware/src/peripherals/include/imu.h @@ -223,6 +223,13 @@ typedef struct { uint8_t y_remap_sign; uint8_t z_remap_sign; }bno055_axis_remap_t; + +typedef struct { + int16_t w; + int16_t x; + int16_t y; + int16_t z; +} bno055_quaternion_t; // Peripheral Type Definitions ----------------------------------------------------------------------------------------- typedef void (*motion_change_callback_t)(bool in_motion); @@ -236,7 +243,7 @@ void imu_register_motion_change_callback(motion_change_callback_t callback, bno0 void imu_read_accel_data(int16_t *x, int16_t *y, int16_t *z); void imu_read_linear_accel_data(int16_t *x, int16_t *y, int16_t *z); void imu_read_gravity_accel_data(int16_t *x, int16_t *y, int16_t *z); -void imu_read_quaternion_data(int16_t *w, int16_t *x, int16_t *y, int16_t *z); +void imu_read_quaternion_data(bno055_quaternion_t *quaternion); void imu_read_gyro_data(int16_t *x, int16_t *y, int16_t *z); void imu_read_temp(int8_t *temp); void imu_read_fw_version(uint8_t *msb, uint8_t *lsb); diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c index 7e48366d..028e2f5b 100644 --- a/software/firmware/src/peripherals/src/imu.c +++ b/software/firmware/src/peripherals/src/imu.c @@ -259,13 +259,13 @@ void imu_read_gravity_accel_data(int16_t *x, int16_t *y, int16_t *z){ *z = (int16_t)(accel_data[2] << 2) / 4; } -void imu_read_quaternion_data(int16_t *w, int16_t *x, int16_t *y, int16_t *z){ +void imu_read_quaternion_data(bno055_quaternion_t *quaternion){ static int16_t quaternion_data[4]; - i2c_read(BNO055_QUATERNION_DATA_W_LSB_ADDR, (uint8_t*)quaternion_data, sizeof(quaternion_data)); - *w = (int16_t)(quaternion_data[0] << 2) / 4; - *x = (int16_t)(quaternion_data[1] << 2) / 4; - *y = (int16_t)(quaternion_data[2] << 2) / 4; - *z = (int16_t)(quaternion_data[3] << 2) / 4; + read_int16_vector(BNO055_QUATERNION_DATA_W_LSB_ADDR, quaternion_data, sizeof(quaternion_data)); + quaternion->w = quaternion_data[0]; + quaternion->x = quaternion_data[1]; + quaternion->y = quaternion_data[2]; + quaternion->z = quaternion_data[3]; } void imu_read_gyro_data(int16_t *x, int16_t *y, int16_t *z){ diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c index ff71e96a..3e017f5c 100644 --- a/software/firmware/tests/peripherals/test_imu.c +++ b/software/firmware/tests/peripherals/test_imu.c @@ -15,12 +15,13 @@ int main(void) system_enable_interrupts(true); // Loop forever, waiting for IMU interrupts - int16_t w, x, y, z; + int16_t x, y, z; int8_t temp; uint8_t rev_msb, rev_lsb; bno55_calib_status_t status; bno055_calib_offsets_t offsets; bno055_axis_remap_t remap; + bno055_quaternion_t quaternion; imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF); @@ -32,14 +33,14 @@ int main(void) while (true) { - am_hal_delay_us(1000000); - imu_read_accel_data(&x, &y, &z); - print("Accel X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z); + am_hal_delay_us(20000); + //imu_read_accel_data(&x, &y, &z); + //print("Accel X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z); imu_read_linear_accel_data(&x, &y, &z); - print("Linear Accel X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z); + imu_read_quaternion_data(&quaternion); + print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d\n", (int32_t)x, (int32_t)y, (int32_t)z, (int32_t)quaternion.w, (int32_t)quaternion.x, (int32_t)quaternion.y, (int32_t)quaternion.z); //imu_read_gravity_accel_data(&x, &y, &z); //print("Gravity Accel X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z); - //imu_read_quaternion_data(&w, &x, &y, &z); //print("Quaternion W = %d, X = %d, Y = %d, Z = %d\n", (int32_t)w, (int32_t)x, (int32_t)y, (int32_t)z); //imu_read_gyro_data(&x, &y, &z); //print("gyro 1 = %d, gyro 2 = %d, gyro 3 = %d\n", (int32_t)x, (int32_t)y, (int32_t)z); @@ -47,8 +48,8 @@ int main(void) //print("temp:%d\n", (int32_t)temp); //0: not calibrated; 3: fully calibrated - imu_read_calibration_status(&status); - print("Calibration status: sys %u, gyro %u, accel %u, mag %u\n",status.sys, status.gyro, status.accel, status.mag); + //imu_read_calibration_status(&status); + //print("Calibration status: sys %u, gyro %u, accel %u, mag %u\n",status.sys, status.gyro, status.accel, status.mag); //imu_read_calibration_offsets(&offsets); //print("Calibration offsets: %d, %d, %d \n", offsets.gyro_offset_x, offsets.gyro_offset_y, offsets.gyro_offset_z);