solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: hil.cpp
- Revision:
- 139:b378528c05f2
- Parent:
- 111:0fae4fbe2a80
- Child:
- 141:725321fe2949
diff -r 4882d71c6a5f -r b378528c05f2 hil.cpp --- a/hil.cpp Wed Dec 01 19:17:36 2021 +0000 +++ b/hil.cpp Mon Dec 06 08:26:16 2021 +0000 @@ -1,84 +1,84 @@ -#include "global.hpp" - -void getHilIMUval() -{ - switch(vp.commandIndex){ - case 1: - NVIC_SystemReset(); - break; - default : - break; - } - rpy_align.x = 0.0f; - rpy_align.y = 0.0f; - accref.z = 1.0f; - - - lsm.readAccel(); - lsm.readMag(); - lsm.readGyro(); - - float accmx = lsm.ax * 9.8f - agoffset[0]; - float accmy = lsm.ay * 9.8f - agoffset[1]; - float accmz = lsm.az * 9.8f - agoffset[2]; - float gyromx = (lsm.gx * M_PI / 180.0f) - agoffset[3]; - float gyromy = (lsm.gy * M_PI / 180.0f) - agoffset[4]; - float gyromz = (lsm.gz * M_PI / 180.0f) - agoffset[5]; - - // gx gy gz ax ay az - // 加速度値を分解能で割って加速度(m/s^2)に変換する - acc.x = 9.8f*float(vp.accData[0]) / 4096.0f + accmx; //FS_SEL_0 16,384 LSB / g - acc.y = 9.8f*float(vp.accData[1]) / 4096.0f + accmy; - acc.z = 9.8f*float(vp.accData[2]) / 4096.0f + accmy; - - // 角速度値を分解能で割って角速度(rad per sec)に変換する - gyro.x = float(vp.gyroData[0]) / 131.0f * 0.0174533f + gyromx; // (rad/s) - gyro.y = float(vp.gyroData[1]) / 131.0f * 0.0174533f + gyromy; - gyro.z = float(vp.gyroData[2]) / 131.0f * 0.0174533f + gyromz; - - Matrix magraw(3,1); - magraw(1,1) = float(vp.magData[0])/1000.0f + (lsm.mx-magref.x); - magraw(2,1) = float(vp.magData[1])/1000.0f + (lsm.my-magref.y); - magraw(3,1) = float(vp.magData[2])/1000.0f + (lsm.mz-magref.z); - - //magres = magCalibrator.calcResidial(magraw); - //if(magres > magresThreshold){ - // magCalibrator.updateParams(magraw,0.001f); - //}; - Matrix magmod = magraw; - mag.x = magmod(1,1); - mag.y = magmod(2,1); - mag.z = magmod(3,1); - - palt = float(vp.piData[2])/1.0f -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);; - - if(abs(vp.accData[0])<0.0001f && abs(vp.accData[1])<0.0001f && abs(vp.accData[2])<0.0001f){ - acc.z += -9.8f; - mag.x += 0.5; - } -} - -void getHilGPSval() -{ - float sigma_vi = 0.1f; - float sigma_pi = 1.0f; - - //GPSの速度情報m/s - vi.x = float(vp.viData[0])/1000.0f + sigma_vi*randn(); - vi.y = float(vp.viData[1])/1000.0f + sigma_vi*randn(); - vi.z = float(vp.viData[2])/1000.0f + sigma_vi*randn(); - - //GPSの位置情報m/s - pi.x = float(vp.piData[0])/1.0f + sigma_pi*randn(); - pi.y = float(vp.piData[1])/1.0f + sigma_pi*randn(); - pi.z = float(vp.piData[2])/1.0f + sigma_pi*randn(); - -} - -float randn() -{ - float x = (float)rand()/RAND_MAX; - float y = (float)rand()/RAND_MAX; - float z1 = sqrt(-2.0 * log(x)) * cos(2.0 * M_PI * y); - return z1; -} \ No newline at end of file +//#include "global.hpp" +// +//void getHilIMUval() +//{ +// switch(vp.commandIndex){ +// case 1: +// NVIC_SystemReset(); +// break; +// default : +// break; +// } +// rpy_align.x = 0.0f; +// rpy_align.y = 0.0f; +// accref.z = 1.0f; +// +// +// lsm.readAccel(); +// lsm.readMag(); +// lsm.readGyro(); +// +// float accmx = lsm.ax * 9.8f - agoffset[0]; +// float accmy = lsm.ay * 9.8f - agoffset[1]; +// float accmz = lsm.az * 9.8f - agoffset[2]; +// float gyromx = (lsm.gx * M_PI / 180.0f) - agoffset[3]; +// float gyromy = (lsm.gy * M_PI / 180.0f) - agoffset[4]; +// float gyromz = (lsm.gz * M_PI / 180.0f) - agoffset[5]; +// +// // gx gy gz ax ay az +// // 加速度値を分解能で割って加速度(m/s^2)に変換する +// acc.x = 9.8f*float(vp.accData[0]) / 4096.0f + accmx; //FS_SEL_0 16,384 LSB / g +// acc.y = 9.8f*float(vp.accData[1]) / 4096.0f + accmy; +// acc.z = 9.8f*float(vp.accData[2]) / 4096.0f + accmy; +// +// // 角速度値を分解能で割って角速度(rad per sec)に変換する +// gyro.x = float(vp.gyroData[0]) / 131.0f * 0.0174533f + gyromx; // (rad/s) +// gyro.y = float(vp.gyroData[1]) / 131.0f * 0.0174533f + gyromy; +// gyro.z = float(vp.gyroData[2]) / 131.0f * 0.0174533f + gyromz; +// +// Matrix magraw(3,1); +// magraw(1,1) = float(vp.magData[0])/1000.0f + (lsm.mx-magref.x); +// magraw(2,1) = float(vp.magData[1])/1000.0f + (lsm.my-magref.y); +// magraw(3,1) = float(vp.magData[2])/1000.0f + (lsm.mz-magref.z); +// +// //magres = magCalibrator.calcResidial(magraw); +// //if(magres > magresThreshold){ +// // magCalibrator.updateParams(magraw,0.001f); +// //}; +// Matrix magmod = magraw; +// mag.x = magmod(1,1); +// mag.y = magmod(2,1); +// mag.z = magmod(3,1); +// +// palt = float(vp.piData[2])/1.0f -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);; +// +// if(abs(vp.accData[0])<0.0001f && abs(vp.accData[1])<0.0001f && abs(vp.accData[2])<0.0001f){ +// acc.z += -9.8f; +// mag.x += 0.5; +// } +//} +// +//void getHilGPSval() +//{ +// float sigma_vi = 0.1f; +// float sigma_pi = 1.0f; +// +// //GPSの速度情報m/s +// vi.x = float(vp.viData[0])/1000.0f + sigma_vi*randn(); +// vi.y = float(vp.viData[1])/1000.0f + sigma_vi*randn(); +// vi.z = float(vp.viData[2])/1000.0f + sigma_vi*randn(); +// +// //GPSの位置情報m/s +// pi.x = float(vp.piData[0])/1.0f + sigma_pi*randn(); +// pi.y = float(vp.piData[1])/1.0f + sigma_pi*randn(); +// pi.z = float(vp.piData[2])/1.0f + sigma_pi*randn(); +// +//} +// +//float randn() +//{ +// float x = (float)rand()/RAND_MAX; +// float y = (float)rand()/RAND_MAX; +// float z1 = sqrt(-2.0 * log(x)) * cos(2.0 * M_PI * y); +// return z1; +//} \ No newline at end of file