Diff: SBUS.cpp

Revision:

15:55548ca76fc4

Parent:

14:481c23f2e4eb

diff -r 481c23f2e4eb -r 55548ca76fc4 SBUS.cpp
--- a/SBUS.cpp	Mon Jul 15 09:05:23 2019 +0000
+++ b/SBUS.cpp	Mon Dec 07 07:41:34 2020 +0000
@@ -24,6 +24,7 @@
     switchPositionValue[2] = 0x008F;
     switchFPositionValue[0] = 0x076F;
     switchFPositionValue[1] = 0x0074;
+    connstatus = false;
 }
 
 float SBUS::getStickValue(int tag) {
@@ -38,6 +39,10 @@
     return channel[tag];
 }
 
+bool SBUS::getConnectStatus() {
+    return connstatus;
+}
+
 void SBUS::receiveData() {
     static int count = 0;
     char buf;
@@ -50,6 +55,10 @@
     if(count == 25 && receivedData[0] == 0x0F) {
         decordReceivedData();
         count = 0;
+        connstatus = true;
+    } else {
+        resetall();
+        connstatus = false;
     }
 }
 
@@ -116,3 +125,10 @@
         }
     }
 }
+
+void SBUS::resetall() {
+    for(int i = 0; i < 16; i++) channel[i] = 0;
+    for(int i = 0; i < 50; i++) receivedData[i] = 0;
+    for(int i = 0; i < 4; i++) stickValue[i] = 0.0f;
+    for(int i = 0; i < 12; i++) switchValue[i] = Neutral;
+}