solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 43:f0dd73430192
- Parent:
- 40:869f3791a3e2
- Child:
- 44:f571273d3223
diff -r 869f3791a3e2 -r f0dd73430192 main.cpp --- a/main.cpp Tue Mar 23 06:37:24 2021 +0000 +++ b/main.cpp Fri Apr 02 07:22:09 2021 +0000 @@ -541,10 +541,13 @@ << 0 << 0 <<1; getIMUval(); triad(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm, accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,mag_x/magnorm,mag_y/magnorm,mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm); + float sumLPaccnorm = 0; for(int i = 0; i<1000 ;i++){ getIMUval(); val_thmg += acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm); + sumLPaccnorm += LPaccnorm; } + accref[2]=-sumLPaccnorm/1000; val_thmg /= 1000; LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt);