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@84:ff48e01ea76b, 2021-09-29 (annotated)

Committer:
NaotoMorita
Date:
Wed Sep 29 04:50:51 2021 +0000
Revision:
84:ff48e01ea76b
Parent:
83:e69ab831031c
Child:
90:96c2b0ed4b96
newer;

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 83:e69ab831031c 15 float sigma_acc = sqrt(0.000020f);
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 73:84ffa0166e6c 20 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 82:c183c29d2427 21 acc.x = float(vp.accData[0]) / ACCEL_SSF + sigma_acc*randn(); //FS_SEL_0 16,384 LSB / g
NaotoMorita 82:c183c29d2427 22 acc.y = float(vp.accData[1]) / ACCEL_SSF + sigma_acc*randn();
NaotoMorita 82:c183c29d2427 23 acc.z = 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 84:ff48e01ea76b 30 mag.x = -float(vp.magData[0])/1000.0f + sigma_mag*randn();
NaotoMorita 84:ff48e01ea76b 31 mag.y = -float(vp.magData[1])/1000.0f + sigma_mag*randn();
NaotoMorita 84:ff48e01ea76b 32 mag.z = -float(vp.magData[2])/1000.0f + sigma_mag*randn();
NaotoMorita 84:ff48e01ea76b 33 mag = magMedian.Process(mag);
NaotoMorita 84:ff48e01ea76b 34 vi.x = float(vp.viData[0])/1000.0f + sigma_vi*randn();
NaotoMorita 84:ff48e01ea76b 35 vi.y = float(vp.viData[1])/1000.0f + sigma_vi*randn();
NaotoMorita 84:ff48e01ea76b 36 vi.z = float(vp.viData[2])/1000.0f + sigma_vi*randn();
NaotoMorita 82:c183c29d2427 37
NaotoMorita 83:e69ab831031c 38 if(acc.Norm()<0.01f){
NaotoMorita 73:84ffa0166e6c 39 acc.x = 0.0f;
NaotoMorita 73:84ffa0166e6c 40 acc.y = 0.0f;
NaotoMorita 73:84ffa0166e6c 41 acc.z = 1.0f;
NaotoMorita 73:84ffa0166e6c 42 mag.x = 0.113657f;
NaotoMorita 73:84ffa0166e6c 43 mag.y = -0.278425f;
NaotoMorita 73:84ffa0166e6c 44 mag.z = 0.980554f;
NaotoMorita 73:84ffa0166e6c 45 }
NaotoMorita 73:84ffa0166e6c 46 }
NaotoMorita 73:84ffa0166e6c 47
NaotoMorita 73:84ffa0166e6c 48 float randn()
NaotoMorita 73:84ffa0166e6c 49 {
NaotoMorita 73:84ffa0166e6c 50 float x = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 51 float y = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 52 float z1 = sqrt(-2.0 * log(x)) * cos(2.0 * M_PI * y);
NaotoMorita 73:84ffa0166e6c 53 return z1;
NaotoMorita 73:84ffa0166e6c 54 }