HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

Eigen Revision

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

imu.cpp@139:b378528c05f2, 2021-12-06 (annotated)

Committer:
cocorlow
Date:
Mon Dec 06 08:26:16 2021 +0000
Revision:
139:b378528c05f2
Parent:
119:a21e283730d1
Child:
140:53dbdb207542
Eigen

Who changed what in which revision?

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