altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Copter_Specific/RCin.cpp@4:dc844fde64d7, 2019-11-22 (annotated)

Committer:
Kiwicjam
Date:
Fri Nov 22 08:40:26 2019 +0000
Revision:
4:dc844fde64d7
Parent:
3:bc24fee36ba3 
Workin set, not running,

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 2:e7874762cc25 1 #include "RCin.h"
altb2 2:e7874762cc25 2
altb2 2:e7874762cc25 3
altb2 2:e7874762cc25 4 using namespace std;
altb2 2:e7874762cc25 5
altb2 2:e7874762cc25 6 RCin::RCin(PinName pin) : pwm1(pin) , dout3(PA_10)
altb2 2:e7874762cc25 7 {
altb2 2:e7874762cc25 8 pwm1.fall(callback(this, &RCin::fall_edge));
altb2 2:e7874762cc25 9 pwm1.rise(callback(this, &RCin::rise_edge));
altb2 2:e7874762cc25 10 local_ti.start();
altb2 2:e7874762cc25 11 local_ti.reset();
altb2 2:e7874762cc25 12 for(uint8_t k = 0;k<20;k++)
altb2 2:e7874762cc25 13 pwms[k] = 0;
altb2 2:e7874762cc25 14 isAlive = false;
altb2 2:e7874762cc25 15 }
altb2 2:e7874762cc25 16
altb2 2:e7874762cc25 17 // *****************************************************************************
altb2 2:e7874762cc25 18 void RCin::map_Channels(void){
altb2 2:e7874762cc25 19 CH1_2_PM1.setup(600.0,1400.0,-1.0,1.0);
altb2 2:e7874762cc25 20 CH2_2_PM1.setup(1400.0,600.0,-1.0,1.0);
altb2 2:e7874762cc25 21 CH3_2_PM1.setup(600.0,1400.0,-1.0,1.0);
altb2 2:e7874762cc25 22 CH4_2_PM1.setup(600.0,1400.0,-1.0,1.0);
altb2 2:e7874762cc25 23 }
altb2 2:e7874762cc25 24
altb2 2:e7874762cc25 25 // *****************************************************************************
altb2 2:e7874762cc25 26 void RCin::rise_edge(void)
altb2 2:e7874762cc25 27 {
altb2 2:e7874762cc25 28 local_ti.reset();
altb2 2:e7874762cc25 29 }
altb2 2:e7874762cc25 30
altb2 2:e7874762cc25 31 // *****************************************************************************
altb2 2:e7874762cc25 32 void RCin::fall_edge(void)
altb2 2:e7874762cc25 33 {
altb2 2:e7874762cc25 34 uint32_t time_us = local_ti.read_us();
altb2 2:e7874762cc25 35 if(time_us > 2500){
altb2 2:e7874762cc25 36 chnr = 0;
altb2 2:e7874762cc25 37 map_pwm_2_PM1();
altb2 2:e7874762cc25 38 }
altb2 2:e7874762cc25 39 else
altb2 2:e7874762cc25 40 pwms[++chnr] = time_us;
altb2 2:e7874762cc25 41 }
altb2 2:e7874762cc25 42 // *****************************************************************************
altb2 2:e7874762cc25 43 uint8_t RCin::get_flightmode(void){
altb2 2:e7874762cc25 44 if(isAlive){
altb2 2:e7874762cc25 45 return current_flightmode;
altb2 2:e7874762cc25 46 }
altb2 2:e7874762cc25 47 else
altb2 2:e7874762cc25 48 return 0;
altb2 2:e7874762cc25 49 }
altb2 2:e7874762cc25 50 // *****************************************************************************
altb2 2:e7874762cc25 51 void RCin::map_pwm_2_PM1(void){
altb2 2:e7874762cc25 52 // data.RC_PM1[0]=CH1_2_PM1((float)pwms[1]); // CH1 -> Roll
altb2 2:e7874762cc25 53 // data.RC_PM1[1]=CH2_2_PM1((float)pwms[2]); // CH2 -> Pitch
altb2 2:e7874762cc25 54 // data.RC_PM1[3]=CH3_2_PM1((float)pwms[3]); // CHANGED HERE: CH3 -> Thrust!!!
altb2 2:e7874762cc25 55 // data.RC_PM1[2]=CH4_2_PM1((float)pwms[4]); // CHANGED HERE: CH4 -> Yaw!!!
altb2 2:e7874762cc25 56
altb2 2:e7874762cc25 57
altb2 2:e7874762cc25 58 dout3.write(1);
altb2 2:e7874762cc25 59
altb2 2:e7874762cc25 60 PM1[0]=CH1_2_PM1((float)pwms[1]); // CH1 -> Roll
altb2 2:e7874762cc25 61 PM1[1]=CH2_2_PM1((float)pwms[2]); // CH2 -> Pitch
altb2 2:e7874762cc25 62 PM1[3]=CH3_2_PM1((float)pwms[3]); // CHANGED HERE: CH3 -> Thrust!!!
altb2 2:e7874762cc25 63 PM1[2]=CH4_2_PM1((float)pwms[4]); // CHANGED HERE: CH4 -> Yaw!!!
altb2 3:bc24fee36ba3 64 // RC Chan 5 6
altb2 3:bc24fee36ba3 65 if(pwms[5] >620 && pwms[5] < 720) // 2 1
altb2 3:bc24fee36ba3 66 current_flightmode = LOITER;
altb2 3:bc24fee36ba3 67 else if(pwms[5] >720 && pwms[5] < 850) // 2 2
altb2 2:e7874762cc25 68 current_flightmode = AUTO;
altb2 3:bc24fee36ba3 69 else if(pwms[5] >870 && pwms[5] < 970) // 2 3
altb2 2:e7874762cc25 70 current_flightmode = RTL;
altb2 3:bc24fee36ba3 71 else if(pwms[5] >1000 && pwms[5] < 1100) // 1 1
altb2 2:e7874762cc25 72 current_flightmode = STABILIZED;
altb2 3:bc24fee36ba3 73 else if(pwms[5] >1100 && pwms[5] < 1200) // 1 2
altb2 2:e7874762cc25 74 current_flightmode = ALT_HOLD;
altb2 3:bc24fee36ba3 75 else if(pwms[5] >1200 && pwms[5] < 1350) // 1 3
altb2 2:e7874762cc25 76 current_flightmode = VEL_OF_Z_POS;
altb2 2:e7874762cc25 77 else
altb2 2:e7874762cc25 78 current_flightmode = STABILIZED;
altb2 2:e7874762cc25 79 if(pwms[3]>550)
altb2 2:e7874762cc25 80 isAlive = true;
altb2 2:e7874762cc25 81 if(old_flightmode != current_flightmode)
altb2 2:e7874762cc25 82 flightmode_changed = true;
altb2 2:e7874762cc25 83 old_flightmode = current_flightmode;
altb2 2:e7874762cc25 84
altb2 2:e7874762cc25 85 dout3.write(0);
altb2 2:e7874762cc25 86 }