Merge useful imu functions

lab11 · Nov 14, 2023 · db90806 · db90806
2 parents e884230 + 761df65
commit db90806
Show file tree

Hide file tree

Showing 3 changed files with 64 additions and 4 deletions.
diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h
@@ -158,7 +158,13 @@ typedef enum {
    ACC_AM_THRE = 0x11,
    ACC_INT_SET = 0x12,
    ACC_NM_THRE = 0x15,
-   ACC_NM_SET = 0x16
+   ACC_NM_SET = 0x16,
+
+   //configs
+   BNO055_MAG_CONFIG_ADDR = 0X09,
+   BNO055_GYRO_CONFIG_ADDR =0X0A,
+   BNO055_GYRO_MODE_CONFIG_ADDR = 0X0B,
+
 } bno055_reg_t;
 
 typedef enum {
@@ -194,5 +200,10 @@ void imu_init(void);
 void imu_deinit(void);
 void imu_register_motion_change_callback(motion_change_callback_t callback, bno055_opmode_t mode);
 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_gyro_data(int16_t *x, int16_t *y, int16_t *z);
+void imu_read_temp(int8_t *temp);
 
 #endif  // #ifndef __IMU_HEADER_H__
diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c
@@ -138,7 +138,6 @@ static void enable_motion_interrupts(void)
    i2c_write8(BNO055_PAGE_ID_ADDR, 0);
 }
 
-
 // Public API Functions ------------------------------------------------------------------------------------------------
 
 void imu_init(void)
@@ -230,4 +229,43 @@ void imu_read_accel_data(int16_t *x, int16_t *y, int16_t *z)
    *x = (int16_t)(accel_data[0] << 2) / 4;
    *y = (int16_t)(accel_data[1] << 2) / 4;
    *z = (int16_t)(accel_data[2] << 2) / 4;
+}
+
+void imu_read_linear_accel_data(int16_t *x, int16_t *y, int16_t *z){
+    static int16_t accel_data[3];
+    i2c_read(BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, (uint8_t*)accel_data, sizeof(accel_data));
+    *x = (int16_t)(accel_data[0] << 2) / 4;
+    *y = (int16_t)(accel_data[1] << 2) / 4;
+    *z = (int16_t)(accel_data[2] << 2) / 4;
+}
+
+void imu_read_gravity_accel_data(int16_t *x, int16_t *y, int16_t *z){
+    static int16_t accel_data[3];
+    i2c_read(BNO055_GRAVITY_DATA_X_LSB_ADDR, (uint8_t*)accel_data, sizeof(accel_data));
+    *x = (int16_t)(accel_data[0] << 2) / 4;
+    *y = (int16_t)(accel_data[1] << 2) / 4;
+    *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){
+    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;
+}
+
+void imu_read_gyro_data(int16_t *x, int16_t *y, int16_t *z){
+    static int16_t gyro_data[3];
+    i2c_read(BNO055_GYRO_DATA_X_LSB_ADDR, (uint8_t*)gyro_data, sizeof(gyro_data));
+    *x = (int16_t)(gyro_data[0] << 2) / 4;
+    *y = (int16_t)(gyro_data[1] << 2) / 4;
+    *z = (int16_t)(gyro_data[2] << 2) / 4;
+}
+
+void imu_read_temp(int8_t *temp){
+    static int8_t temp_data;
+    i2c_read(BNO055_TEMP_ADDR, (uint8_t*)&temp_data, 1);
+    *temp = (int8_t)temp_data;
 }
diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c
@@ -15,13 +15,24 @@ int main(void)
    system_enable_interrupts(true);
 
    // Loop forever, waiting for IMU interrupts
-   int16_t x, y, z;
+   int16_t w, x, y, z;
+   int16_t temp;
    imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF);
    while (true)
    {
-      am_hal_delay_us(5000000);
+      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);
+      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);
+      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);
+      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);
+	  imu_read_gyro_data(&x, &y, &z);
+	  print("")?????
+      imu_read_temp(&temp);
+      print("temp:%d\n", (int32_t)temp);
    }
 
    // Should never reach this point