address the sign bit flip issue in signed 16 bit quaternion readings

lab11 · Nov 21, 2023 · e1f7f9a · e1f7f9a
1 parent 62f1b15
commit e1f7f9a
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 3 deletions.
diff --git 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
@@ -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);