From f665f9898a7573c1dc1d1ddbd83944ef6b9adeed Mon Sep 17 00:00:00 2001
From: wimalopaan <wilhelm.wm.meier@googlemail.com>
Date: Sat, 26 Mar 2022 06:57:06 +0100
Subject: [PATCH] fixed silly mistakes decoding CRSF

---
 radio/src/crsf.cpp                            | 28 +++++++++++++------
 radio/src/crsf.h                              |  1 +
 .../common/arm/stm32/timers_driver.cpp        |  2 ++
 3 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/radio/src/crsf.cpp b/radio/src/crsf.cpp
index 577ca5f046f..512e1f0e106 100644
--- 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
index e0db325e1ac..d535a18a99d 100644
--- 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
index c3f644984ac..0a07c38efaf 100644
--- 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