Diff: imu.cpp

Revision:

140:53dbdb207542

Parent:

139:b378528c05f2

--- a/imu.cpp	Mon Dec 06 08:26:16 2021 +0000
+++ b/imu.cpp	Mon Dec 06 11:37:55 2021 +0000
@@ -1,47 +1,43 @@
-//#include "global.hpp"
-//
-//void getIMUval()
-//{
-//        lsm.readAccel();
-//        lsm.readMag();
-//        lsm.readGyro();
-//        
-//        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 = SensorAlignmentAG*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 = SensorAlignmentAG*gyromat;
-//        gyro.x = gyroAlign(1,1);
-//        gyro.y = gyroAlign(2,1);
-//        gyro.z = gyroAlign(3,1);
-//        
-//        Matrix magraw(3,1);
-//        magraw << lsm.mx <<lsm.my << lsm.mz;
-//        magraw = SensorAlignmentMAG*magraw;
-//        float inputMag[3];
-//        float outputMag[3];
-//        inputMag[0] = magraw(1,1)*1000.0f;
-//        inputMag[1] = magraw(2,1)*1000.0f;
-//        inputMag[2] = magraw(3,1)*1000.0f;
-//        magCalibrator.run(inputMag,outputMag);
-//        mag.x = outputMag[0];
-//        mag.y = outputMag[1];
-//        mag.z = outputMag[2];
-//        //twelite.printf("%f %f %f : %f %f %f\r\n",magraw(1,1),magraw(2,1),magraw(3,1),magmod(1,1),magmod(2,1),magmod(3,1));
-//        
-//        palt = -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);
-//        
-//        //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();
-//        //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
-//}   
\ No newline at end of file
+#include "global.hpp"
+
+void getIMUval()
+{
+        lsm.readAccel();
+        lsm.readMag();
+        lsm.readGyro();
+        
+        Vector3f accmat;
+        accmat << lsm.ax * 9.8f - agoffset[0], lsm.ay * 9.8f - agoffset[1], lsm.az * 9.8f - agoffset[2];
+        Vector3f accAlign = SensorAlignmentAG*accmat;
+        
+        acc = accAlign;
+        
+        Vector3f gyromat;
+        gyromat << (lsm.gx * M_PI_F / 180.0f) - agoffset[3], (lsm.gy * M_PI_F / 180.0f) - agoffset[4], (lsm.gz * M_PI_F / 180.0f) - agoffset[5];
+        Vector3f gyroAlign = SensorAlignmentAG*gyromat;
+        gyro = gyroAlign;
+        
+        Vector3f magraw;
+        magraw << lsm.mx, lsm.my, lsm.mz;
+        magraw = SensorAlignmentMAG*magraw;
+        float inputMag[3];
+        float outputMag[3];
+        inputMag[0] = magraw(0)*1000.0f;
+        inputMag[1] = magraw(1)*1000.0f;
+        inputMag[2] = magraw(2)*1000.0f;
+        magCalibrator.run(inputMag,outputMag);
+        mag(0) = outputMag[0];
+        mag(1) = outputMag[1];
+        mag(2) = outputMag[2];
+        //twelite.printf("%f %f %f : %f %f %f\r\n",magraw(1,1),magraw(2,1),magraw(3,1),magmod(1,1),magmod(2,1),magmod(3,1));
+        
+        palt = -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);
+        
+        //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();
+        //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
+}   
\ No newline at end of file