Diff: main.cpp

Revision:

16:802f457588c6

Parent:

15:6abaac6e5b03

Child:

17:6b7a363d06d2

--- 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();