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@90:96c2b0ed4b96, 2021-10-26 (annotated)

Committer:
NaotoMorita
Date:
Tue Oct 26 05:36:18 2021 +0000
Revision:
90:96c2b0ed4b96
Parent:
84:ff48e01ea76b
Child:
92:00460f6df439
solaeskf compiled

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NaotoMorita 73:84ffa0166e6c 1 #include "global.hpp"
NaotoMorita 73:84ffa0166e6c 2
NaotoMorita 73:84ffa0166e6c 3 void getHILval()
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 83:e69ab831031c 18 float sigma_vi = sqrt(0.002f);
NaotoMorita 73:84ffa0166e6c 19 // gx gy gz ax ay az
NaotoMorita 90:96c2b0ed4b96 20 // 加速度値を分解能で割って加速度(m/s^2)に変換する
NaotoMorita 90:96c2b0ed4b96 21 acc.x = 9.8f*float(vp.accData[0]) / ACCEL_SSF + sigma_acc*randn(); //FS_SEL_0 16,384 LSB / g
NaotoMorita 90:96c2b0ed4b96 22 acc.y = 9.8f*float(vp.accData[1]) / ACCEL_SSF + sigma_acc*randn();
NaotoMorita 90:96c2b0ed4b96 23 acc.z = 9.8f*float(vp.accData[2]) / ACCEL_SSF + sigma_acc*randn();
NaotoMorita 73:84ffa0166e6c 24 acc = accMedian.Process(acc);
NaotoMorita 73:84ffa0166e6c 25 // 角速度値を分解能で割って角速度(rad per sec)に変換する
NaotoMorita 82:c183c29d2427 26 gyro.x = float(vp.gyroData[0]) / GYRO_SSF * 0.0174533f + sigma_gyro*randn(); // (rad/s)
NaotoMorita 82:c183c29d2427 27 gyro.y = float(vp.gyroData[1]) / GYRO_SSF * 0.0174533f + sigma_gyro*randn();
NaotoMorita 82:c183c29d2427 28 gyro.z = float(vp.gyroData[2]) / GYRO_SSF * 0.0174533f + sigma_gyro*randn();
NaotoMorita 73:84ffa0166e6c 29 gyro = gyroMedian.Process(gyro);
NaotoMorita 90:96c2b0ed4b96 30
NaotoMorita 90:96c2b0ed4b96 31 mag.x = float(vp.magData[0])/1000.0f + sigma_mag*randn();
NaotoMorita 90:96c2b0ed4b96 32 mag.y = float(vp.magData[1])/1000.0f + sigma_mag*randn();
NaotoMorita 90:96c2b0ed4b96 33 mag.z = float(vp.magData[2])/1000.0f + sigma_mag*randn();
NaotoMorita 84:ff48e01ea76b 34 mag = magMedian.Process(mag);
NaotoMorita 90:96c2b0ed4b96 35
NaotoMorita 90:96c2b0ed4b96 36 //GPSの速度情報m/s
NaotoMorita 84:ff48e01ea76b 37 vi.x = float(vp.viData[0])/1000.0f + sigma_vi*randn();
NaotoMorita 84:ff48e01ea76b 38 vi.y = float(vp.viData[1])/1000.0f + sigma_vi*randn();
NaotoMorita 84:ff48e01ea76b 39 vi.z = float(vp.viData[2])/1000.0f + sigma_vi*randn();
NaotoMorita 82:c183c29d2427 40
NaotoMorita 83:e69ab831031c 41 if(acc.Norm()<0.01f){
NaotoMorita 73:84ffa0166e6c 42 acc.x = 0.0f;
NaotoMorita 73:84ffa0166e6c 43 acc.y = 0.0f;
NaotoMorita 73:84ffa0166e6c 44 acc.z = 1.0f;
NaotoMorita 73:84ffa0166e6c 45 mag.x = 0.113657f;
NaotoMorita 73:84ffa0166e6c 46 mag.y = -0.278425f;
NaotoMorita 73:84ffa0166e6c 47 mag.z = 0.980554f;
NaotoMorita 73:84ffa0166e6c 48 }
NaotoMorita 73:84ffa0166e6c 49 }
NaotoMorita 73:84ffa0166e6c 50
NaotoMorita 73:84ffa0166e6c 51 float randn()
NaotoMorita 73:84ffa0166e6c 52 {
NaotoMorita 73:84ffa0166e6c 53 float x = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 54 float y = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 55 float z1 = sqrt(-2.0 * log(x)) * cos(2.0 * M_PI * y);
NaotoMorita 73:84ffa0166e6c 56 return z1;
NaotoMorita 73:84ffa0166e6c 57 }