From e1f7f9a13a7f16e3ca8896f9e430230ad7edea7b Mon Sep 17 00:00:00 2001 From: corruptbear Date: Mon, 20 Nov 2023 16:38:59 -0800 Subject: [PATCH] address the sign bit flip issue in signed 16 bit quaternion readings --- software/firmware/src/peripherals/src/imu.c | 13 ++++++++++++- software/firmware/tests/peripherals/test_imu.c | 4 ++-- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c index 081bb204..0a9146f3 100644 --- a/software/firmware/src/peripherals/src/imu.c +++ b/software/firmware/src/peripherals/src/imu.c @@ -89,6 +89,7 @@ static void imu_isr(void *args) 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); @@ -148,7 +149,7 @@ static void enable_motion_interrupts(void) } // Math helper functions ----------------------------------------------------------------------------------------------- -void quaternion_to_euler(const bno055_quaternion_t quaternion, bno055_euler_t *euler) +void quaternion_to_euler(bno055_quaternion_t quaternion, bno055_euler_t *euler) { int32_t sqw = quaternion.w * quaternion.w; int32_t sqx = quaternion.x * quaternion.x; @@ -292,6 +293,16 @@ 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)); + //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; + } + } quaternion->w = quaternion_data[0]; quaternion->x = quaternion_data[1]; quaternion->y = quaternion_data[2]; diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c index b57f7ab3..08b5d658 100644 --- a/software/firmware/tests/peripherals/test_imu.c +++ b/software/firmware/tests/peripherals/test_imu.c @@ -44,7 +44,7 @@ int main(void) 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); - am_hal_delay_us(100000); + am_hal_delay_us(40000); //imu_read_accel_data(&acc); //print("Accel X = %d, Y = %d, Z = %d\n", (int32_t)acc.x, (int32_t)acc.y, (int32_t)acc.z); memset(&acc, 0, sizeof(acc)); @@ -56,7 +56,7 @@ int main(void) imu_read_quaternion_data(&quaternion); quaternion_to_euler(quaternion, &euler); imu_read_euler_data(&device_calculated_euler); - print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d, yaw = %lf, pitch = %lf, roll = %lf, d_yaw = %lf, d_pitch = %lf, d_roll = %lf\n", (int32_t)acc.x, (int32_t)acc.y, (int32_t)acc.z, (int32_t)quaternion.w, (int32_t)quaternion.x, (int32_t)quaternion.y, (int32_t)quaternion.z, RAD_TO_DEG(euler.yaw), RAD_TO_DEG(euler.pitch), RAD_TO_DEG(euler.roll), device_calculated_euler.yaw, device_calculated_euler.pitch, device_calculated_euler.roll); + print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d, yaw = %lf, pitch = %lf, roll = %lf, d_yaw = %lf, d_pitch = %lf, d_roll = %lf\n", (int16_t)acc.x, (int16_t)acc.y, (int16_t)acc.z, (int16_t)quaternion.w, (int16_t)quaternion.x, (int16_t)quaternion.y, (int16_t)quaternion.z, RAD_TO_DEG(euler.yaw), RAD_TO_DEG(euler.pitch), RAD_TO_DEG(euler.roll), device_calculated_euler.yaw, device_calculated_euler.pitch, device_calculated_euler.roll); //imu_read_gravity_accel_data(&acc); //print("Gravity Accel X = %d, Y = %d, Z = %d\n", (int32_t)acc.x, (int32_t)acc.y, (int32_t)acc.z); //imu_read_gyro_data(&x, &y, &z);