Plugin some of the telemetry functions.

Basic squeleton in place. Need to update telemetry data and avoid sending telemetry slot twice.
iNavFlight · Jun 24, 2024 · a3a129e · a3a129e
1 parent 459fec9
commit a3a129e
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 6 deletions.
diff --git a/src/main/rx/sbus.h b/src/main/rx/sbus.h
@@ -19,6 +19,8 @@
 
 #define SBUS_DEFAULT_INTERFRAME_DELAY_US    3000    // According to FrSky interframe is 6.67ms, we go smaller just in case
 
+#include "rx/rx.h"
+
 bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
 bool sbusInitFast(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
 

diff --git a/src/main/telemetry/sbus2.c b/src/main/telemetry/sbus2.c
@@ -21,20 +21,44 @@
 
 #include "build/debug.h"
 
-#include "sbus2.h"
+#include "common/time.h"
+
+#include "telemetry/sbus2.h"
+
+#include "rx/sbus.h"
 
 #ifdef USE_SBUS2_TELEMETRY
 
-const uint8_t Slot_ID[SBUS2_SLOT_COUNT] = {
-    0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3,
-    0x13, 0x93, 0x53, 0xD3, 0x33, 0xB3, 0x73, 0xF3,
-    0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB,
-    0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB
+const uint8_t sbus2SlotIds[SBUS2_TELEMETRY_PAGES][SBUS2_TELEMETRY_SLOTS] = {
+    {0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3},
+    {0x13, 0x93, 0x53, 0xD3, 0x33, 0xB3, 0x73, 0xF3},
+    {0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB},
+    {0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB}
 };
 
 
 sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_TELEMETRY_PAGES][SBUS2_TELEMETRY_SLOTS] = {{}};
 uint8_t sbusTelemetryDataStatus[SBUS2_TELEMETRY_PAGES][SBUS2_TELEMETRY_SLOTS] = {{}};
 
+void handleSbus2Telemetry(timeUs_t currentTimeUs) 
+{
+    uint8_t telemetryPage = sbusGetCurrentTelemetryPage();
+    uint8_t lastFrame = sbusGetLastFrameTime();
+
+    timeUs_t elapsedTime = currentTimeUs - lastFrame - MS2US(2);
+
+    // 2ms after sbus2 frame = slot 0
+    // +660us for next slot
+    if(elapsedTime > 0) {
+        uint8_t slot = elapsedTime % 660;
+        if(slot < SBUS2_TELEMETRY_SLOTS) {
+            if(sbusTelemetryDataStatus[telemetryPage][slot] != 0) {
+                sbusTelemetryData[telemetryPage][slot].slotId = sbus2SlotIds[telemetryPage][slot];
+                // send 
+                sbusTelemetryDataStatus[telemetryPage][slot] = 0;
+            }
+        }
+    }
+}
 
 #endif
diff --git a/src/main/telemetry/sbus2.h b/src/main/telemetry/sbus2.h
@@ -108,4 +108,5 @@ extern const uint8_t Slot_ID[SBUS2_SLOT_COUNT];
 extern sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_TELEMETRY_PAGES][SBUS2_TELEMETRY_SLOTS];
 extern uint8_t sbusTelemetryDataStatus[SBUS2_TELEMETRY_PAGES][SBUS2_TELEMETRY_SLOTS];
 
+void handleSbus2Telemetry(timeUs_t currentTimeUs);
 #endif
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
@@ -245,6 +245,10 @@ void telemetryProcess(timeUs_t currentTimeUs)
 #ifdef USE_TELEMETRY_GHST
     handleGhstTelemetry(currentTimeUs);
 #endif
+
+#ifdef USE_TELMETRY_SBUS2
+    handleSbus2Telemetry(currentTimeUs);
+#endif
 }
 
 #endif