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@73:84ffa0166e6c, 2021-07-15 (annotated)

Committer:
NaotoMorita
Date:
Thu Jul 15 05:26:15 2021 +0000
Revision:
73:84ffa0166e6c
Child:
74:f67062e7813e
9state

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 73:84ffa0166e6c 5 rpy_align.x = 0.0f;
NaotoMorita 73:84ffa0166e6c 6 rpy_align.y = 0.0f;
NaotoMorita 73:84ffa0166e6c 7 accref.z = 1.0f;
NaotoMorita 73:84ffa0166e6c 8 float sigma_acc = sqrt(0.000020f);
NaotoMorita 73:84ffa0166e6c 9 float sigma_gyro = sqrt(0.000005f);
NaotoMorita 73:84ffa0166e6c 10 float sigma_mag = sqrt(0.002f);
NaotoMorita 73:84ffa0166e6c 11 // gx gy gz ax ay az
NaotoMorita 73:84ffa0166e6c 12 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 73:84ffa0166e6c 13 acc.x = float(vp.accData[0]) / ACCEL_SSF + sigma_acc*randn(); //FS_SEL_0 16,384 LSB / g
NaotoMorita 73:84ffa0166e6c 14 acc.y = float(vp.accData[1]) / ACCEL_SSF + sigma_acc*randn();
NaotoMorita 73:84ffa0166e6c 15 acc.z = float(vp.accData[2]) / ACCEL_SSF + sigma_acc*randn();
NaotoMorita 73:84ffa0166e6c 16 acc = accMedian.Process(acc);
NaotoMorita 73:84ffa0166e6c 17 // 角速度値を分解能で割って角速度(rad per sec)に変換する
NaotoMorita 73:84ffa0166e6c 18 gyro.x = float(vp.gyroData[0]) / GYRO_SSF * 0.0174533f + sigma_gyro*randn(); // (rad/s)
NaotoMorita 73:84ffa0166e6c 19 gyro.y = float(vp.gyroData[1]) / GYRO_SSF * 0.0174533f + sigma_gyro*randn();
NaotoMorita 73:84ffa0166e6c 20 gyro.z = float(vp.gyroData[2]) / GYRO_SSF * 0.0174533f + sigma_gyro*randn();
NaotoMorita 73:84ffa0166e6c 21 gyro = gyroMedian.Process(gyro);
NaotoMorita 73:84ffa0166e6c 22 mag.x = -float(vp.magData[0])/1000.0f + sigma_mag*randn();
NaotoMorita 73:84ffa0166e6c 23 mag.y = -float(vp.magData[1])/1000.0f + sigma_mag*randn();
NaotoMorita 73:84ffa0166e6c 24 mag.z = -float(vp.magData[2])/1000.0f + sigma_mag*randn();
NaotoMorita 73:84ffa0166e6c 25 mag = magMedian.Process(mag);
NaotoMorita 73:84ffa0166e6c 26 if(acc.Norm()<0.01f || mag.Norm() < 0.001f){
NaotoMorita 73:84ffa0166e6c 27 acc.x = 0.0f;
NaotoMorita 73:84ffa0166e6c 28 acc.y = 0.0f;
NaotoMorita 73:84ffa0166e6c 29 acc.z = 1.0f;
NaotoMorita 73:84ffa0166e6c 30 mag.x = 0.113657f;
NaotoMorita 73:84ffa0166e6c 31 mag.y = -0.278425f;
NaotoMorita 73:84ffa0166e6c 32 mag.z = 0.980554f;
NaotoMorita 73:84ffa0166e6c 33 }
NaotoMorita 73:84ffa0166e6c 34 }
NaotoMorita 73:84ffa0166e6c 35
NaotoMorita 73:84ffa0166e6c 36 float randn()
NaotoMorita 73:84ffa0166e6c 37 {
NaotoMorita 73:84ffa0166e6c 38 float x = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 39 float y = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 40 float z1 = sqrt(-2.0 * log(x)) * cos(2.0 * M_PI * y);
NaotoMorita 73:84ffa0166e6c 41 return z1;
NaotoMorita 73:84ffa0166e6c 42 }