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@92:00460f6df439, 2021-10-28 (annotated)

Committer:
NaotoMorita
Date:
Thu Oct 28 09:44:47 2021 +0000
Revision:
92:00460f6df439
Parent:
90:96c2b0ed4b96
Child:
93:b827f78a717a
for PmodNAV

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 90:96c2b0ed4b96 15 float sigma_acc = sqrt(0.00020f);
NaotoMorita 83:e69ab831031c 16 float sigma_gyro = sqrt(0.000005f);
NaotoMorita 83:e69ab831031c 17 float sigma_mag = sqrt(0.002f);
NaotoMorita 73:84ffa0166e6c 18 // gx gy gz ax ay az
NaotoMorita 90:96c2b0ed4b96 19 // 加速度値を分解能で割って加速度(m/s^2)に変換する
NaotoMorita 92:00460f6df439 20 acc.x = 9.8f*float(vp.accData[0]) / 4096.0f + sigma_acc*randn(); //FS_SEL_0 16,384 LSB / g
NaotoMorita 92:00460f6df439 21 acc.y = 9.8f*float(vp.accData[1]) / 4096.0f + sigma_acc*randn();
NaotoMorita 92:00460f6df439 22 acc.z = 9.8f*float(vp.accData[2]) / 4096.0f + sigma_acc*randn();
NaotoMorita 73:84ffa0166e6c 23 acc = accMedian.Process(acc);
NaotoMorita 73:84ffa0166e6c 24 // 角速度値を分解能で割って角速度(rad per sec)に変換する
NaotoMorita 92:00460f6df439 25 gyro.x = float(vp.gyroData[0]) / 131.0f * 0.0174533f + sigma_gyro*randn(); // (rad/s)
NaotoMorita 92:00460f6df439 26 gyro.y = float(vp.gyroData[1]) / 131.0f * 0.0174533f + sigma_gyro*randn();
NaotoMorita 92:00460f6df439 27 gyro.z = float(vp.gyroData[2]) / 131.0f * 0.0174533f + sigma_gyro*randn();
NaotoMorita 73:84ffa0166e6c 28 gyro = gyroMedian.Process(gyro);
NaotoMorita 90:96c2b0ed4b96 29
NaotoMorita 90:96c2b0ed4b96 30 mag.x = float(vp.magData[0])/1000.0f + sigma_mag*randn();
NaotoMorita 90:96c2b0ed4b96 31 mag.y = float(vp.magData[1])/1000.0f + sigma_mag*randn();
NaotoMorita 90:96c2b0ed4b96 32 mag.z = float(vp.magData[2])/1000.0f + sigma_mag*randn();
NaotoMorita 84:ff48e01ea76b 33 mag = magMedian.Process(mag);
NaotoMorita 90:96c2b0ed4b96 34
NaotoMorita 92:00460f6df439 35 if(abs(vp.accData[0])<0.0001f && abs(vp.accData[1])<0.0001f && abs(vp.accData[2])<0.0001f){
NaotoMorita 92:00460f6df439 36 acc.x = 0.0f;
NaotoMorita 92:00460f6df439 37 acc.y = 0.0f;
NaotoMorita 92:00460f6df439 38 acc.z = -9.8f;
NaotoMorita 92:00460f6df439 39 gyro.x = 0.0f;
NaotoMorita 92:00460f6df439 40 gyro.y = 0.0f;
NaotoMorita 92:00460f6df439 41 gyro.z = 0.0f;
NaotoMorita 92:00460f6df439 42 mag.x = 1.0f;
NaotoMorita 92:00460f6df439 43 mag.y = 0.0f;
NaotoMorita 92:00460f6df439 44 mag.z = 0.0f;
NaotoMorita 92:00460f6df439 45 }
NaotoMorita 92:00460f6df439 46 }
NaotoMorita 92:00460f6df439 47
NaotoMorita 92:00460f6df439 48 void getHilGPSval()
NaotoMorita 92:00460f6df439 49 {
NaotoMorita 92:00460f6df439 50 float sigma_vi = 2.0f/1.1774f;
NaotoMorita 92:00460f6df439 51 float sigma_pi = 2.0f/1.1774f;
NaotoMorita 92:00460f6df439 52
NaotoMorita 90:96c2b0ed4b96 53 //GPSの速度情報m/s
NaotoMorita 84:ff48e01ea76b 54 vi.x = float(vp.viData[0])/1000.0f + sigma_vi*randn();
NaotoMorita 84:ff48e01ea76b 55 vi.y = float(vp.viData[1])/1000.0f + sigma_vi*randn();
NaotoMorita 84:ff48e01ea76b 56 vi.z = float(vp.viData[2])/1000.0f + sigma_vi*randn();
NaotoMorita 82:c183c29d2427 57
NaotoMorita 92:00460f6df439 58 //GPSの位置情報m/s
NaotoMorita 92:00460f6df439 59 pi.x = float(vp.piData[0])/1.0f + sigma_pi*randn();
NaotoMorita 92:00460f6df439 60 pi.y = float(vp.piData[1])/1.0f + sigma_pi*randn();
NaotoMorita 92:00460f6df439 61 pi.z = float(vp.piData[2])/1.0f + sigma_pi*randn();
NaotoMorita 92:00460f6df439 62
NaotoMorita 73:84ffa0166e6c 63 }
NaotoMorita 73:84ffa0166e6c 64
NaotoMorita 73:84ffa0166e6c 65 float randn()
NaotoMorita 73:84ffa0166e6c 66 {
NaotoMorita 73:84ffa0166e6c 67 float x = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 68 float y = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 69 float z1 = sqrt(-2.0 * log(x)) * cos(2.0 * M_PI * y);
NaotoMorita 73:84ffa0166e6c 70 return z1;
NaotoMorita 73:84ffa0166e6c 71 }