Diff: SBUS.cpp

Revision:

1:a1307d47b1b8

Parent:

0:e2bc011508c7

Child:

2:493d10424466

diff -r e2bc011508c7 -r a1307d47b1b8 SBUS.cpp
--- a/SBUS.cpp	Fri Jun 28 07:34:58 2019 +0000
+++ b/SBUS.cpp	Fri Jun 28 07:59:32 2019 +0000
@@ -66,9 +66,26 @@
 }
 
 void SBUS::convertReceivedData() {
-    stickValue[analog_rx] = 1.0f - ((channel[right_RL] - stickMinimumValue) / (float)stickResolution);
-    stickValue[analog_ry] = 1.0f - ((channel[right_UD] - stickMinimumValue) / (float)stickResolution);
-    stickValue[analog_lx] = 1.0f - ((channel[left_RL] - stickMinimumValue) / (float)stickResolution);
-    stickValue[analog_ly] = (channel[left_UD] - stickMinimumValue) / (float)stickResolution;
+    for(int i = 0; i < 4; i++) {
+        float buf;
+        if(channel[i] > stickNeutralValue) buf = ((channel[i] - stickNeutralValue) / (float)(stickMaximumValue - stickNeutralValue));
+        else buf = -((channel[i] - stickNeutralValue) / (float)(stickMinimumValue - stickNeutralValue));
+        switch(i) {
+            case right_RL:
+                stickValue[analog_rx] = -buf;
+                break;
+            case right_UD:
+                stickValue[analog_ry] = -buf;
+                break;
+            case left_RL:
+                stickValue[analog_lx] = -buf;
+                break;
+            case left_UD:
+                stickValue[analog_ly] = buf;
+                break;
+            default:
+                break;
+        }
+    }
     for(int i = 0; i < 12; i++) switchValue[i] = ((channel[i + 3] == switchPositionValue[High]) ? 1 : 0);
 }