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

hil.cpp@122:68efdee114fa, 2021-11-19 (annotated)

Committer:
NaotoMorita
Date:
Fri Nov 19 05:44:23 2021 +0000
Revision:
122:68efdee114fa
Parent:
111:0fae4fbe2a80 
hatena

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 122:68efdee114fa 40 magraw(1,1) = float(vp.magData[0])/1000.0f +(lsm.mx-magref.x);
NaotoMorita 122:68efdee114fa 41 magraw(2,1) = float(vp.magData[1])/1000.0f +(lsm.my-magref.y);
NaotoMorita 122:68efdee114fa 42 magraw(3,1) = float(vp.magData[2])/1000.0f +(lsm.mz-magref.z);
NaotoMorita 102:1c77ff6e2a85 43
NaotoMorita 122:68efdee114fa 44 //float inputMag[3];
NaotoMorita 122:68efdee114fa 45 //float outputMag[3];
NaotoMorita 122:68efdee114fa 46 //inputMag[0] = magraw(1,1)*1000.0f;
NaotoMorita 122:68efdee114fa 47 //inputMag[1] = magraw(2,1)*1000.0f;
NaotoMorita 122:68efdee114fa 48 //inputMag[2] = magraw(3,1)*1000.0f;
NaotoMorita 122:68efdee114fa 49 //magCalibrator.run(inputMag,outputMag);
NaotoMorita 122:68efdee114fa 50 mag.x = magraw(1,1);
NaotoMorita 122:68efdee114fa 51 mag.y = magraw(2,1);
NaotoMorita 122:68efdee114fa 52 mag.z = magraw(3,1);
NaotoMorita 90:96c2b0ed4b96 53
NaotoMorita 102:1c77ff6e2a85 54 palt = float(vp.piData[2])/1.0f -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);;
NaotoMorita 94:579e875a4244 55
NaotoMorita 92:00460f6df439 56 if(abs(vp.accData[0])<0.0001f && abs(vp.accData[1])<0.0001f && abs(vp.accData[2])<0.0001f){
NaotoMorita 102:1c77ff6e2a85 57 acc.z += -9.8f;
NaotoMorita 111:0fae4fbe2a80 58 mag.x += 0.5;
NaotoMorita 92:00460f6df439 59 }
NaotoMorita 92:00460f6df439 60 }
NaotoMorita 92:00460f6df439 61
NaotoMorita 92:00460f6df439 62 void getHilGPSval()
NaotoMorita 92:00460f6df439 63 {
NaotoMorita 93:b827f78a717a 64 float sigma_vi = 0.1f;
NaotoMorita 93:b827f78a717a 65 float sigma_pi = 1.0f;
NaotoMorita 92:00460f6df439 66
NaotoMorita 90:96c2b0ed4b96 67 //GPSの速度情報m/s
NaotoMorita 84:ff48e01ea76b 68 vi.x = float(vp.viData[0])/1000.0f + sigma_vi*randn();
NaotoMorita 84:ff48e01ea76b 69 vi.y = float(vp.viData[1])/1000.0f + sigma_vi*randn();
NaotoMorita 84:ff48e01ea76b 70 vi.z = float(vp.viData[2])/1000.0f + sigma_vi*randn();
NaotoMorita 82:c183c29d2427 71
NaotoMorita 92:00460f6df439 72 //GPSの位置情報m/s
NaotoMorita 92:00460f6df439 73 pi.x = float(vp.piData[0])/1.0f + sigma_pi*randn();
NaotoMorita 92:00460f6df439 74 pi.y = float(vp.piData[1])/1.0f + sigma_pi*randn();
NaotoMorita 92:00460f6df439 75 pi.z = float(vp.piData[2])/1.0f + sigma_pi*randn();
NaotoMorita 92:00460f6df439 76
NaotoMorita 73:84ffa0166e6c 77 }
NaotoMorita 73:84ffa0166e6c 78
NaotoMorita 73:84ffa0166e6c 79 float randn()
NaotoMorita 73:84ffa0166e6c 80 {
NaotoMorita 73:84ffa0166e6c 81 float x = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 82 float y = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 83 float z1 = sqrt(-2.0 * log(x)) * cos(2.0 * M_PI * y);
NaotoMorita 73:84ffa0166e6c 84 return z1;
NaotoMorita 73:84ffa0166e6c 85 }