Diff: main.cpp

Revision:

10:d2d196cf93ab

Parent:

7:70161eb0f854

Child:

13:ac7c1c861aaf

--- a/main.cpp	Thu Jan 21 15:50:49 2021 +0000
+++ b/main.cpp	Thu Jan 28 04:02:18 2021 +0000
@@ -1,11 +1,12 @@
 #include "mbed.h"
 #include "SBUS.hpp"
+//#include "IMUfilter.h"
 #include "LoopTicker.hpp"
 
 
 SBUS sbus(PD_5, PD_6);
 Serial pc(USBTX, USBRX);
-
+//IMUfilter filter(100, 0.1);
 DigitalOut led1(LED1);
 DigitalOut led3(LED3);
 PwmOut servo1(PB_4);
@@ -13,6 +14,9 @@
 
 int ch1, ch2;
 float rc1, rc2;
+float pitch = 0.0;
+float roll = 0.0;
+float yaw = 0.0;
 int out1, out2;
 
 long map(long x, long in_min, long in_max, long out_min, long out_max)
@@ -43,17 +47,19 @@
         out2 = map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax);
         if(out2<pwmMin){out2 = pwmMin;};
         if(out2>pwmMax){out2 = pwmMax;};
-        
-        pc.printf("out1:%d ", out1);
-        pc.printf("out2:%d\r\n", out2);
-        servo1.pulsewidth_us(out1);
-        servo2.pulsewidth_us(out2);
     }
+    pitch = filter.getPitch();
+    
+    pc.printf("pitch:%d ", pitch);
+    pc.printf("out1:%d ", out1);
+    pc.printf("out2:%d\r\n", out2);
+    servo1.pulsewidth_us(out1);
+    servo2.pulsewidth_us(out2);
 }
 
 void interrupt_Gyro()
 {
-    led3 = !led3;
+    //filter.updateFilter(0, 0, 0, 0, 0, 1.0);
 }
 
 int main()
@@ -64,9 +70,9 @@
     LoopTicker timer_SBUS;
     LoopTicker timer_Gyro;
     timer_SBUS.attach(interrupt_SBUS, 0.020);
-    timer_Gyro.attach(interrupt_Gyro, 0.5);
+    timer_Gyro.attach(interrupt_Gyro, 0.010);
     while(1) {
         timer_SBUS.loop();
-        timer_SBUS.loop();
+        timer_Gyro.loop();
     }
 }
\ No newline at end of file