File content as of revision 144:b3a713b4f1c4:

#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);
}