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:
- 94:579e875a4244
- Parent:
- 93:b827f78a717a
- Child:
- 102:1c77ff6e2a85
diff -r b827f78a717a -r 579e875a4244 hil.cpp --- a/hil.cpp Fri Oct 29 13:30:24 2021 +0000 +++ b/hil.cpp Mon Nov 01 09:18:07 2021 +0000 @@ -14,6 +14,7 @@ accref.z = 1.0f; float sigma_mag = 0.1f; + float sigma_palt = 1.0f; lsm.readAccel(); lsm.readMag(); @@ -43,6 +44,8 @@ mag.z = float(vp.magData[2])/1000.0f + sigma_mag*randn(); mag = magMedian.Process(mag); + palt = float(vp.piData[2])/1.0f + sigma_palt*randn(); + if(abs(vp.accData[0])<0.0001f && abs(vp.accData[1])<0.0001f && abs(vp.accData[2])<0.0001f){ acc.x = 0.0f; acc.y = 0.0f;