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

hil.cpp@94:579e875a4244, 2021-11-01 (annotated)

Committer:
NaotoMorita
Date:
Mon Nov 01 09:18:07 2021 +0000
Revision:
94:579e875a4244
Parent:
93:b827f78a717a
Child:
102:1c77ff6e2a85
parameter modified;

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 float sigma_mag = 0.1f;
NaotoMorita 94:579e875a4244 17 float sigma_palt = 1.0f;
NaotoMorita 93:b827f78a717a 18
NaotoMorita 93:b827f78a717a 19 lsm.readAccel();
NaotoMorita 93:b827f78a717a 20 lsm.readMag();
NaotoMorita 93:b827f78a717a 21 lsm.readGyro();
NaotoMorita 93:b827f78a717a 22
NaotoMorita 93:b827f78a717a 23 float accmx = lsm.ax * 9.8f - agoffset[0];
NaotoMorita 93:b827f78a717a 24 float accmy = lsm.ay * 9.8f - agoffset[1];
NaotoMorita 93:b827f78a717a 25 float accmz = lsm.az * 9.8f - agoffset[2];
NaotoMorita 93:b827f78a717a 26 float gyromx = (lsm.gx * M_PI / 180.0f) - agoffset[3];
NaotoMorita 93:b827f78a717a 27 float gyromy = (lsm.gy * M_PI / 180.0f) - agoffset[4];
NaotoMorita 93:b827f78a717a 28 float gyromz = (lsm.gz * M_PI / 180.0f) - agoffset[5];
NaotoMorita 93:b827f78a717a 29
NaotoMorita 73:84ffa0166e6c 30 // gx gy gz ax ay az
NaotoMorita 90:96c2b0ed4b96 31 // 加速度値を分解能で割って加速度(m/s^2)に変換する
NaotoMorita 93:b827f78a717a 32 acc.x = 9.8f*float(vp.accData[0]) / 4096.0f + accmx; //FS_SEL_0 16,384 LSB / g
NaotoMorita 93:b827f78a717a 33 acc.y = 9.8f*float(vp.accData[1]) / 4096.0f + accmy;
NaotoMorita 93:b827f78a717a 34 acc.z = 9.8f*float(vp.accData[2]) / 4096.0f + accmy;
NaotoMorita 73:84ffa0166e6c 35 acc = accMedian.Process(acc);
NaotoMorita 73:84ffa0166e6c 36 // 角速度値を分解能で割って角速度(rad per sec)に変換する
NaotoMorita 93:b827f78a717a 37 gyro.x = float(vp.gyroData[0]) / 131.0f * 0.0174533f + gyromx; // (rad/s)
NaotoMorita 93:b827f78a717a 38 gyro.y = float(vp.gyroData[1]) / 131.0f * 0.0174533f + gyromy;
NaotoMorita 93:b827f78a717a 39 gyro.z = float(vp.gyroData[2]) / 131.0f * 0.0174533f + gyromz;
NaotoMorita 73:84ffa0166e6c 40 gyro = gyroMedian.Process(gyro);
NaotoMorita 90:96c2b0ed4b96 41
NaotoMorita 90:96c2b0ed4b96 42 mag.x = float(vp.magData[0])/1000.0f + sigma_mag*randn();
NaotoMorita 90:96c2b0ed4b96 43 mag.y = float(vp.magData[1])/1000.0f + sigma_mag*randn();
NaotoMorita 90:96c2b0ed4b96 44 mag.z = float(vp.magData[2])/1000.0f + sigma_mag*randn();
NaotoMorita 84:ff48e01ea76b 45 mag = magMedian.Process(mag);
NaotoMorita 90:96c2b0ed4b96 46
NaotoMorita 94:579e875a4244 47 palt = float(vp.piData[2])/1.0f + sigma_palt*randn();
NaotoMorita 94:579e875a4244 48
NaotoMorita 92:00460f6df439 49 if(abs(vp.accData[0])<0.0001f && abs(vp.accData[1])<0.0001f && abs(vp.accData[2])<0.0001f){
NaotoMorita 92:00460f6df439 50 acc.x = 0.0f;
NaotoMorita 92:00460f6df439 51 acc.y = 0.0f;
NaotoMorita 92:00460f6df439 52 acc.z = -9.8f;
NaotoMorita 92:00460f6df439 53 gyro.x = 0.0f;
NaotoMorita 92:00460f6df439 54 gyro.y = 0.0f;
NaotoMorita 92:00460f6df439 55 gyro.z = 0.0f;
NaotoMorita 92:00460f6df439 56 mag.x = 1.0f;
NaotoMorita 92:00460f6df439 57 mag.y = 0.0f;
NaotoMorita 92:00460f6df439 58 mag.z = 0.0f;
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 }