HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

imu.cpp@144:b3a713b4f1c4, 22 months ago (annotated)

Committer:
NaotoMorita
Date:
Wed Jun 29 07:57:10 2022 +0000
Revision:
144:b3a713b4f1c4
Parent:
140:53dbdb207542 
can fly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 140:53dbdb207542 1 #include "global.hpp"
cocorlow 140:53dbdb207542 2
cocorlow 140:53dbdb207542 3 void getIMUval()
cocorlow 140:53dbdb207542 4 {
cocorlow 140:53dbdb207542 5 lsm.readAccel();
cocorlow 140:53dbdb207542 6 lsm.readMag();
cocorlow 140:53dbdb207542 7 lsm.readGyro();
cocorlow 140:53dbdb207542 8
cocorlow 140:53dbdb207542 9 Vector3f accmat;
cocorlow 140:53dbdb207542 10 accmat << lsm.ax * 9.8f - agoffset[0], lsm.ay * 9.8f - agoffset[1], lsm.az * 9.8f - agoffset[2];
cocorlow 140:53dbdb207542 11 Vector3f accAlign = SensorAlignmentAG*accmat;
cocorlow 140:53dbdb207542 12
cocorlow 140:53dbdb207542 13 acc = accAlign;
cocorlow 140:53dbdb207542 14
cocorlow 140:53dbdb207542 15 Vector3f gyromat;
cocorlow 140:53dbdb207542 16 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];
cocorlow 140:53dbdb207542 17 Vector3f gyroAlign = SensorAlignmentAG*gyromat;
cocorlow 140:53dbdb207542 18 gyro = gyroAlign;
cocorlow 140:53dbdb207542 19
cocorlow 140:53dbdb207542 20 Vector3f magraw;
cocorlow 140:53dbdb207542 21 magraw << lsm.mx, lsm.my, lsm.mz;
cocorlow 140:53dbdb207542 22 magraw = SensorAlignmentMAG*magraw;
cocorlow 140:53dbdb207542 23 float inputMag[3];
cocorlow 140:53dbdb207542 24 float outputMag[3];
cocorlow 140:53dbdb207542 25 inputMag[0] = magraw(0)*1000.0f;
cocorlow 140:53dbdb207542 26 inputMag[1] = magraw(1)*1000.0f;
cocorlow 140:53dbdb207542 27 inputMag[2] = magraw(2)*1000.0f;
cocorlow 140:53dbdb207542 28 magCalibrator.run(inputMag,outputMag);
cocorlow 140:53dbdb207542 29 mag(0) = outputMag[0];
cocorlow 140:53dbdb207542 30 mag(1) = outputMag[1];
cocorlow 140:53dbdb207542 31 mag(2) = outputMag[2];
cocorlow 140:53dbdb207542 32 //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));
cocorlow 140:53dbdb207542 33
cocorlow 140:53dbdb207542 34 palt = -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);
cocorlow 140:53dbdb207542 35
cocorlow 140:53dbdb207542 36 //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);
cocorlow 140:53dbdb207542 37 //printf("%f %f %f\n", lsm.gx, lsm.gy, lsm.gz);
cocorlow 140:53dbdb207542 38 //printf("%f %f %f\n", lsm.mx, lsm.my, lsm.mz);
cocorlow 140:53dbdb207542 39 //float pressure = lps.readPressureMillibars();
cocorlow 140:53dbdb207542 40 //float altitude = lps.pressureToAltitudeMeters(pressure);
cocorlow 140:53dbdb207542 41 //float temperature = lps.readTemperatureC();
cocorlow 140:53dbdb207542 42 //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
cocorlow 140:53dbdb207542 43 }