- refine SBUS RX code (http://www.multiwii.com/forum/viewtopic.php?f=…

…8&t=4425&start=30#p45299)

git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii_shared@1648 02679b44-d973-9852-f2fa-63770883b36c

MultiWii.cpp

@@ -265,9 +265,10 @@ uint8_t rcSerialCount = 0;   // a counter to select legacy RX when there is no m
 int16_t lookupPitchRollRC[5];// lookup table for expo & RC rate PITCH+ROLL
 int16_t lookupThrottleRC[11];// lookup table for expo & mid THROTTLE
 
-#if defined(SPEKTRUM)
+#if defined(SPEKTRUM) || defined(SBUS)
   volatile uint8_t  spekFrameFlags;
   volatile uint32_t spekTimeLast;
+  uint8_t  spekFrameDone;
 #endif
 
 #if defined(OPENLRSv2MULTI)
@@ -774,12 +775,21 @@ void loop () {
   #if defined(SPEKTRUM)
     if (spekFrameFlags == 0x01) readSpektrum();
   #endif
-
+
+  #if defined(SBUS)
+    if (spekFrameFlags == 0x01) readSBus();
+  #endif
+
   #if defined(OPENLRSv2MULTI) 
     Read_OpenLRS_RC();
   #endif 
 
+  #if defined(SPEKTRUM) || defined(SBUS)
+  if ((spekFrameDone == 0x01) || ((int16_t)(currentTime-rcTime) >0 )) { 
+    spekFrameDone = 0x00;
+  #else
   if ((int16_t)(currentTime-rcTime) >0 ) { // 50Hz
+  #endif
     rcTime = currentTime + 20000;
     computeRC();
     // Failsafe routine - added by MIS

MultiWii.h

@@ -172,6 +172,7 @@ extern int16_t lookupThrottleRC[11];
 
   extern volatile uint8_t  spekFrameFlags;
   extern volatile uint32_t spekTimeLast;
+  extern uint8_t  spekFrameDone;
 
   #if defined(OPENLRSv2MULTI)
     extern uint8_t pot_P,pot_I; // OpenLRS onboard potentiometers for P and I trim or other usages

RX.cpp

@@ -300,6 +300,7 @@ void configureReceiver() {
 #if defined(SBUS)
 void  readSBus(){
   #define SBUS_SYNCBYTE 0x0F // Not 100% sure: at the beginning of coding it was 0xF0 !!!
+  #define SBUS_ENDBYTE 0x00
   static uint16_t sbus[25]={0};
   while(SerialAvailable(SBUS_SERIAL_PORT)){
     int val = SerialRead(SBUS_SERIAL_PORT);
@@ -308,6 +309,8 @@ void  readSBus(){
     sbus[sbusIndex++] = val;
     if(sbusIndex==25){
       sbusIndex=0;
+      spekFrameFlags = 0x00;
+      if (sbus[24] != SBUS_ENDBYTE) continue; 
       rcValue[0]  = ((sbus[1]|sbus[2]<< 8) & 0x07FF)/2+SBUS_MID_OFFSET;
       rcValue[1]  = ((sbus[2]>>3|sbus[3]<<5) & 0x07FF)/2+SBUS_MID_OFFSET; 
       rcValue[2]  = ((sbus[3]>>6|sbus[4]<<2|sbus[5]<<10) & 0x07FF)/2+SBUS_MID_OFFSET; 
@@ -328,6 +331,7 @@ void  readSBus(){
       // now the two Digital-Channels
       if ((sbus[23]) & 0x0001)       rcValue[16] = 2000; else rcValue[16] = 1000;
       if ((sbus[23] >> 1) & 0x0001)  rcValue[17] = 2000; else rcValue[17] = 1000;
+      spekFrameDone = 0x01;
 
       // Failsafe: there is one Bit in the SBUS-protocol (Byte 25, Bit 4) whitch is the failsafe-indicator-bit
       #if defined(FAILSAFE)
@@ -369,6 +373,7 @@ void readSpektrum(void) {
         if (spekChannel < RC_CHANS) rcValue[spekChannel] = 988 + ((((uint16_t)(bh & SPEK_CHAN_MASK) << 8) + bl) SPEK_DATA_SHIFT);
       }
       spekFrameFlags = 0x00;
+      spekFrameDone = 0x01;
       #if defined(FAILSAFE)
         if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0;   // Valid frame, clear FailSafe counter
       #endif
@@ -382,8 +387,7 @@ void readSpektrum(void) {
 
 uint16_t readRawRC(uint8_t chan) {
   uint16_t data;
-  #if defined(SPEKTRUM)
-    readSpektrum();
+  #if defined(SPEKTRUM) || defined(SBUS)
     if (chan < RC_CHANS) {
       data = rcValue[rcChannel[chan]];
     } else data = 1500;
@@ -404,9 +408,6 @@ void computeRC() {
   static uint8_t rc4ValuesIndex = 0;
   uint8_t chan,a;
   #if !defined(OPENLRSv2MULTI) // dont know if this is right here
-    #if defined(SBUS)
-      readSBus();
-    #endif
     rc4ValuesIndex++;
     if (rc4ValuesIndex == 4) rc4ValuesIndex = 0;
     for (chan = 0; chan < RC_CHANS; chan++) {

RX.h

@@ -5,6 +5,7 @@ void configureReceiver();
 void computeRC();
 uint16_t readRawRC(uint8_t chan);
 void readSpektrum(void);
+void readSBus(void);
 #if defined(OPENLRSv2MULTI)
   void initOpenLRS(void);
   void Read_OpenLRS_RC(void);

Serial.cpp

@@ -153,6 +153,11 @@ void SerialEnd(uint8_t port) {
 void store_uart_in_buf(uint8_t data, uint8_t portnum) {
   #if defined(SPEKTRUM)
     if (portnum == SPEK_SERIAL_PORT) {
+  #endif
+  #if defined(SBUS) 
+    if (portnum == SBUS_SERIAL_PORT) {
+  #endif
+  #if defined(SPEKTRUM) || defined(SBUS)
       if (!spekFrameFlags) { 
         sei();
         uint32_t spekTimeNow = (timer0_overflow_count << 8) * (64 / clockCyclesPerMicrosecond()); //Move timer0_overflow_count into registers so we don't touch a volatile twice