solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: run.cpp
- Revision:
- 102:1c77ff6e2a85
- Parent:
- 100:7589b663d462
- Child:
- 103:fec71c2051c5
diff -r 200e98dad7f8 -r 1c77ff6e2a85 run.cpp --- a/run.cpp Sun Nov 07 05:38:17 2021 +0000 +++ b/run.cpp Mon Nov 08 09:20:21 2021 +0000 @@ -21,10 +21,20 @@ agoffset[4] +=(lsm.gy * M_PI / 180.0f); agoffset[5] +=(lsm.gz * M_PI / 180.0f); palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars()); + if(hilFlag == true){ + magref.x += lsm.mx; + magref.y += lsm.my; + magref.z += lsm.mz; + } } for(int i = 0;i<6;i++){ agoffset[i] /= float(n_init); } + if(hilFlag == true){ + magref.x /= float(n_init); + magref.y /= float(n_init); + magref.z /= float(n_init); + } gps.Update(); gps.CalculateUnit(); palt0 /= float(n_init); @@ -62,9 +72,13 @@ Matrix RaccConst(4,4); setDiag(RaccConst,50.0f); - RaccConst(4,4) = 5.0f; + RaccConst(4,4) = 1.0f; //Matrix RgyroConst(2,2); //setDiag(RgyroConst,100); + Matrix Rmag(2,2); + Rmag(1,1) = 1.0f; + Rmag(2,2) = 1.0f; + _t.start(); float tgps = _t.read(); while(1) @@ -96,6 +110,9 @@ eskf.updateGPS(MatrixMath::Vector2mat(pi),MatrixMath::Vector2mat(vi),Rgps); } eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),palt,RaccConst); + if(magres < magresThreshold){ + eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag); + }; eskf.fuseErr2Nominal(); PIDtick.loop();