Diff: AR6210.cpp

Revision:

1:25aad26619fb

Parent:

0:9a1f7660704d

--- a/AR6210.cpp	Wed Nov 20 10:26:54 2013 +0000
+++ b/AR6210.cpp	Sun Nov 24 09:46:45 2013 +0000
@@ -4,10 +4,10 @@
 // Modified by Joram Querner for Spektrum AR6210
 
 AR6210::AR6210() :
-  ChInt0(InterruptIn(p6)), // throttle
-  ChInt1(InterruptIn(p8)), // aileron
+  ChInt0(InterruptIn(p5)), // throttle
+  ChInt1(InterruptIn(p6)), // aileron
   ChInt2(InterruptIn(p7)), // elevator
-  ChInt3(InterruptIn(p5)), // rudder
+  ChInt3(InterruptIn(p8)), // rudder
   ChInt4(InterruptIn(p9)), // gear
   ChInt5(InterruptIn(p10)) // aux
 {}
@@ -37,12 +37,22 @@
     for(int i= 0; i<6; i++)
         RawChannels[i]= dTime[i];
     
-    //Steuerbefehle berechnen
-    Throttle= float(dTime[0]-1000) * 0.001;
-    Aileron= float(dTime[1]-1500) * 0.002;
-    Elevator= float(dTime[2]-1500) * 0.002;
-    Rudder= float(dTime[3]-1500) * 0.002;
+    Throttle= map(dTime[0], 1095, 1910, 0.0, 1.0);
+    Aileron= map(dTime[1], 1110, 1900, -1.0, 1.0);
+    Elevator= map(dTime[2], 1100, 1900, -1.0, 1.0);
+    Rudder= map(dTime[3], 1105, 1895, -1.0, 1.0);
     
-    Gear= float(dTime[4]-1500) * 0.002;
-    Aux= float(dTime[5]-1500) * 0.002;
+    Gear= map(dTime[4], 1095, 1910, 0.0, 1.0);
+    Aux= map(dTime[5], 1095, 1910, 0.0, 1.0);
+}
+
+float AR6210::map(int x, int in_min, int in_max, float out_min, float out_max)
+{
+    float returnValue = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+    if (returnValue < out_min)
+        return out_min;
+    if (returnValue > out_max)
+        return out_max;
+
+      return returnValue;
 }
\ No newline at end of file