Setup serial port for bi direction, half duplex operation

iNavFlight · Jun 24, 2024 · 4c56952 · 4c56952
1 parent bd7f488
commit 4c56952
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 5 deletions.
diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c
@@ -66,6 +66,9 @@ typedef struct sbusFrameData_s {
     timeUs_t lastActivityTimeUs;
 } sbusFrameData_t;
 
+static uint8_t sbus2ActiveTelemetryFrame = 0;
+timeUs_t frameTime = 0;
+
 // Receive ISR callback
 static void sbusDataReceive(uint16_t c, void *data)
 {
@@ -99,9 +102,19 @@ static void sbusDataReceive(uint16_t c, void *data)
                 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;
+
                         frameValid = true;
                         sbusFrameData->state = STATE_SBUS_WAIT_SYNC;
                         break;
@@ -179,9 +192,10 @@ static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeC
         sbusDataReceive,
         &sbusFrameData,
         sbusBaudRate,
-        portShared ? MODE_RXTX : MODE_RX,
+        (portShared || rxConfig->serialrx_provider == SERIALRX_SBUS2) ? MODE_RXTX : MODE_RX,
         SBUS_PORT_OPTIONS |
             (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) |
+            ((rxConfig->serialrx_provider == SERIALRX_SBUS2) ? SERIAL_BIDIR : 0) |
             (tristateWithDefaultOffIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
         );
 
@@ -203,4 +217,15 @@ bool sbusInitFast(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
 {
     return sbusInitEx(rxConfig, rxRuntimeConfig, SBUS_BAUDRATE_FAST);
 }
-#endif
+
+#if defined(USE_TELEMETRY) && defined(USE_SBUS2_TELEMETRY)
+uint8_t sbusGetLastFrameTime(void) {
+    return frameTime;
+}
+
+uint8_t sbusGetCurrentTelemetryFrame(void) {
+    return sbus2ActiveTelemetryFrame;
+}
+#endif // USE_TELEMETRY && USE_SBUS2_TELEMETRY
+
+#endif // USE_SERIAL_RX
diff --git a/src/main/rx/sbus.h b/src/main/rx/sbus.h
@@ -23,5 +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();
+uint8_t sbusGetCurrentTelemetryFrame(void);
+uint8_t sbusGetLastFrameTime(void);
 #endif
diff --git a/src/main/telemetry/sbus2.h b/src/main/telemetry/sbus2.h
@@ -24,12 +24,12 @@
 
 #define SBUS2_TELEMETRY_PAYLOAD_SIZE 3
 
-#define SBUS2_SLOT_COUNT            32
 #define SBUS2_TELEMETRY_ITEM_SIZE   3
 #define SBUS2_TELEMETRY_SLOTS       8
 #define SBUS2_TELEMETRY_PAGES       4
+#define SBUS2_SLOT_COUNT            (SBUS2_TELEMETRY_PAGES * SBUS2_TELEMETRY_SLOTS)
 
-#ifdef USE_SBUS2_TELEMETRY
+#if defined(USE_TELEMETRY) && defined(USE_SBUS2_TELEMETRY)
 
 // Information on SBUS2 sensors from: https://github.com/BrushlessPower/SBUS2-Telemetry/tree/master
 // Temperature: