Diff: RCin.cpp

Revision:

0:3312872854c4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RCin.cpp	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,76 @@
+#include "RCin.h"
+
+
+using namespace std;
+
+RCin::RCin(PinName pin) : pwm1(pin){
+    pwm1.fall(callback(this, &RCin::fall_edge));
+    pwm1.rise(callback(this, &RCin::rise_edge));
+    local_ti.start();
+    local_ti.reset();
+    }
+
+// *****************************************************************************
+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);
+    CH3_2_PM1.setup(600.0,1400.0,-1.0,1.0);
+    CH4_2_PM1.setup(600.0,1400.0,-1.0,1.0);
+    }
+
+// *****************************************************************************
+void RCin::rise_edge(void)
+    {
+     local_ti.reset();
+     }
+
+// *****************************************************************************
+void RCin::fall_edge(void)
+   {
+        uint32_t time_us = local_ti.read_us();
+        if(time_us > 2500){
+            if(cou<200)
+                cou++;
+            chnr = 0;
+            map_pwm_2_PM1();
+        }
+        else
+        {
+            pwms[++chnr] = time_us;
+            test_pwms[cou][chnr-1]=time_us;
+        }
+    }
+// *****************************************************************************
+uint8_t RCin::get_flightmode(void){
+    if(isAlive){
+        flightmode_changed = (old_flightmode != current_flightmode);
+        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]);
+    PM1[2]=CH2_2_PM1((float)pwms[2]);
+    PM1[3]=CH3_2_PM1((float)pwms[3]);
+    PM1[4]=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;
+    if(pwms[3]>550)
+        isAlive = true;
+
+    }
\ No newline at end of file