Diff: imu.cpp

Revision:

93:b827f78a717a

Parent:

92:00460f6df439

Child:

98:bdaa6bbfb1d9

--- a/imu.cpp	Thu Oct 28 09:44:47 2021 +0000
+++ b/imu.cpp	Fri Oct 29 13:30:24 2021 +0000
@@ -6,15 +6,20 @@
         lsm.readMag();
         lsm.readGyro();
         
-        acc.x = lsm.ax * 9.8f - agoffset[0];
-        acc.y = lsm.ay * 9.8f - agoffset[1];
-        acc.z = lsm.az * 9.8f - agoffset[2];
-        gyro.x = (lsm.gx * M_PI / 180.0f) - agoffset[3];
-        gyro.y = (lsm.gy * M_PI / 180.0f) - agoffset[4];
-        gyro.z = (lsm.gz * M_PI / 180.0f) - agoffset[5];      
-        mag.x = lsm.mx;
-        mag.y = lsm.my;
-        mag.z = lsm.mz;
+        Matrix accmat(3,1);
+        accmat << lsm.ax * 9.8f - agoffset[0] << lsm.ay * 9.8f - agoffset[1] << lsm.az * 9.8f - agoffset[2];
+        Matrix accAlign = SensorAlignment*accmat;
+        
+        acc.x = accAlign(1,1);
+        acc.y = accAlign(2,1);
+        acc.z = accAlign(3,1);
+        
+        Matrix gyromat(3,1);
+        gyromat << (lsm.gx * M_PI / 180.0f) - agoffset[3] << (lsm.gy * M_PI / 180.0f) - agoffset[4] << (lsm.gz * M_PI / 180.0f) - agoffset[5];
+        Matrix gyroAlign = SensorAlignment*gyromat;
+        gyro.x = gyroAlign(1,1);
+        gyro.y = gyroAlign(2,1);
+        gyro.z = gyroAlign(3,1);
         
         float pressure = lps.readPressureMillibars();
         palt = lps.pressureToAltitudeMeters(pressure);