Diff: SBUS.cpp

Revision:

4:da8728f3d289

Parent:

2:493d10424466

Child:

5:1836bd646986

--- a/SBUS.cpp	Fri Jun 28 08:43:53 2019 +0000
+++ b/SBUS.cpp	Sat Jun 29 02:33:56 2019 +0000
@@ -3,14 +3,22 @@
 
 SBUS::SBUS(PinName tx, PinName rx) :
 com(tx, rx),
-stickMaximumValue(0x068F),
+/*stickMaximumValue(0x068F),
 stickNeutralValue(0x03FF),
-stickMinimumValue(0x016F),
+stickMinimumValue(0x016F),*/
+stickMaximumValue(0x0690),
+stickNeutralValue(0x0400),
+stickMinimumValue(0x0170),
 stickResolution(stickMaximumValue - stickMinimumValue)
 {
     com.baud(100000);
     com.attach(this, &SBUS::receiveData, Serial::RxIrq);
     com.format(8, Serial::Even, 2);
+    /*switchPositionValue[0] = 0x076F;
+    switchPositionValue[1] = 0x03FF;
+    switchPositionValue[2] = 0x008F;
+    switchFPositionValue[0] = 0x076F;
+    switchFPositionValue[1] = 0x0074;*/
     switchPositionValue[0] = 0x076F;
     switchPositionValue[1] = 0x03FF;
     switchPositionValue[2] = 0x008F;
@@ -34,9 +42,9 @@
     static int count = 0;
     char buf;
     
-    buf = ~com.getc();
+    buf = com.getc();
     
-    if(buf == 0xF0) {
+    if(buf == 0x0F) {
         decordReceivedData();
         count = 0;
     }
@@ -72,16 +80,16 @@
         else buf = -((channel[i] - stickNeutralValue) / (float)(stickMinimumValue - stickNeutralValue));
         switch(i) {
             case right_RL:
-                stickValue[analog_rx] = -buf;
+                stickValue[analog_rx] = buf;
                 break;
             case right_UD:
-                stickValue[analog_ry] = -buf;
+                stickValue[analog_ry] = buf;
                 break;
             case left_RL:
-                stickValue[analog_lx] = -buf;
+                stickValue[analog_lx] = buf;
                 break;
             case left_UD:
-                stickValue[analog_ly] = buf;
+                stickValue[analog_ly] = -buf;
                 break;
             default:
                 break;
@@ -89,18 +97,18 @@
     }
     for(int i = 0; i < 12; i++) {
         switch(channel[i + 4]) {
-            case 0x008F:
-            case 0x0074:
+            case 0x078b:
+            case 0x770:
                 switchValue[i] = High;
                 break;
-            case 0x03FF:
+            case 0x0400:
                 switchValue[i] = Neutral;
                 break;
-            case 0x076F:
+            case 0x0090:
                 switchValue[i] = Low;
                 break;
             default:
-                switchValue[i] = channel[i + 3] / 1903.0f;
+                switchValue[i] = channel[i + 3] / 1904.0f;
                 break;
         }
     }