Diff: SBUS.cpp

Revision:

15:c96df23cad7d

Parent:

14:481c23f2e4eb

--- a/SBUS.cpp	Mon Jul 15 09:05:23 2019 +0000
+++ b/SBUS.cpp	Mon Jun 29 15:54:10 2020 +0000
@@ -2,38 +2,41 @@
 #include "SBUS.h"
 
 SBUS::SBUS(PinName tx, PinName rx) :
-com(tx, rx),
-/*stickMaximumValue(0x068F),
-stickNeutralValue(0x03FF),
-stickMinimumValue(0x016F),*/
-stickMaximumValue(0x0690),
-stickNeutralValue(0x0400),
-stickMinimumValue(0x0170),
-stickResolution(stickMaximumValue - stickMinimumValue)
+sbus(tx, rx),
+
+//CALIBRATE INPUT SIGNALS (USE getChannelValue)
+
+sbusMaximumValue(1811),
+sbusNeutralValue(992),
+sbusMinimumValue(172),
+sbusDeadband(10)
+
 {
-    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;
-    switchFPositionValue[0] = 0x076F;
-    switchFPositionValue[1] = 0x0074;
+    sbus.baud(100000);
+    sbus.attach(callback(this, &SBUS::receiveData), Serial::RxIrq);      //Z TEMLE BEREMO UART ZMER KO PRIDEJO PODATKI
+    sbus.format(8, Serial::Even, 2);
+}
+
+int SBUS::checkFailsafeTimer() {
+    failsafetime = 12;
+    if(lastreadtime < failsafetime){
+        reportreadtime = lastreadtime;
+        lastreadtime = failsafetime + 1;
+        return 1;
+    }
+    else{
+        return 0;
+    }
+}
+
+int SBUS::failSafeTimerMs() {
+    return reportreadtime;
 }
 
 float SBUS::getStickValue(int tag) {
     return stickValue[tag];
 }
 
-int SBUS::getSwitchValue(int tag) {
-    return switchValue[tag];
-}
-
 int SBUS::getChannelValue(int tag) {
     return channel[tag];
 }
@@ -41,8 +44,8 @@
 void SBUS::receiveData() {
     static int count = 0;
     char buf;
-    
-    buf = com.getc();
+        
+    buf = sbus.getc();
     if(count >= 25) return;
     receivedData[count] = buf;
     count++;
@@ -54,6 +57,10 @@
 }
 
 void SBUS::decordReceivedData() {
+    failsafetimer.stop();
+    lastreadtime = failsafetimer.read_ms();
+    failsafetimer.reset();
+    failsafetimer.start();
     channel[0]  = ((receivedData[1]    |receivedData[2]<<8)                 & 0x07FF);
     channel[1]  = ((receivedData[2]>>3 |receivedData[3]<<5)                 & 0x07FF);
     channel[2]  = ((receivedData[3]>>6 |receivedData[4]<<2 |receivedData[5]<<10)  & 0x07FF);
@@ -74,45 +81,22 @@
 }
 
 void SBUS::convertReceivedData() {
-    for(int i = 0; i < 4; i++) {
+    for(int i = 0; i < 16; i++) {
         float buf;
-        if(channel[i] > (stickNeutralValue + 20)) buf = ((channel[i] - stickNeutralValue) / (float)(stickMaximumValue - stickNeutralValue));
-        else if(channel[i] < (stickNeutralValue - 20)) buf = -((channel[i] - stickNeutralValue) / (float)(stickMinimumValue - stickNeutralValue));
+        if(channel[i] > (sbusNeutralValue + sbusDeadband)){                                                           //THIS IS MIDDLE DEADBAND (DEAD ZONE)
+            buf = ((float)(channel[i] - sbusNeutralValue) / (float)(sbusMaximumValue - sbusNeutralValue)); 
+        }   
+        else if(channel[i] < (sbusNeutralValue - sbusDeadband)){                                                      //THIS IS MIDDLE DEADBAND (DEAD ZONE)
+            buf = -((float)(channel[i] - sbusNeutralValue) / (float)(sbusMinimumValue - sbusNeutralValue));
+        }
         else buf = 0.0f;
-        buf = (int)(buf*100)/100.0f;
-        buf = (buf > 1.0f) ? 1.0f : buf;
-        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;
+        buf = (int)(buf*100)/100.0f;             //Limit output resolution to 2 decimals (x.yz)           
+        if(buf > 1.0f){                  //Limit to -1.0 to 1.0
+            buf = 1.0f;
         }
-    }
-    for(int i = 0; i < 12; i++) {
-        switch(channel[i + 4]) {
-            case 0x078b:
-            case 0x770:
-                switchValue[i] = High;
-                break;
-            case 0x0400:
-                switchValue[i] = Neutral;
-                break;
-            case 0x0090:
-                switchValue[i] = Low;
-                break;
-            default:
-                //switchValue[i] = 3;
-                break;
+        if(buf < -1.0f){
+            buf = -1.0f;
         }
+        stickValue[i] = buf;
     }
 }