From 459fec940f520d71edfb62436b138e3500cbc0a5 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 24 Jun 2024 23:58:13 +0200
Subject: [PATCH] Update frame check

---
 src/main/rx/sbus.c | 31 +++++++++++++++----------------
 src/main/rx/sbus.h |  2 +-
 2 files changed, 16 insertions(+), 17 deletions(-)

diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c
index 8984be2abbe..8a5c3f926a1 100644
--- a/src/main/rx/sbus.c
+++ b/src/main/rx/sbus.c
@@ -66,7 +66,7 @@ typedef struct sbusFrameData_s {
     timeUs_t lastActivityTimeUs;
 } sbusFrameData_t;
 
-static uint8_t sbus2ActiveTelemetryFrame = 0;
+static uint8_t sbus2ActiveTelemetryPage = 0;
 timeUs_t frameTime = 0;
 
 // Receive ISR callback
@@ -101,19 +101,18 @@ static void sbusDataReceive(uint16_t c, void *data)
                 // Do some sanity check
                 switch (frame->endByte) {
                     case 0x00:  // This is S.BUS 1
-                    case 0x04:  // S.BUS 2 receiver voltage
-                        sbus2ActiveTelemetryFrame = 0;
-                        goto process_end_frame;
-                    case 0x14:  // S.BUS 2 GPS/baro
-                        sbus2ActiveTelemetryFrame = 1;
-                        goto process_end_frame;
-                    case 0x24:  // Unknown SBUS2 data
-                        sbus2ActiveTelemetryFrame = 2;
-                        goto process_end_frame;
-                    case 0x34:  // Unknown SBUS2 data
-                        sbus2ActiveTelemetryFrame = 3;
-                    process_end_frame:
-                        frameTime = currentTimeUs;
+                    case 0x04:  // S.BUS 2 telemetry page 1
+                    case 0x14:  // S.BUS 2 telemetry page 2
+                    case 0x24:  // S.BUS 2 telemetry page 3
+                    case 0x34:  // S.BUS 2 telemetry page 4
+                        if(frame->endByte & 0x4) {
+                            sbus2ActiveTelemetryPage = (frame->endByte >> 4) & 0xF;
+                            frameTime = currentTimeUs;
+                        } else {
+                            sbus2ActiveTelemetryPage = 0;
+                            frameTime = -1;
+                        }
+
 
                         frameValid = true;
                         sbusFrameData->state = STATE_SBUS_WAIT_SYNC;
@@ -223,8 +222,8 @@ uint8_t sbusGetLastFrameTime(void) {
     return frameTime;
 }
 
-uint8_t sbusGetCurrentTelemetryFrame(void) {
-    return sbus2ActiveTelemetryFrame;
+uint8_t sbusGetCurrentTelemetryPage(void) {
+    return sbus2ActiveTelemetryPage;
 }
 #endif // USE_TELEMETRY && USE_SBUS2_TELEMETRY
 
diff --git a/src/main/rx/sbus.h b/src/main/rx/sbus.h
index f698eecb8eb..94af636a43f 100644
--- a/src/main/rx/sbus.h
+++ b/src/main/rx/sbus.h
@@ -23,6 +23,6 @@ bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeCon
 bool sbusInitFast(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
 
 #ifdef USE_SBUS2_TELEMETRY
-uint8_t sbusGetCurrentTelemetryFrame(void);
+uint8_t sbusGetCurrentTelemetryPage(void);
 uint8_t sbusGetLastFrameTime(void);
 #endif