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@77:2bf856e3eca4, 2021-09-07 (annotated)

Committer:
NaotoMorita
Date:
Tue Sep 07 08:13:45 2021 +0000
Revision:
77:2bf856e3eca4
Parent:
76:7fd3ac1afe3e
Child:
82:c183c29d2427
update0907

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 77:2bf856e3eca4 14 //accref.z = 1.0f;
NaotoMorita 77:2bf856e3eca4 15
NaotoMorita 77:2bf856e3eca4 16 ax = accelgyro.getAccelerationX() - agoffset[0];
NaotoMorita 77:2bf856e3eca4 17 ay = accelgyro.getAccelerationY() - agoffset[1];
NaotoMorita 77:2bf856e3eca4 18 az = accelgyro.getAccelerationZ() - agoffset[2];
NaotoMorita 77:2bf856e3eca4 19 gx = accelgyro.getRotationX() - agoffset[3];
NaotoMorita 77:2bf856e3eca4 20 gy = accelgyro.getRotationY() - agoffset[4];
NaotoMorita 77:2bf856e3eca4 21 gz = accelgyro.getRotationZ() - agoffset[5];
NaotoMorita 77:2bf856e3eca4 22
NaotoMorita 73:84ffa0166e6c 23 float sigma_acc = sqrt(0.000020f);
NaotoMorita 73:84ffa0166e6c 24 float sigma_gyro = sqrt(0.000005f);
NaotoMorita 75:a505b9896da1 25 float sigma_mag = sqrt(0.00001f);
NaotoMorita 73:84ffa0166e6c 26 // gx gy gz ax ay az
NaotoMorita 73:84ffa0166e6c 27 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 77:2bf856e3eca4 28 acc.x = float(vp.accData[0]) / ACCEL_SSF + mapfloat(float(ax) / ACCEL_SSF,accMin[0],accMax[0],-1.0f,1.0f); //FS_SEL_0 16,384 LSB / g
NaotoMorita 77:2bf856e3eca4 29 acc.y = float(vp.accData[1]) / ACCEL_SSF + mapfloat(float(ay) / ACCEL_SSF,accMin[1],accMax[1],-1.0f,1.0f);
NaotoMorita 77:2bf856e3eca4 30 acc.z = float(vp.accData[2]) / ACCEL_SSF + mapfloat(float(az) / ACCEL_SSF,accMin[2],accMax[2],-1.0f,1.0f)-accref.z;
NaotoMorita 73:84ffa0166e6c 31 acc = accMedian.Process(acc);
NaotoMorita 73:84ffa0166e6c 32 // 角速度値を分解能で割って角速度(rad per sec)に変換する
NaotoMorita 77:2bf856e3eca4 33 gyro.x = float(vp.gyroData[0]) / GYRO_SSF * 0.0174533f + float(gx) / GYRO_SSF * 0.0174533f; // (rad/s)
NaotoMorita 77:2bf856e3eca4 34 gyro.y = float(vp.gyroData[1]) / GYRO_SSF * 0.0174533f + float(gy) / GYRO_SSF * 0.0174533f;
NaotoMorita 77:2bf856e3eca4 35 gyro.z = float(vp.gyroData[2]) / GYRO_SSF * 0.0174533f + float(gz) / GYRO_SSF * 0.0174533f;
NaotoMorita 73:84ffa0166e6c 36 gyro = gyroMedian.Process(gyro);
NaotoMorita 75:a505b9896da1 37 mag.x = -float(vp.magData[1])/1000.0f + sigma_mag*randn();
NaotoMorita 75:a505b9896da1 38 mag.y = -float(vp.magData[0])/1000.0f + sigma_mag*randn();
NaotoMorita 74:f67062e7813e 39 mag.z = float(vp.magData[2])/1000.0f + sigma_mag*randn();
NaotoMorita 73:84ffa0166e6c 40 mag = magMedian.Process(mag);
NaotoMorita 73:84ffa0166e6c 41 if(acc.Norm()<0.01f || mag.Norm() < 0.001f){
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 77:2bf856e3eca4 45 gyro.x = 0.0f;
NaotoMorita 77:2bf856e3eca4 46 gyro.y = 0.0f;
NaotoMorita 77:2bf856e3eca4 47 gyro.z = 0.0f;
NaotoMorita 73:84ffa0166e6c 48 mag.x = 0.113657f;
NaotoMorita 73:84ffa0166e6c 49 mag.y = -0.278425f;
NaotoMorita 73:84ffa0166e6c 50 mag.z = 0.980554f;
NaotoMorita 73:84ffa0166e6c 51 }
NaotoMorita 73:84ffa0166e6c 52 }
NaotoMorita 73:84ffa0166e6c 53
NaotoMorita 73:84ffa0166e6c 54 float randn()
NaotoMorita 73:84ffa0166e6c 55 {
NaotoMorita 73:84ffa0166e6c 56 float x = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 57 float y = (float)rand()/RAND_MAX;
NaotoMorita 73:84ffa0166e6c 58 float z1 = sqrt(-2.0 * log(x)) * cos(2.0 * M_PI * y);
NaotoMorita 73:84ffa0166e6c 59 return z1;
NaotoMorita 73:84ffa0166e6c 60 }