Diff: imu.cpp

Revision:

92:00460f6df439

Parent:

89:c9f64bd655d9

Child:

93:b827f78a717a

--- a/imu.cpp	Tue Oct 26 05:37:25 2021 +0000
+++ b/imu.cpp	Thu Oct 28 09:44:47 2021 +0000
@@ -5,11 +5,25 @@
         lsm.readAccel();
         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;
+        
+        float pressure = lps.readPressureMillibars();
+        palt = lps.pressureToAltitudeMeters(pressure);
+        
         //printf("%f %f %f %f %f %f %f %f %f\n", lsm.ax, lsm.ay, lsm.az, lsm.gx, lsm.gy, lsm.gz, lsm.mx, lsm.my, lsm.mz);
         //printf("%f %f %f\n", lsm.gx, lsm.gy, lsm.gz);
         //printf("%f %f %f\n", lsm.mx, lsm.my, lsm.mz);
         //float pressure = lps.readPressureMillibars();
         //float altitude = lps.pressureToAltitudeMeters(pressure);
         //float temperature = lps.readTemperatureC();
-        //printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
+        //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
 }   
\ No newline at end of file