HAPSRG / Mbed 2 deprecated hexaTest_Eigen

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

Committer:
cocorlow
Date:
Thu Jan 28 03:32:13 2021 +0000
Revision:
11:ec79564cf265
Parent:
7:70161eb0f854
Child:
12:06891de5c601
20210128

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 7:70161eb0f854 18 long map(long x, long in_min, long in_max, long out_min, long out_max)
cocorlow 7:70161eb0f854 19 {
cocorlow 7:70161eb0f854 20 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cocorlow 7:70161eb0f854 21 }
cocorlow 7:70161eb0f854 22
cocorlow 3:79e62f9b13c8 23 void interrupt_SBUS()
cocorlow 3:79e62f9b13c8 24 {
cocorlow 3:79e62f9b13c8 25 led1 = !led1;
cocorlow 3:79e62f9b13c8 26 if(sbus.failSafe == false)
cocorlow 3:79e62f9b13c8 27 {
cocorlow 3:79e62f9b13c8 28 ch1 = int(sbus.getData(1));
cocorlow 3:79e62f9b13c8 29 ch2 = int(sbus.getData(2));
cocorlow 6:2cba569272fe 30 pc.printf("ch1 :%d ", ch1);
cocorlow 6:2cba569272fe 31 pc.printf("ch2 :%d ", ch2);
cocorlow 6:2cba569272fe 32 float LP_rc = 0.65f;
cocorlow 6:2cba569272fe 33 float LP_rc3 = 0.15f;
cocorlow 7:70161eb0f854 34 rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1;
cocorlow 7:70161eb0f854 35 rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2;
cocorlow 6:2cba569272fe 36 pc.printf("rc1 :%f ", rc1);
cocorlow 6:2cba569272fe 37 pc.printf("rc2 :%f ", rc1);
cocorlow 3:79e62f9b13c8 38 int pwmMax = 1800;
cocorlow 3:79e62f9b13c8 39 int pwmMin = 1200;
cocorlow 11:ec79564cf265 40 out1 = map((int)(rc1*2000.0f),-2000,2000,pwmMin,pwmMax);
cocorlow 3:79e62f9b13c8 41 if(out1<pwmMin){out1 = pwmMin;};
cocorlow 3:79e62f9b13c8 42 if(out1>pwmMax){out1 = pwmMax;};
cocorlow 11:ec79564cf265 43 out2 = map((int)(rc2*2000.0f),-2000,2000,pwmMin,pwmMax);
cocorlow 3:79e62f9b13c8 44 if(out2<pwmMin){out2 = pwmMin;};
cocorlow 3:79e62f9b13c8 45 if(out2>pwmMax){out2 = pwmMax;};
cocorlow 3:79e62f9b13c8 46
cocorlow 6:2cba569272fe 47 pc.printf("out1:%d ", out1);
cocorlow 3:79e62f9b13c8 48 pc.printf("out2:%d\r\n", out2);
cocorlow 3:79e62f9b13c8 49 servo1.pulsewidth_us(out1);
cocorlow 3:79e62f9b13c8 50 servo2.pulsewidth_us(out2);
cocorlow 3:79e62f9b13c8 51 }
cocorlow 3:79e62f9b13c8 52 }
cocorlow 3:79e62f9b13c8 53
cocorlow 3:79e62f9b13c8 54 void interrupt_Gyro()
cocorlow 3:79e62f9b13c8 55 {
cocorlow 3:79e62f9b13c8 56 led3 = !led3;
cocorlow 3:79e62f9b13c8 57 }
cocorlow 3:79e62f9b13c8 58
NaotoMorita 0:6b18a09a6628 59 int main()
NaotoMorita 0:6b18a09a6628 60 {
NaotoMorita 0:6b18a09a6628 61 pc.baud(9600);
cocorlow 3:79e62f9b13c8 62 servo1.period(0.020);
cocorlow 3:79e62f9b13c8 63 servo2.period(0.020);
cocorlow 6:2cba569272fe 64 LoopTicker timer_SBUS;
cocorlow 6:2cba569272fe 65 LoopTicker timer_Gyro;
cocorlow 11:ec79564cf265 66 timer_SBUS.attach(interrupt_SBUS, 0.015);
cocorlow 6:2cba569272fe 67 timer_Gyro.attach(interrupt_Gyro, 0.5);
NaotoMorita 0:6b18a09a6628 68 while(1) {
cocorlow 6:2cba569272fe 69 timer_SBUS.loop();
cocorlow 11:ec79564cf265 70 timer_Gyro.loop();
cocorlow 11:ec79564cf265 71 // interrupt_SBUS();
cocorlow 11:ec79564cf265 72 interrupt_Gyro();
NaotoMorita 0:6b18a09a6628 73 }
NaotoMorita 0:6b18a09a6628 74 }