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:
- 10:d2d196cf93ab
- Parent:
- 7:70161eb0f854
- Child:
- 13:ac7c1c861aaf
diff -r 58c2411703f7 -r d2d196cf93ab main.cpp --- 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