solaESKF_EIGEN

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

Committer:
NaotoMorita
Date:
Mon Nov 08 09:20:21 2021 +0000
Revision:
102:1c77ff6e2a85
Parent:
94:579e875a4244
Child:
103:fec71c2051c5
mag debuging

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NaotoMorita 73:84ffa0166e6c 1 #include "global.hpp"
NaotoMorita 73:84ffa0166e6c 2
NaotoMorita 92:00460f6df439 3 void getHilIMUval()
NaotoMorita 73:84ffa0166e6c 4 {
NaotoMorita 76:7fd3ac1afe3e 5 switch(vp.commandIndex){
NaotoMorita 76:7fd3ac1afe3e 6 case 1:
NaotoMorita 76:7fd3ac1afe3e 7 NVIC_SystemReset();
NaotoMorita 76:7fd3ac1afe3e 8 break;
NaotoMorita 76:7fd3ac1afe3e 9 default :
NaotoMorita 76:7fd3ac1afe3e 10 break;
NaotoMorita 76:7fd3ac1afe3e 11 }
NaotoMorita 73:84ffa0166e6c 12 rpy_align.x = 0.0f;
NaotoMorita 73:84ffa0166e6c 13 rpy_align.y = 0.0f;
NaotoMorita 82:c183c29d2427 14 accref.z = 1.0f;
NaotoMorita 93:b827f78a717a 15
NaotoMorita 93:b827f78a717a 16
NaotoMorita 93:b827f78a717a 17 lsm.readAccel();
NaotoMorita 93:b827f78a717a 18 lsm.readMag();
NaotoMorita 93:b827f78a717a 19 lsm.readGyro();
NaotoMorita 93:b827f78a717a 20
NaotoMorita 93:b827f78a717a 21 float accmx = lsm.ax * 9.8f - agoffset[0];
NaotoMorita 93:b827f78a717a 22 float accmy = lsm.ay * 9.8f - agoffset[1];
NaotoMorita 93:b827f78a717a 23 float accmz = lsm.az * 9.8f - agoffset[2];
NaotoMorita 93:b827f78a717a 24 float gyromx = (lsm.gx * M_PI / 180.0f) - agoffset[3];
NaotoMorita 93:b827f78a717a 25 float gyromy = (lsm.gy * M_PI / 180.0f) - agoffset[4];
NaotoMorita 93:b827f78a717a 26 float gyromz = (lsm.gz * M_PI / 180.0f) - agoffset[5];
NaotoMorita 93:b827f78a717a 27
NaotoMorita 73:84ffa0166e6c 28 // gx gy gz ax ay az
NaotoMorita 90:96c2b0ed4b96 29 // 加速度値を分解能で割って加速度(m/s^2)に変換する
NaotoMorita 93:b827f78a717a 30 acc.x = 9.8f*float(vp.accData[0]) / 4096.0f + accmx; //FS_SEL_0 16,384 LSB / g
NaotoMorita 93:b827f78a717a 31 acc.y = 9.8f*float(vp.accData[1]) / 4096.0f + accmy;
NaotoMorita 93:b827f78a717a 32 acc.z = 9.8f*float(vp.accData[2]) / 4096.0f + accmy;
NaotoMorita 102:1c77ff6e2a85 33
NaotoMorita 73:84ffa0166e6c 34 // 角速度値を分解能で割って角速度(rad per sec)に変換する
NaotoMorita 93:b827f78a717a 35 gyro.x = float(vp.gyroData[0]) / 131.0f * 0.0174533f + gyromx; // (rad/s)
NaotoMorita 93:b827f78a717a 36 gyro.y = float(vp.gyroData[1]) / 131.0f * 0.0174533f + gyromy;
NaotoMorita 93:b827f78a717a 37 gyro.z = float(vp.gyroData[2]) / 131.0f * 0.0174533f + gyromz;
NaotoMorita 90:96c2b0ed4b96 38
NaotoMorita 102:1c77ff6e2a85 39 Matrix magraw(3,1);
NaotoMorita 102:1c77ff6e2a85 40 magraw(1,1) = float(vp.magData[0])/1000.0f + (lsm.mx-magref.x);
NaotoMorita 102:1c77ff6e2a85 41 magraw(2,1) = float(vp.magData[1])/1000.0f + (lsm.my-magref.y);
NaotoMorita 102:1c77ff6e2a85 42 magraw(3,1) = float(vp.magData[2])/1000.0f + (lsm.mz-magref.z);
NaotoMorita 102:1c77ff6e2a85 43
NaotoMorita 102:1c77ff6e2a85 44 magres = magCalibrator.calcResidial(magraw);
NaotoMorita 102:1c77ff6e2a85 45 if(magres > magresThreshold){
NaotoMorita 102:1c77ff6e2a85 46 magCalibrator.updateParams(magraw,0.001f);
NaotoMorita 102:1c77ff6e2a85 47 };
NaotoMorita 102:1c77ff6e2a85 48 Matrix magmod = SensorAlignment*magCalibrator.outCalibratedMag(magraw);
NaotoMorita 102:1c77ff6e2a85 49 mag.x = magmod(1,1);
NaotoMorita 102:1c77ff6e2a85 50 mag.y = magmod(2,1);
NaotoMorita 102:1c77ff6e2a85 51 mag.z = magmod(3,1);
NaotoMorita 90:96c2b0ed4b96 52
NaotoMorita 102:1c77ff6e2a85 53 palt = float(vp.piData[2])/1.0f -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);;
NaotoMorita 94:579e875a4244 54
NaotoMorita 92:00460f6df439 55 if(abs(vp.accData[0])<0.0001f && abs(vp.accData[1])<0.0001f && abs(vp.accData[2])<0.0001f){
NaotoMorita 102:1c77ff6e2a85 56 acc.z += -9.8f;
NaotoMorita 102:1c77ff6e2a85 57 mag.x += 0.2f;
NaotoMorita 92:00460f6df439 58 }
NaotoMorita 92:00460f6df439 59 }
NaotoMorita 92:00460f6df439 60
NaotoMorita 92:00460f6df439 61 void getHilGPSval()
NaotoMorita 92:00460f6df439 62 {
NaotoMorita 93:b827f78a717a 63 float sigma_vi = 0.1f;
NaotoMorita 93:b827f78a717a 64 float sigma_pi = 1.0f;
NaotoMorita 92:00460f6df439 65
NaotoMorita 90:96c2b0ed4b96 66 //GPSの速度情報m/s
NaotoMorita 84:ff48e01ea76b 67 vi.x = float(vp.viData[0])/1000.0f + sigma_vi*randn();
NaotoMorita 84:ff48e01ea76b 68 vi.y = float(vp.viData[1])/1000.0f + sigma_vi*randn();
NaotoMorita 84:ff48e01ea76b 69 vi.z = float(vp.viData[2])/1000.0f + sigma_vi*randn();
NaotoMorita 82:c183c29d2427 70
NaotoMorita 92:00460f6df439 71 //GPSの位置情報m/s
NaotoMorita 92:00460f6df439 72 pi.x = float(vp.piData[0])/1.0f + sigma_pi*randn();
NaotoMorita 92:00460f6df439 73 pi.y = float(vp.piData[1])/1.0f + sigma_pi*randn();
NaotoMorita 92:00460f6df439 74 pi.z = float(vp.piData[2])/1.0f + sigma_pi*randn();
NaotoMorita 92:00460f6df439 75
NaotoMorita 73:84ffa0166e6c 76 }
NaotoMorita 73:84ffa0166e6c 77
NaotoMorita 73:84ffa0166e6c 78 float randn()
NaotoMorita 73:84ffa0166e6c 79 {
NaotoMorita 73:84ffa0166e6c 80 float x = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 81 float y = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 82 float z1 = sqrt(-2.0 * log(x)) * cos(2.0 * M_PI * y);
NaotoMorita 73:84ffa0166e6c 83 return z1;
NaotoMorita 73:84ffa0166e6c 84 }