add functionality to convert the BNO055 quaternion to Tait-Bryan angles

software/firmware/src/peripherals/include/imu.h

@@ -244,6 +244,13 @@ typedef struct
    int16_t z;
 } bno055_quaternion_t;
 
+typedef struct
+{
+   double yaw;
+   double pitch;
+   double roll;
+}bno055_euler_t;
+
 typedef struct
 {
    int16_t x;
@@ -269,4 +276,7 @@ void imu_read_calibration_offsets(bno055_calib_offsets_t *offsets);
 void imu_read_axis_remap(bno055_axis_remap_t *remap);
 bool imu_read_in_motion(void);
 
+// Math utilities
+void quaternion_to_euler(bno055_quaternion_t quaternion, bno055_euler_t *euler);
+
 #endif  // #ifndef __IMU_HEADER_H__

software/firmware/src/peripherals/src/imu.c

@@ -1,6 +1,7 @@
 // Header Inclusions ---------------------------------------------------------------------------------------------------
 
 #include "imu.h"
+#include "math.h"
 
 
 // Static Global Variables ---------------------------------------------------------------------------------------------
@@ -87,6 +88,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);
    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);
@@ -145,6 +147,19 @@ static void enable_motion_interrupts(void)
    i2c_write8(BNO055_PAGE_ID_ADDR, 0);
 }
 
+// Math helper functions -----------------------------------------------------------------------------------------------
+void quaternion_to_euler(const bno055_quaternion_t quaternion, bno055_euler_t *euler)
+{
+   int32_t sqw = quaternion.w * quaternion.w;
+   int32_t sqx = quaternion.x * quaternion.x;
+   int32_t sqy = quaternion.y * quaternion.y;
+   int32_t sqz = quaternion.z * quaternion.z;
+
+   euler->yaw = atan2(2.0*(quaternion.x*quaternion.y + quaternion.z*quaternion.w),(sqx - sqy - sqz + sqw));
+   euler->pitch = asin((double)(-2*(quaternion.x*quaternion.z - quaternion.y*quaternion.w))/(double)(sqx + sqy + sqz + sqw));
+   euler->roll = atan2(2.0*(quaternion.y*quaternion.z + quaternion.x*quaternion.w),(-sqx - sqy + sqz + sqw));
+}
+
 // Public API Functions ------------------------------------------------------------------------------------------------
 
 void imu_init(void)

software/firmware/tests/peripherals/test_imu.c

@@ -2,6 +2,7 @@
 #include "logging.h"
 #include "system.h"
 
+#define RAD_TO_DEG(radian) (radian * 180.0 / 3.14159265358979f)
 static void motion_interrupt(bool in_motion)
 {
    print("Device is %s\n", in_motion ? "IN MOTION" : "STATIONARY");
@@ -18,11 +19,12 @@ int main(void)
    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;
-   bno055_acc_t acc;
+   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_acc_t acc = {0};
 
    imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF);
 
@@ -34,24 +36,26 @@ int main(void)
 
    while (true)
    {
-      am_hal_delay_us(20000);
+      //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_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(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);
       imu_read_linear_accel_data(&acc);
       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)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);
+      quaternion_to_euler(quaternion, &euler);
+      print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d, yaw = %lf, pitch = %lf, 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));
       //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);
       //print("gyro 1 = %d, gyro 2 = %d, gyro 3 = %d\n", (int32_t)x, (int32_t)y, (int32_t)z);
       //imu_read_temp(&temp);
       //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_offsets(&offsets);
-      //print("Calibration offsets: %d, %d, %d \n", offsets.gyro_offset_x, offsets.gyro_offset_y, offsets.gyro_offset_z);
+
 
    }