File content as of revision 102:1c77ff6e2a85:

#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 = 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);
        
        Matrix magraw(3,1);
        magraw << lsm.mx <<lsm.my << lsm.mz;
        magres = magCalibrator.calcResidial(magraw);
        if(magres > magresThreshold){
            magCalibrator.updateParams(magraw,0.001f);
        };
        Matrix magmod = SensorAlignment*magCalibrator.outCalibratedMag(magraw);
        mag.x = magmod(1,1);
        mag.y = magmod(2,1);
        mag.z = 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);
}