Diff: imu.cpp

Revision:

139:b378528c05f2

Parent:

119:a21e283730d1

Child:

140:53dbdb207542

--- a/imu.cpp	Wed Dec 01 19:17:36 2021 +0000
+++ b/imu.cpp	Mon Dec 06 08:26:16 2021 +0000
@@ -1,47 +1,47 @@
-#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();
+//        
+//        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