.
RCin.cpp@5:69857f8b6931, 2018-10-18 (annotated)
- Committer:
- altb
- Date:
- Thu Oct 18 12:41:15 2018 +0000
- Revision:
- 5:69857f8b6931
- Parent:
- 4:288253c4da29
- Child:
- 6:da6b1350783c
.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb | 4:288253c4da29 | 1 | #include "RCin.h" |
altb | 4:288253c4da29 | 2 | |
altb | 4:288253c4da29 | 3 | |
altb | 4:288253c4da29 | 4 | using namespace std; |
altb | 4:288253c4da29 | 5 | |
altb | 4:288253c4da29 | 6 | RCin::RCin(PinName pin) : pwm1(pin){ |
altb | 4:288253c4da29 | 7 | pwm1.fall(callback(this, &RCin::fall_edge)); |
altb | 4:288253c4da29 | 8 | pwm1.rise(callback(this, &RCin::rise_edge)); |
altb | 5:69857f8b6931 | 9 | local_ti.start(); |
altb | 5:69857f8b6931 | 10 | local_ti.reset(); |
altb | 4:288253c4da29 | 11 | } |
altb | 4:288253c4da29 | 12 | |
altb | 5:69857f8b6931 | 13 | // ***************************************************************************** |
altb | 4:288253c4da29 | 14 | void RCin::map_Channels(void){ |
altb | 4:288253c4da29 | 15 | CH1_2_PM1.setup(600.0,1400.0,-1.0,1.0); |
altb | 4:288253c4da29 | 16 | CH2_2_PM1.setup(1400.0,600.0,-1.0,1.0); |
altb | 4:288253c4da29 | 17 | CH3_2_PM1.setup(600.0,1400.0,-1.0,1.0); |
altb | 4:288253c4da29 | 18 | CH4_2_PM1.setup(600.0,1400.0,-1.0,1.0); |
altb | 4:288253c4da29 | 19 | } |
altb | 4:288253c4da29 | 20 | |
altb | 5:69857f8b6931 | 21 | // ***************************************************************************** |
altb | 4:288253c4da29 | 22 | void RCin::rise_edge(void) |
altb | 4:288253c4da29 | 23 | { |
altb | 5:69857f8b6931 | 24 | local_ti.reset(); |
altb | 4:288253c4da29 | 25 | } |
altb | 5:69857f8b6931 | 26 | |
altb | 5:69857f8b6931 | 27 | // ***************************************************************************** |
altb | 4:288253c4da29 | 28 | void RCin::fall_edge(void) |
altb | 4:288253c4da29 | 29 | { |
altb | 5:69857f8b6931 | 30 | uint32_t time_us = local_ti.read_us(); |
altb | 4:288253c4da29 | 31 | if(time_us > 2500){ |
altb | 5:69857f8b6931 | 32 | if(cou<200) |
altb | 5:69857f8b6931 | 33 | cou++; |
altb | 5:69857f8b6931 | 34 | chnr = 0; |
altb | 5:69857f8b6931 | 35 | map_pwm_2_PM1(); |
altb | 4:288253c4da29 | 36 | } |
altb | 4:288253c4da29 | 37 | else |
altb | 4:288253c4da29 | 38 | { |
altb | 4:288253c4da29 | 39 | pwms[++chnr] = time_us; |
altb | 4:288253c4da29 | 40 | test_pwms[cou][chnr-1]=time_us; |
altb | 4:288253c4da29 | 41 | } |
altb | 4:288253c4da29 | 42 | } |
altb | 5:69857f8b6931 | 43 | // ***************************************************************************** |
altb | 4:288253c4da29 | 44 | bool RCin::isAlive(void){ |
altb | 4:288253c4da29 | 45 | if(pwms[3]>550) |
altb | 4:288253c4da29 | 46 | return true; |
altb | 4:288253c4da29 | 47 | else |
altb | 4:288253c4da29 | 48 | return false; |
altb | 4:288253c4da29 | 49 | } |
altb | 4:288253c4da29 | 50 | |
altb | 5:69857f8b6931 | 51 | // ***************************************************************************** |
altb | 4:288253c4da29 | 52 | uint8_t RCin::get_flightmode(void){ |
altb | 5:69857f8b6931 | 53 | if(isAlive()){ |
altb | 5:69857f8b6931 | 54 | flightmode_changed = (old_flightmode != current_flightmode); |
altb | 5:69857f8b6931 | 55 | return current_flightmode; |
altb | 5:69857f8b6931 | 56 | } |
altb | 4:288253c4da29 | 57 | else |
altb | 4:288253c4da29 | 58 | return 0; |
altb | 4:288253c4da29 | 59 | } |
altb | 5:69857f8b6931 | 60 | // ***************************************************************************** |
altb | 5:69857f8b6931 | 61 | void RCin::map_pwm_2_PM1(void){ |
altb | 5:69857f8b6931 | 62 | PM1[1]=CH1_2_PM1((float)pwms[1]); |
altb | 5:69857f8b6931 | 63 | this->PM1[2]=this->CH2_2_PM1((float)pwms[2]); |
altb | 5:69857f8b6931 | 64 | this->PM1[3]=this->CH3_2_PM1((float)pwms[3]); |
altb | 5:69857f8b6931 | 65 | this->PM1[4]=this->CH4_2_PM1((float)pwms[4]); |
altb | 5:69857f8b6931 | 66 | if(pwms[5] >620 && pwms[5] < 720) |
altb | 5:69857f8b6931 | 67 | current_flightmode = LOITER; |
altb | 5:69857f8b6931 | 68 | else if(pwms[5] >720 && pwms[5] < 850) |
altb | 5:69857f8b6931 | 69 | current_flightmode = AUTO; |
altb | 5:69857f8b6931 | 70 | else if(pwms[5] >870 && pwms[5] < 970) |
altb | 5:69857f8b6931 | 71 | current_flightmode = RTL; |
altb | 5:69857f8b6931 | 72 | else if(pwms[5] >1000 && pwms[5] < 1100) |
altb | 5:69857f8b6931 | 73 | current_flightmode = STABILIZED; |
altb | 5:69857f8b6931 | 74 | else if(pwms[5] >1100 && pwms[5] < 1200) |
altb | 5:69857f8b6931 | 75 | current_flightmode = ACRO; |
altb | 5:69857f8b6931 | 76 | else if(pwms[5] >1200 && pwms[5] < 1350) |
altb | 5:69857f8b6931 | 77 | current_flightmode = LAND; |
altb | 5:69857f8b6931 | 78 | else |
altb | 5:69857f8b6931 | 79 | current_flightmode = STABILIZED; |
altb | 5:69857f8b6931 | 80 | } |