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:
- 16:802f457588c6
- Parent:
- 15:6abaac6e5b03
- Child:
- 17:6b7a363d06d2
diff -r 6abaac6e5b03 -r 802f457588c6 main.cpp --- a/main.cpp Thu Jan 28 05:13:24 2021 +0000 +++ b/main.cpp Thu Jan 28 05:27:17 2021 +0000 @@ -7,7 +7,7 @@ SBUS sbus(PD_5, PD_6); MPU6050 mpu(PB_9,PB_8); Serial pc(USBTX, USBRX); -IMUfilter filter(1/100, 0.1); +IMUfilter filter(0.01, 0.1); DigitalOut led1(LED1); DigitalOut led3(LED3); PwmOut servo1(PB_4); @@ -51,6 +51,7 @@ if(out2<pwmMin){out2 = pwmMin;}; if(out2>pwmMax){out2 = pwmMax;}; } + filter.computeEuler(); pitch = filter.getPitch(); pc.printf("pitch:%f ", pitch); @@ -64,10 +65,9 @@ { // gx gy gz ax ay az mpu.getGyro(gyro); - printf("gyro: %f\n",gyro[0]); mpu.getAccelero(acc); - printf("acc: %f\n",acc[0]); - filter.updateFilter(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2]); + + filter.updateFilter(gyro[0]/180.0*3.14,gyro[1]/180.0*3.14,gyro[2]/180.0*3.14,acc[0],acc[1],acc[2]); } int main() @@ -79,8 +79,8 @@ servo2.period(0.020); LoopTicker timer_SBUS; LoopTicker timer_Gyro; - timer_SBUS.attach(interrupt_SBUS, 1/50); - timer_Gyro.attach(interrupt_Gyro, 1/100); + timer_SBUS.attach(interrupt_SBUS, 0.02); + timer_Gyro.attach(interrupt_Gyro, 0.01); while(1) { timer_SBUS.loop(); timer_Gyro.loop();