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@139:b378528c05f2, 2021-12-06 (annotated)

Committer:
cocorlow
Date:
Mon Dec 06 08:26:16 2021 +0000
Revision:
139:b378528c05f2
Parent:
111:0fae4fbe2a80
Child:
141:725321fe2949
Eigen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 139:b378528c05f2 1 //#include "global.hpp"
cocorlow 139:b378528c05f2 2 //
cocorlow 139:b378528c05f2 3 //void getHilIMUval()
cocorlow 139:b378528c05f2 4 //{
cocorlow 139:b378528c05f2 5 // switch(vp.commandIndex){
cocorlow 139:b378528c05f2 6 // case 1:
cocorlow 139:b378528c05f2 7 // NVIC_SystemReset();
cocorlow 139:b378528c05f2 8 // break;
cocorlow 139:b378528c05f2 9 // default :
cocorlow 139:b378528c05f2 10 // break;
cocorlow 139:b378528c05f2 11 // }
cocorlow 139:b378528c05f2 12 // rpy_align.x = 0.0f;
cocorlow 139:b378528c05f2 13 // rpy_align.y = 0.0f;
cocorlow 139:b378528c05f2 14 // accref.z = 1.0f;
cocorlow 139:b378528c05f2 15 //
cocorlow 139:b378528c05f2 16 //
cocorlow 139:b378528c05f2 17 // lsm.readAccel();
cocorlow 139:b378528c05f2 18 // lsm.readMag();
cocorlow 139:b378528c05f2 19 // lsm.readGyro();
cocorlow 139:b378528c05f2 20 //
cocorlow 139:b378528c05f2 21 // float accmx = lsm.ax * 9.8f - agoffset[0];
cocorlow 139:b378528c05f2 22 // float accmy = lsm.ay * 9.8f - agoffset[1];
cocorlow 139:b378528c05f2 23 // float accmz = lsm.az * 9.8f - agoffset[2];
cocorlow 139:b378528c05f2 24 // float gyromx = (lsm.gx * M_PI / 180.0f) - agoffset[3];
cocorlow 139:b378528c05f2 25 // float gyromy = (lsm.gy * M_PI / 180.0f) - agoffset[4];
cocorlow 139:b378528c05f2 26 // float gyromz = (lsm.gz * M_PI / 180.0f) - agoffset[5];
cocorlow 139:b378528c05f2 27 //
cocorlow 139:b378528c05f2 28 // // gx gy gz ax ay az
cocorlow 139:b378528c05f2 29 // // 加速度値を分解能で割って加速度(m/s^2)に変換する
cocorlow 139:b378528c05f2 30 // acc.x = 9.8f*float(vp.accData[0]) / 4096.0f + accmx; //FS_SEL_0 16,384 LSB / g
cocorlow 139:b378528c05f2 31 // acc.y = 9.8f*float(vp.accData[1]) / 4096.0f + accmy;
cocorlow 139:b378528c05f2 32 // acc.z = 9.8f*float(vp.accData[2]) / 4096.0f + accmy;
cocorlow 139:b378528c05f2 33 //
cocorlow 139:b378528c05f2 34 // // 角速度値を分解能で割って角速度(rad per sec)に変換する
cocorlow 139:b378528c05f2 35 // gyro.x = float(vp.gyroData[0]) / 131.0f * 0.0174533f + gyromx; // (rad/s)
cocorlow 139:b378528c05f2 36 // gyro.y = float(vp.gyroData[1]) / 131.0f * 0.0174533f + gyromy;
cocorlow 139:b378528c05f2 37 // gyro.z = float(vp.gyroData[2]) / 131.0f * 0.0174533f + gyromz;
cocorlow 139:b378528c05f2 38 //
cocorlow 139:b378528c05f2 39 // Matrix magraw(3,1);
cocorlow 139:b378528c05f2 40 // magraw(1,1) = float(vp.magData[0])/1000.0f + (lsm.mx-magref.x);
cocorlow 139:b378528c05f2 41 // magraw(2,1) = float(vp.magData[1])/1000.0f + (lsm.my-magref.y);
cocorlow 139:b378528c05f2 42 // magraw(3,1) = float(vp.magData[2])/1000.0f + (lsm.mz-magref.z);
cocorlow 139:b378528c05f2 43 //
cocorlow 139:b378528c05f2 44 // //magres = magCalibrator.calcResidial(magraw);
cocorlow 139:b378528c05f2 45 // //if(magres > magresThreshold){
cocorlow 139:b378528c05f2 46 // // magCalibrator.updateParams(magraw,0.001f);
cocorlow 139:b378528c05f2 47 // //};
cocorlow 139:b378528c05f2 48 // Matrix magmod = magraw;
cocorlow 139:b378528c05f2 49 // mag.x = magmod(1,1);
cocorlow 139:b378528c05f2 50 // mag.y = magmod(2,1);
cocorlow 139:b378528c05f2 51 // mag.z = magmod(3,1);
cocorlow 139:b378528c05f2 52 //
cocorlow 139:b378528c05f2 53 // palt = float(vp.piData[2])/1.0f -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);;
cocorlow 139:b378528c05f2 54 //
cocorlow 139:b378528c05f2 55 // if(abs(vp.accData[0])<0.0001f && abs(vp.accData[1])<0.0001f && abs(vp.accData[2])<0.0001f){
cocorlow 139:b378528c05f2 56 // acc.z += -9.8f;
cocorlow 139:b378528c05f2 57 // mag.x += 0.5;
cocorlow 139:b378528c05f2 58 // }
cocorlow 139:b378528c05f2 59 //}
cocorlow 139:b378528c05f2 60 //
cocorlow 139:b378528c05f2 61 //void getHilGPSval()
cocorlow 139:b378528c05f2 62 //{
cocorlow 139:b378528c05f2 63 // float sigma_vi = 0.1f;
cocorlow 139:b378528c05f2 64 // float sigma_pi = 1.0f;
cocorlow 139:b378528c05f2 65 //
cocorlow 139:b378528c05f2 66 // //GPSの速度情報m/s
cocorlow 139:b378528c05f2 67 // vi.x = float(vp.viData[0])/1000.0f + sigma_vi*randn();
cocorlow 139:b378528c05f2 68 // vi.y = float(vp.viData[1])/1000.0f + sigma_vi*randn();
cocorlow 139:b378528c05f2 69 // vi.z = float(vp.viData[2])/1000.0f + sigma_vi*randn();
cocorlow 139:b378528c05f2 70 //
cocorlow 139:b378528c05f2 71 // //GPSの位置情報m/s
cocorlow 139:b378528c05f2 72 // pi.x = float(vp.piData[0])/1.0f + sigma_pi*randn();
cocorlow 139:b378528c05f2 73 // pi.y = float(vp.piData[1])/1.0f + sigma_pi*randn();
cocorlow 139:b378528c05f2 74 // pi.z = float(vp.piData[2])/1.0f + sigma_pi*randn();
cocorlow 139:b378528c05f2 75 //
cocorlow 139:b378528c05f2 76 //}
cocorlow 139:b378528c05f2 77 //
cocorlow 139:b378528c05f2 78 //float randn()
cocorlow 139:b378528c05f2 79 //{
cocorlow 139:b378528c05f2 80 // float x = (float)rand()/RAND_MAX;
cocorlow 139:b378528c05f2 81 // float y = (float)rand()/RAND_MAX;
cocorlow 139:b378528c05f2 82 // float z1 = sqrt(-2.0 * log(x)) * cos(2.0 * M_PI * y);
cocorlow 139:b378528c05f2 83 // return z1;
cocorlow 139:b378528c05f2 84 //}