fixed units for WT901

ClemensElflein · Jul 19, 2022 · 0153e94 · 0153e94
1 parent e361f00
commit 0153e94
Showing 1 changed file with 6 additions and 6 deletions.
diff --git a/Firmware/LowLevel/src/imu/WT901/imu.cpp b/Firmware/LowLevel/src/imu/WT901/imu.cpp
@@ -16,13 +16,13 @@ bool init_imu()
 
 bool imu_read(float *acceleration_mss, float *gyro_rads, float *mag_uT)
 {
-    acceleration_mss[0] = (float)IMU.stcAcc.a[0] / 32768 * 16;
-    acceleration_mss[1] = (float)IMU.stcAcc.a[1] / 32768 * 16;
-    acceleration_mss[2] = (float)IMU.stcAcc.a[2] / 32768 * 16;
+    acceleration_mss[0] = (float)IMU.stcAcc.a[0] / 32768.0f * 16.0f * 9.81f;
+    acceleration_mss[1] = (float)IMU.stcAcc.a[1] / 32768.0f * 16.0f * 9.81f;
+    acceleration_mss[2] = (float)IMU.stcAcc.a[2] / 32768.0f * 16.0f * 9.81f;
 
-    gyro_rads[0] = (float)IMU.stcGyro.w[0]/32768.0f*2000.0f;
-    gyro_rads[1] = (float)IMU.stcGyro.w[1]/32768.0f*2000.0f;
-    gyro_rads[2] = (float)IMU.stcGyro.w[2]/32768.0f*2000.0f;
+    gyro_rads[0] = (float)IMU.stcGyro.w[0]/32768.0f*2000.0f * PI / 180.0f;
+    gyro_rads[1] = (float)IMU.stcGyro.w[1]/32768.0f*2000.0f * PI / 180.0f;
+    gyro_rads[2] = (float)IMU.stcGyro.w[2]/32768.0f*2000.0f * PI / 180.0f;
 
     mag_uT[0] = IMU.stcMag.h[0];
     mag_uT[1] = IMU.stcMag.h[1];