Diff: main.cpp

Revision:

15:6abaac6e5b03

Parent:

14:468ccb88e387

Child:

16:802f457588c6

--- a/main.cpp	Thu Jan 28 04:41:40 2021 +0000
+++ b/main.cpp	Thu Jan 28 05:13:24 2021 +0000
@@ -2,11 +2,12 @@
 #include "SBUS.hpp"
 #include "IMUfilter.h"
 #include "LoopTicker.hpp"
-
+#include "MPU6050.h"
 
 SBUS sbus(PD_5, PD_6);
+MPU6050 mpu(PB_9,PB_8);
 Serial pc(USBTX, USBRX);
-IMUfilter filter(100, 0.1);
+IMUfilter filter(1/100, 0.1);
 DigitalOut led1(LED1);
 DigitalOut led3(LED3);
 PwmOut servo1(PB_4);
@@ -14,9 +15,9 @@
 
 int ch1, ch2;
 float rc1, rc2;
-float pitch = 0.0;
-float roll = 0.0;
-float yaw = 0.0;
+double pitch = 0.0;
+double roll = 0.0;
+double yaw = 0.0;
 float gyro[3] = {0,0,0};
 float acc[3] = {0,0,1.0};
 int out1, out2;
@@ -52,7 +53,7 @@
     }
     pitch = filter.getPitch();
     
-    pc.printf("pitch:%d ", pitch);
+    pc.printf("pitch:%f ", pitch);
     pc.printf("out1:%d ", out1);
     pc.printf("out2:%d\r\n", out2);
     servo1.pulsewidth_us(out1);
@@ -62,22 +63,24 @@
 void interrupt_Gyro()
 {
     // gx gy gz ax ay az
-    mpu.getGyro(gyor);
+    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]);
 }
 
 int main()
 {
     mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
-    mpu.setGyroRange(MPU6050_GYRO_RANGE_250)
+    mpu.setGyroRange(MPU6050_GYRO_RANGE_250);
     pc.baud(9600);
     servo1.period(0.020);
     servo2.period(0.020);
     LoopTicker timer_SBUS;
     LoopTicker timer_Gyro;
-    timer_SBUS.attach(interrupt_SBUS, 0.020);
-    timer_Gyro.attach(interrupt_Gyro, 0.010);
+    timer_SBUS.attach(interrupt_SBUS, 1/50);
+    timer_Gyro.attach(interrupt_Gyro, 1/100);
     while(1) {
         timer_SBUS.loop();
         timer_Gyro.loop();