Diff: RCin.cpp

Revision:

5:69857f8b6931

Parent:

4:288253c4da29

Child:

6:da6b1350783c

--- a/RCin.cpp	Wed Oct 17 13:02:19 2018 +0000
+++ b/RCin.cpp	Thu Oct 18 12:41:15 2018 +0000
@@ -6,28 +6,11 @@
 RCin::RCin(PinName pin) : pwm1(pin){
     pwm1.fall(callback(this, &RCin::fall_edge));
     pwm1.rise(callback(this, &RCin::rise_edge));
-    loc_ti.start();
-    loc_ti.reset();
+    local_ti.start();
+    local_ti.reset();
     }
 
-uint8_t RCin::map_CH5_2_Flightmode(uint16_t pwm){
-    if(pwm >620 && pwm < 720)
-        return LOITER;
-    else if(pwm >720 && pwm < 850)
-        return AUTO;
-    else if(pwm >870 && pwm < 970)
-        return RTL;
-    else if(pwm >1000 && pwm < 1100)
-        return STABILIZED;
-    else if(pwm >1100 && pwm < 1200)
-        return ACRO;
-    else if(pwm >1200 && pwm < 1350)
-        return LAND;
-    else
-        return STABILIZED;
-}
-
-
+// *****************************************************************************
 void RCin::map_Channels(void){
     CH1_2_PM1.setup(600.0,1400.0,-1.0,1.0);
     CH2_2_PM1.setup(1400.0,600.0,-1.0,1.0);
@@ -35,39 +18,29 @@
     CH4_2_PM1.setup(600.0,1400.0,-1.0,1.0);
     }
 
+// *****************************************************************************
 void RCin::rise_edge(void)
     {
-     loc_ti.reset();
+     local_ti.reset();
      }
+
+// *****************************************************************************
 void RCin::fall_edge(void)
    {
-        uint32_t time_us = loc_ti.read_us();
+        uint32_t time_us = local_ti.read_us();
         if(time_us > 2500){
-        if(cou<200)
-            cou++;
-        chnr = 0;
+            if(cou<200)
+                cou++;
+            chnr = 0;
+            map_pwm_2_PM1();
         }
         else
         {
             pwms[++chnr] = time_us;
             test_pwms[cou][chnr-1]=time_us;
-            switch(chnr){
-                case 1:
-                    this->PM1[1]=this->CH1_2_PM1((float)pwms[1]);
-                    break;
-                case 2:
-                    this->PM1[2]=this->CH2_2_PM1((float)pwms[2]);
-                    break;
-                case 3:
-                    this->PM1[3]=this->CH3_2_PM1((float)pwms[3]);
-                    break;
-                case 4:
-                    this->PM1[4]=this->CH4_2_PM1((float)pwms[4]);
-                    break;
-                default: break;
-                }
         }
     }
+// *****************************************************************************
 bool RCin::isAlive(void){
         if(pwms[3]>550)
             return true;
@@ -75,13 +48,33 @@
             return false;
 }
 
-
+// *****************************************************************************
 uint8_t RCin::get_flightmode(void){
-    if(isAlive())
-        return map_CH5_2_Flightmode(pwms[5]);
+    if(isAlive()){
+        flightmode_changed = (old_flightmode != current_flightmode);
+        return current_flightmode;
+        }
     else 
         return 0;
     }
-
-
-    
+// *****************************************************************************
+void RCin::map_pwm_2_PM1(void){
+    PM1[1]=CH1_2_PM1((float)pwms[1]);
+    this->PM1[2]=this->CH2_2_PM1((float)pwms[2]);
+    this->PM1[3]=this->CH3_2_PM1((float)pwms[3]);
+    this->PM1[4]=this->CH4_2_PM1((float)pwms[4]);
+    if(pwms[5] >620 && pwms[5] < 720)
+        current_flightmode = LOITER;
+    else if(pwms[5] >720 && pwms[5] < 850)
+        current_flightmode = AUTO;
+    else if(pwms[5] >870 && pwms[5] < 970)
+        current_flightmode = RTL;
+    else if(pwms[5] >1000 && pwms[5] < 1100)
+        current_flightmode = STABILIZED;
+    else if(pwms[5] >1100 && pwms[5] < 1200)
+        current_flightmode = ACRO;
+    else if(pwms[5] >1200 && pwms[5] < 1350)
+        current_flightmode = LAND;
+    else
+        current_flightmode = STABILIZED;
+    }
\ No newline at end of file