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@6:2cba569272fe, 2021-01-21 (annotated)

Committer:
cocorlow
Date:
Thu Jan 21 15:25:51 2021 +0000
Revision:
6:2cba569272fe
Parent:
5:9cad4ce807b9
Child:
7:70161eb0f854
[Attention] SBUS library has been revised! LoopTicker library

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"
cocorlow 6:2cba569272fe 3 #include "LoopTicker.hpp"
cocorlow 6:2cba569272fe 4
cocorlow 6:2cba569272fe 5
NaotoMorita 0:6b18a09a6628 6 SBUS sbus(PD_5, PD_6);
NaotoMorita 0:6b18a09a6628 7 Serial pc(USBTX, USBRX);
NaotoMorita 0:6b18a09a6628 8
cocorlow 3:79e62f9b13c8 9 DigitalOut led1(LED1);
cocorlow 3:79e62f9b13c8 10 DigitalOut led3(LED3);
cocorlow 3:79e62f9b13c8 11 PwmOut servo1(PB_4);
cocorlow 3:79e62f9b13c8 12 PwmOut servo2(PB_5);
cocorlow 3:79e62f9b13c8 13
NaotoMorita 5:9cad4ce807b9 14 int ch1, ch2;
cocorlow 3:79e62f9b13c8 15 float rc1, rc2;
cocorlow 3:79e62f9b13c8 16 int out1, out2;
cocorlow 3:79e62f9b13c8 17
cocorlow 3:79e62f9b13c8 18 void interrupt_SBUS()
cocorlow 3:79e62f9b13c8 19 {
cocorlow 3:79e62f9b13c8 20 led1 = !led1;
cocorlow 3:79e62f9b13c8 21 if(sbus.failSafe == false)
cocorlow 3:79e62f9b13c8 22 {
cocorlow 3:79e62f9b13c8 23 ch1 = int(sbus.getData(1));
cocorlow 3:79e62f9b13c8 24 ch2 = int(sbus.getData(2));
cocorlow 6:2cba569272fe 25 pc.printf("ch1 :%d ", ch1);
cocorlow 6:2cba569272fe 26 pc.printf("ch2 :%d ", ch2);
cocorlow 6:2cba569272fe 27 float LP_rc = 0.65f;
cocorlow 6:2cba569272fe 28 float LP_rc3 = 0.15f;
cocorlow 6:2cba569272fe 29 rc1 = LP_rc*float(SBUS::map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1;
cocorlow 6:2cba569272fe 30 rc2 = LP_rc*float(SBUS::map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2;
cocorlow 6:2cba569272fe 31 pc.printf("rc1 :%f ", rc1);
cocorlow 6:2cba569272fe 32 pc.printf("rc2 :%f ", rc1);
cocorlow 3:79e62f9b13c8 33 int pwmMax = 1800;
cocorlow 3:79e62f9b13c8 34 int pwmMin = 1200;
cocorlow 6:2cba569272fe 35 out1 = SBUS::map((int)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax);
cocorlow 3:79e62f9b13c8 36 if(out1<pwmMin){out1 = pwmMin;};
cocorlow 3:79e62f9b13c8 37 if(out1>pwmMax){out1 = pwmMax;};
cocorlow 6:2cba569272fe 38 out2 = SBUS::map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax);
cocorlow 3:79e62f9b13c8 39 if(out2<pwmMin){out2 = pwmMin;};
cocorlow 3:79e62f9b13c8 40 if(out2>pwmMax){out2 = pwmMax;};
cocorlow 3:79e62f9b13c8 41
cocorlow 6:2cba569272fe 42 pc.printf("out1:%d ", out1);
cocorlow 3:79e62f9b13c8 43 pc.printf("out2:%d\r\n", out2);
cocorlow 3:79e62f9b13c8 44 servo1.pulsewidth_us(out1);
cocorlow 3:79e62f9b13c8 45 servo2.pulsewidth_us(out2);
cocorlow 3:79e62f9b13c8 46 }
cocorlow 3:79e62f9b13c8 47 }
cocorlow 3:79e62f9b13c8 48
cocorlow 3:79e62f9b13c8 49 void interrupt_Gyro()
cocorlow 3:79e62f9b13c8 50 {
cocorlow 3:79e62f9b13c8 51 led3 = !led3;
cocorlow 3:79e62f9b13c8 52 }
cocorlow 3:79e62f9b13c8 53
NaotoMorita 0:6b18a09a6628 54 int main()
NaotoMorita 0:6b18a09a6628 55 {
NaotoMorita 0:6b18a09a6628 56 pc.baud(9600);
cocorlow 3:79e62f9b13c8 57 servo1.period(0.020);
cocorlow 3:79e62f9b13c8 58 servo2.period(0.020);
cocorlow 6:2cba569272fe 59 LoopTicker timer_SBUS;
cocorlow 6:2cba569272fe 60 LoopTicker timer_Gyro;
NaotoMorita 4:7d2eae0115a2 61 timer_SBUS.attach(interrupt_SBUS, 0.020);
cocorlow 6:2cba569272fe 62 timer_Gyro.attach(interrupt_Gyro, 0.5);
NaotoMorita 0:6b18a09a6628 63 while(1) {
cocorlow 6:2cba569272fe 64 timer_SBUS.loop();
cocorlow 6:2cba569272fe 65 timer_SBUS.loop();
NaotoMorita 0:6b18a09a6628 66 }
NaotoMorita 0:6b18a09a6628 67 }