solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
main.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-01-28
- Revision:
- 13:ac7c1c861aaf
- Parent:
- 10:d2d196cf93ab
- Child:
- 14:468ccb88e387
File content as of revision 13:ac7c1c861aaf:
#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); PwmOut servo2(PB_5); 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) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } void interrupt_SBUS() { led1 = !led1; if(sbus.failSafe == false) { ch1 = int(sbus.getData(1)); ch2 = int(sbus.getData(2)); pc.printf("ch1 :%d ", ch1); pc.printf("ch2 :%d ", ch2); float LP_rc = 0.65f; float LP_rc3 = 0.15f; rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1; rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2; pc.printf("rc1 :%f ", rc1); pc.printf("rc2 :%f ", rc1); int pwmMax = 1800; int pwmMin = 1200; out1 = map((int)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax); if(out1<pwmMin){out1 = pwmMin;}; if(out1>pwmMax){out1 = pwmMax;}; out2 = map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax); if(out2<pwmMin){out2 = pwmMin;}; if(out2>pwmMax){out2 = pwmMax;}; } 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() { filter.updateFilter(0, 0, 0, 0, 0, 1.0); } int main() { 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); while(1) { timer_SBUS.loop(); timer_Gyro.loop(); } }