Read official euler from registers

lab11 · Nov 18, 2023 · 62f1b15 · 62f1b15
1 parent ad280e4
commit 62f1b15
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 3 deletions.
diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h
@@ -1,6 +1,7 @@
 #ifndef __IMU_HEADER_H__
 #define __IMU_HEADER_H__
 
+#define RAD_TO_DEG(radian) (radian * 180.0 / 3.14159265358979f)
 // Header Inclusions ---------------------------------------------------------------------------------------------------
 
 #include "app_config.h"
@@ -276,6 +277,7 @@ void imu_read_calibration_status(bno55_calib_status_t *status);
 void imu_read_calibration_offsets(bno055_calib_offsets_t *offsets);
 void imu_read_axis_remap(bno055_axis_remap_t *remap);
 bool imu_set_axis_remap(bno055_axis_remap_t remap);
+void imu_read_euler_data(bno055_euler_t *euler);
 bool imu_read_in_motion(void);
 
 // Math utilities

diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c
@@ -279,6 +279,15 @@ void imu_read_gravity_accel_data(bno055_acc_t *acc)
    acc->z = accel_data[2];
 }
 
+void imu_read_euler_data(bno055_euler_t *euler)
+{
+   static int16_t euler_data[3];
+   read_int16_vector(BNO055_EULER_H_LSB_ADDR, euler_data, sizeof(euler_data));
+   euler->yaw = (euler_data[0]/16.0);
+   euler->roll = (euler_data[1]/16.0);
+   euler->pitch = (euler_data[2]/16.0);
+}
+
 void imu_read_quaternion_data(bno055_quaternion_t *quaternion)
 {
    static int16_t quaternion_data[4];

diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c
@@ -2,7 +2,6 @@
 #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");
@@ -21,9 +20,11 @@ int main(void)
    uint8_t rev_msb, rev_lsb;
    bno55_calib_status_t status = {0};
    bno055_calib_offsets_t offsets = {0};
-   bno055_axis_remap_t remap = {.x_remap_val = 1, .y_remap_val = 0, .z_remap_val = 2};
+   //bno055_axis_remap_t remap = {.x_remap_val = 1, .y_remap_val = 0, .z_remap_val = 2};
+   bno055_axis_remap_t remap = {0};
    bno055_quaternion_t quaternion = {0};
    bno055_euler_t euler = {0};
+   bno055_euler_t device_calculated_euler = {0};
    bno055_acc_t acc = {0};
 
    imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF);
@@ -49,11 +50,13 @@ int main(void)
       memset(&acc, 0, sizeof(acc));
       memset(&quaternion, 0, sizeof(quaternion));
       memset(&euler, 0, sizeof(euler));
+	  memset(&device_calculated_euler, 0 , sizeof(device_calculated_euler));
 
       imu_read_linear_accel_data(&acc);
       imu_read_quaternion_data(&quaternion);
       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_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);
       //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);