Revision 15:c96df23cad7d, committed 2020-06-29

Comitter:

sllez

Date:

Mon Jun 29 15:54:10 2020 +0000

Parent:

14:481c23f2e4eb

Commit message:

koncno

Changed in this revision

diff -r 481c23f2e4eb -r c96df23cad7d SBUS.cpp
--- 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;
     }
 }

diff -r 481c23f2e4eb -r c96df23cad7d SBUS.h
--- a/SBUS.h	Mon Jul 15 09:05:23 2019 +0000
+++ b/SBUS.h	Mon Jun 29 15:54:10 2020 +0000
@@ -2,6 +2,7 @@
 #define INCLUDED_SBUS_H
 
 #include "mbed.h"
+#include "BufferedSerial.h"
 
 class SBUS {
     public:
@@ -9,62 +10,44 @@
         //          rx  communocation rx pin
         SBUS(PinName tx, PinName rx);
         
-        // channel array elements tag
-        enum channelTag { right_RL, left_UD, right_UD, left_RL, sw1, sw2, sw3, sw4, sw5, sw6 };
-        
-        // stick value array elements tag
-        enum stickName {
-            analog_rx,
-            analog_ry,
-            analog_lx,
-            analog_ly
-        };
-        
-        // switch position tag
-        enum switchPosition {
-            Low,
-            Neutral,
-            High
-        };
-        
-        // switchF position tag
-        enum switchFPosition {
-            FHigh,
-            FLow
-        };
-        
         // function of getting stick value
         // @parm    tag stickName
         float getStickValue(int tag);
         
-        // function of getting switch value
-        // @parm    tag 0-12
-        int getSwitchValue(int tag);
-        
         // function of getting channel value
         // @parm    tag 0-25
         int getChannelValue(int tag);
+        
+        // checks failsafe timer, returns 1 if data was received in last 100ms, 
+        // 0 if there was no data in last 100ms
+        int checkFailsafeTimer();
+        
+        int failSafeTimerMs();
+        
+        Timer failsafetimer;
     
     private:
-        Serial com;
+        Serial sbus;
+        //BufferedSerial sbus;
         
-        int stickMaximumValue;
-        int stickNeutralValue;
-        int stickMinimumValue;
-        int stickResolution;
-        
+        int sbusMaximumValue;
+        int sbusNeutralValue;
+        int sbusMinimumValue;
+        int sbusDeadband;
+
         int channel[16];
         int receivedData[50];
-        float stickValue[4];
-        int switchFPositionValue[2];
-        int switchPositionValue[3];
-        int switchValue[12];
+        float stickValue[16];
         
         void receiveData();
         
         void decordReceivedData();
         
         void convertReceivedData();
+        
+        int failsafetime;
+        int lastreadtime;
+        int reportreadtime;
 };
 
 #endif