fixed silly mistakes decoding CRSF

EdgeTX · Mar 26, 2022 · f665f98 · f665f98
1 parent cf8ebbc
commit f665f98
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 9 deletions.
diff --git a/radio/src/crsf.cpp b/radio/src/crsf.cpp
@@ -63,30 +63,29 @@ namespace CRSF {
                 break;
             case State::Data:
                 csum += b;
-                if (++mIndex == mLength) {
+                if (++mIndex >= mLength) {
                     mState = State::AwaitCRC;
                 }
                 break;
             case State::Channels:
                 csum += b;
+                mData[mIndex] = b;
+                if (++mIndex >= mLength) {
+                    mState = State::AwaitCRCAndDecode;
+                }
                 break;
             case State::AwaitCRC:
                 if (csum == b) {
-                    mState = State::Undefined;
+                    // only channel data is decoded, nothing todo here
                 } 
-                else {
-                    mState = State::Undefined;
-                }
+                mState = State::Undefined;
                 break;
             case State::AwaitCRCAndDecode:
                 if (csum == b) {
                     ++mPackages;
                     f();
-                    mState = State::Undefined;
                 } 
-                else {
-                    mState = State::Undefined;
-                }
+                mState = State::Undefined;
                 break;
             }            
         }        
@@ -132,6 +131,17 @@ namespace CRSF {
     uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms
 }
 
+void crsfTrainerPauseCheck() {
+#if !defined(SIMU)
+# if defined(AUX_SERIAL) || defined(AUX2_SERIAL) || defined(TRAINER_MODULE_SBUS)
+    if (hasSerialMode(UART_MODE_CRSF_TRAINER) >= 0) {
+        CRSF::Servo::tick1ms();
+        processCrsfInput();    
+    }
+# endif
+#endif    
+}
+
 void processCrsfInput() {
 #if !defined(SIMU)
   uint8_t rxchar;

diff --git a/radio/src/crsf.h b/radio/src/crsf.h
@@ -7,4 +7,5 @@
 #define CRSF_BAUDRATE         420000
 
 void processCrsfInput();
+void crsfTrainerPauseCheck();
 
diff --git a/radio/src/targets/common/arm/stm32/timers_driver.cpp b/radio/src/targets/common/arm/stm32/timers_driver.cpp
@@ -65,8 +65,10 @@ static void interrupt1ms()
     flysky_hall_stick_loop();
   }
 #endif
+
 #if defined(SBUS_TRAINER)
   sbusTrainerPauseCheck();
+  crsfTrainerPauseCheck();
 #endif
 
   // 5ms loop
Original file line number	Diff line number	Diff line change
Expand Up		@@ -7,4 +7,5 @@
		#define CRSF_BAUDRATE 420000

		void processCrsfInput();
		void crsfTrainerPauseCheck();