HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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@10:d2d196cf93ab, 2021-01-28 (annotated)

Committer:
NaotoMorita
Date:
Thu Jan 28 04:02:18 2021 +0000
Revision:
10:d2d196cf93ab
Parent:
7:70161eb0f854
Child:
13:ac7c1c861aaf
implimented imu filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NaotoMorita 0:6b18a09a6628 1 #include "mbed.h"
NaotoMorita 0:6b18a09a6628 2 #include "SBUS.hpp"
NaotoMorita 10:d2d196cf93ab 3 //#include "IMUfilter.h"
cocorlow 6:2cba569272fe 4 #include "LoopTicker.hpp"
cocorlow 6:2cba569272fe 5
cocorlow 6:2cba569272fe 6
NaotoMorita 0:6b18a09a6628 7 SBUS sbus(PD_5, PD_6);
NaotoMorita 0:6b18a09a6628 8 Serial pc(USBTX, USBRX);
NaotoMorita 10:d2d196cf93ab 9 //IMUfilter filter(100, 0.1);
cocorlow 3:79e62f9b13c8 10 DigitalOut led1(LED1);
cocorlow 3:79e62f9b13c8 11 DigitalOut led3(LED3);
cocorlow 3:79e62f9b13c8 12 PwmOut servo1(PB_4);
cocorlow 3:79e62f9b13c8 13 PwmOut servo2(PB_5);
cocorlow 3:79e62f9b13c8 14
NaotoMorita 5:9cad4ce807b9 15 int ch1, ch2;
cocorlow 3:79e62f9b13c8 16 float rc1, rc2;
NaotoMorita 10:d2d196cf93ab 17 float pitch = 0.0;
NaotoMorita 10:d2d196cf93ab 18 float roll = 0.0;
NaotoMorita 10:d2d196cf93ab 19 float yaw = 0.0;
cocorlow 3:79e62f9b13c8 20 int out1, out2;
cocorlow 3:79e62f9b13c8 21
cocorlow 7:70161eb0f854 22 long map(long x, long in_min, long in_max, long out_min, long out_max)
cocorlow 7:70161eb0f854 23 {
cocorlow 7:70161eb0f854 24 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cocorlow 7:70161eb0f854 25 }
cocorlow 7:70161eb0f854 26
cocorlow 3:79e62f9b13c8 27 void interrupt_SBUS()
cocorlow 3:79e62f9b13c8 28 {
cocorlow 3:79e62f9b13c8 29 led1 = !led1;
cocorlow 3:79e62f9b13c8 30 if(sbus.failSafe == false)
cocorlow 3:79e62f9b13c8 31 {
cocorlow 3:79e62f9b13c8 32 ch1 = int(sbus.getData(1));
cocorlow 3:79e62f9b13c8 33 ch2 = int(sbus.getData(2));
cocorlow 6:2cba569272fe 34 pc.printf("ch1 :%d ", ch1);
cocorlow 6:2cba569272fe 35 pc.printf("ch2 :%d ", ch2);
cocorlow 6:2cba569272fe 36 float LP_rc = 0.65f;
cocorlow 6:2cba569272fe 37 float LP_rc3 = 0.15f;
cocorlow 7:70161eb0f854 38 rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1;
cocorlow 7:70161eb0f854 39 rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2;
cocorlow 6:2cba569272fe 40 pc.printf("rc1 :%f ", rc1);
cocorlow 6:2cba569272fe 41 pc.printf("rc2 :%f ", rc1);
cocorlow 3:79e62f9b13c8 42 int pwmMax = 1800;
cocorlow 3:79e62f9b13c8 43 int pwmMin = 1200;
cocorlow 7:70161eb0f854 44 out1 = map((int)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax);
cocorlow 3:79e62f9b13c8 45 if(out1<pwmMin){out1 = pwmMin;};
cocorlow 3:79e62f9b13c8 46 if(out1>pwmMax){out1 = pwmMax;};
cocorlow 7:70161eb0f854 47 out2 = map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax);
cocorlow 3:79e62f9b13c8 48 if(out2<pwmMin){out2 = pwmMin;};
cocorlow 3:79e62f9b13c8 49 if(out2>pwmMax){out2 = pwmMax;};
cocorlow 3:79e62f9b13c8 50 }
NaotoMorita 10:d2d196cf93ab 51 pitch = filter.getPitch();
NaotoMorita 10:d2d196cf93ab 52
NaotoMorita 10:d2d196cf93ab 53 pc.printf("pitch:%d ", pitch);
NaotoMorita 10:d2d196cf93ab 54 pc.printf("out1:%d ", out1);
NaotoMorita 10:d2d196cf93ab 55 pc.printf("out2:%d\r\n", out2);
NaotoMorita 10:d2d196cf93ab 56 servo1.pulsewidth_us(out1);
NaotoMorita 10:d2d196cf93ab 57 servo2.pulsewidth_us(out2);
cocorlow 3:79e62f9b13c8 58 }
cocorlow 3:79e62f9b13c8 59
cocorlow 3:79e62f9b13c8 60 void interrupt_Gyro()
cocorlow 3:79e62f9b13c8 61 {
NaotoMorita 10:d2d196cf93ab 62 //filter.updateFilter(0, 0, 0, 0, 0, 1.0);
cocorlow 3:79e62f9b13c8 63 }
cocorlow 3:79e62f9b13c8 64
NaotoMorita 0:6b18a09a6628 65 int main()
NaotoMorita 0:6b18a09a6628 66 {
NaotoMorita 0:6b18a09a6628 67 pc.baud(9600);
cocorlow 3:79e62f9b13c8 68 servo1.period(0.020);
cocorlow 3:79e62f9b13c8 69 servo2.period(0.020);
cocorlow 6:2cba569272fe 70 LoopTicker timer_SBUS;
cocorlow 6:2cba569272fe 71 LoopTicker timer_Gyro;
NaotoMorita 4:7d2eae0115a2 72 timer_SBUS.attach(interrupt_SBUS, 0.020);
NaotoMorita 10:d2d196cf93ab 73 timer_Gyro.attach(interrupt_Gyro, 0.010);
NaotoMorita 0:6b18a09a6628 74 while(1) {
cocorlow 6:2cba569272fe 75 timer_SBUS.loop();
NaotoMorita 10:d2d196cf93ab 76 timer_Gyro.loop();
NaotoMorita 0:6b18a09a6628 77 }
NaotoMorita 0:6b18a09a6628 78 }