Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
main.cpp
- Committer:
- naker
- Date:
- 2021-01-28
- Revision:
- 15:6abaac6e5b03
- Parent:
- 14:468ccb88e387
- Child:
- 16:802f457588c6
File content as of revision 15:6abaac6e5b03:
#include "mbed.h" #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(1/100, 0.1); DigitalOut led1(LED1); DigitalOut led3(LED3); PwmOut servo1(PB_4); PwmOut servo2(PB_5); int ch1, ch2; float rc1, rc2; 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; 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:%f ", pitch); pc.printf("out1:%d ", out1); pc.printf("out2:%d\r\n", out2); servo1.pulsewidth_us(out1); servo2.pulsewidth_us(out2); } void interrupt_Gyro() { // 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]); } int main() { mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G); 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, 1/50); timer_Gyro.attach(interrupt_Gyro, 1/100); while(1) { timer_SBUS.loop(); timer_Gyro.loop(); } }