Add functionality to read the imu calibration offsets

lab11 · Nov 14, 2023 · a1278e2 · a1278e2
1 parent b76ea67
commit a1278e2
Show file tree

Hide file tree

Showing 3 changed files with 69 additions and 8 deletions.
diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h
@@ -191,12 +191,29 @@ typedef enum {
 
 typedef struct
 {
-    int sys;
-    int gyro;
-    int accel;
-    int mag;
+    uint8_t sys;
+    uint8_t gyro;
+    uint8_t accel;
+    uint8_t mag;
 } bno55_calib_status_t;
 
+typedef struct {
+  int16_t accel_offset_x;
+  int16_t accel_offset_y;
+  int16_t accel_offset_z;
+
+  int16_t mag_offset_x;
+  int16_t mag_offset_y;
+  int16_t mag_offset_z;
+
+  int16_t gyro_offset_x;
+  int16_t gyro_offset_y;
+  int16_t gyro_offset_z;
+
+  int16_t accel_radius;
+
+  int16_t mag_radius;
+} bno055_calib_offsets_t;
 // Peripheral Type Definitions -----------------------------------------------------------------------------------------
 
 typedef void (*motion_change_callback_t)(bool in_motion);
@@ -215,4 +232,5 @@ 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);
 void imu_read_calibration_status(bno55_calib_status_t *status);
+void imu_read_calibration_offsets(bno055_calib_offsets_t *offsets);
 #endif  // #ifndef __IMU_HEADER_H__
diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c
@@ -90,6 +90,13 @@ static void imu_isr(void *args)
    previously_in_motion = in_motion;
 }
 
+static void read_int16_vector(uint8_t reg_number, int16_t *read_buffer, uint32_t byte_count){
+	static uint8_t byte_array[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);
+    }
+}
 
 // IMU Chip-Specific API Functions -------------------------------------------------------------------------------------
 
@@ -100,6 +107,11 @@ static void set_mode(bno055_opmode_t mode)
    am_util_delay_ms(30);
 }
 
+static bno055_opmode_t get_mode(void)
+{
+   return (bno055_opmode_t)i2c_read8(BNO055_OPR_MODE_ADDR);
+}
+
 static void set_use_external_crystal(void)
 {
    // Set the IMU to use an external crystal clock source
@@ -282,4 +294,30 @@ void imu_read_calibration_status(bno55_calib_status_t *status) {
     status->accel = (reg_value >> 2) & 0x03;
     status->gyro = (reg_value >> 4) & 0x03;
     status->sys = (reg_value >> 6) & 0x03;
+}
+
+void imu_read_calibration_offsets(bno055_calib_offsets_t *offsets){
+    static int16_t calib_data[11];
+    bno055_opmode_t saved_mode = get_mode();
+    //calibration values are only availble in config mode
+    set_mode(OPERATION_MODE_CONFIG);
+    //read the 11 offset values
+    read_int16_vector(ACCEL_OFFSET_X_LSB_ADDR, calib_data, sizeof(calib_data));
+    //revert to the previous mode
+    set_mode(saved_mode);
+
+    offsets->accel_offset_x = calib_data[0];
+    offsets->accel_offset_y = calib_data[1];
+    offsets->accel_offset_z = calib_data[2];
+
+    offsets->mag_offset_x = calib_data[3];
+    offsets->mag_offset_y = calib_data[4];
+    offsets->mag_offset_z = calib_data[5];
+
+    offsets->gyro_offset_x = calib_data[6];
+    offsets->gyro_offset_y = calib_data[7];
+    offsets->gyro_offset_z = calib_data[8];
+
+    offsets->accel_radius = calib_data[9];
+    offsets->mag_radius = calib_data[10];
 }
diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c
@@ -19,21 +19,24 @@ int main(void)
    int8_t temp;
    uint8_t rev_msb, rev_lsb;
    bno55_calib_status_t status;
+   bno055_calib_offsets_t offsets;
+
    imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF);
 
    imu_read_fw_version(&rev_msb, &rev_lsb);
    print("BNO055 firmware version:%u.%u\n",rev_msb, rev_lsb);
+
    while (true)
    {
       am_hal_delay_us(1000000);
       imu_read_accel_data(&x, &y, &z);
-      print("X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)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("X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z);
+      print("Linear Accel X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z);
       imu_read_gravity_accel_data(&x, &y, &z);
-      print("X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)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("W = %d, X = %d, Y = %d, Z = %d\n", (int32_t)w, (int32_t)x, (int32_t)y, (int32_t)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);
       imu_read_temp(&temp);
@@ -42,6 +45,8 @@ int main(void)
       //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);
    }
 
    // Should never reach this point