First attempt at transmitting telemetry

May need to break out the osciloscope/logic analizer soon.
iNavFlight · Jul 10, 2024 · 6a3a0d1 · 6a3a0d1
1 parent d01e762
commit 6a3a0d1
Show file tree

Hide file tree

Showing 6 changed files with 45 additions and 18 deletions.
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
@@ -78,6 +78,7 @@ typedef enum {
     DEBUG_HEADTRACKING,
     DEBUG_GPS,
     DEBUG_LULU,
+    DEBUG_SBUS2,
     DEBUG_COUNT // also update debugModeNames in cli.c
 } debugType_e;
 

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
@@ -211,7 +211,9 @@ static const char *debugModeNames[DEBUG_COUNT] = {
     "POS_EST",
     "ADAPTIVE_FILTER",
     "HEADTRACKER",
-    "GPS"
+    "GPS",
+    "LULU",
+    "SBUS2"
 };
 
 /* Sensor names (used in lookup tables for *_hardware settings and in status

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -84,7 +84,7 @@ tables:
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
       "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", 
-      "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU"]
+      "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2"]
   - name: aux_operator
     values: ["OR", "AND"]
     enum: modeActivationOperator_e

diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c
@@ -199,7 +199,7 @@ static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeC
         );
 
 #ifdef USE_TELEMETRY
-    if (portShared) {
+    if (portShared || (rxConfig->serialrx_provider == SERIALRX_SBUS2)) {
         telemetrySharedPort = sBusPort;
     }
 #endif

diff --git a/src/main/telemetry/sbus2.c b/src/main/telemetry/sbus2.c
@@ -24,45 +24,67 @@
 #include "common/utils.h"
 #include "common/time.h"
 
+#include "telemetry/telemetry.h"
 #include "telemetry/sbus2.h"
 
 #include "rx/sbus.h"
 
 #ifdef USE_SBUS2_TELEMETRY
 
-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}
+const uint8_t sbus2SlotIds[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
 };
 
 
-sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_TELEMETRY_PAGES][SBUS2_TELEMETRY_SLOTS] = {{}};
-uint8_t sbusTelemetryDataStatus[SBUS2_TELEMETRY_PAGES][SBUS2_TELEMETRY_SLOTS] = {{}};
+sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_SLOT_COUNT] = {};
+uint8_t sbusTelemetryDataUsed[SBUS2_SLOT_COUNT] = {};
+timeUs_t sbusTelemetryDataLastSent[SBUS2_SLOT_COUNT] = {};
 
 void handleSbus2Telemetry(timeUs_t currentTimeUs) 
 {
     UNUSED(currentTimeUs);
 
+    // TODO: placeholder
+
+    int16_t temp = 42 | 0x4000;
+    sbusTelemetryData[0].payload.temp125.tempHigh = ((temp >> 8) & 0xFF);
+    sbusTelemetryData[0].payload.temp125.tempLow = (temp & 0xFF);
+    sbusTelemetryDataUsed[0] = 1;
+
+    int16_t rpm = 9000;
+    sbusTelemetryData[1].payload.rpm.rpmHigh = (rpm >> 8) & 0xFF;
+    sbusTelemetryData[1].payload.rpm.rpmLow = rpm & 0xFF;
+    sbusTelemetryDataUsed[1] = 1;
+    // P1S0: TEMP
+    // P1S1: RPM
     // update telemetry info
 }
 
 void taskSendSbus2Telemetry(timeUs_t currentTimeUs)
 {
+    if(!telemetrySharedPort || rxConfig()->receiverType != RX_TYPE_SERIAL || rxConfig()->serialrx_provider != SERIALRX_SBUS2)
+    {
+        return;
+    }
+
     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) {
+    if(elapsedTime > MS2US(2)) {
         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;
+        int slotIndex = (telemetryPage * SBUS2_TELEMETRY_SLOTS) + slot;
+        if(slot < SBUS2_TELEMETRY_SLOTS && slotIndex < SBUS2_SLOT_COUNT && (currentTimeUs - sbusTelemetryDataLastSent[slotIndex]) > MS2US(2)) {
+            if(sbusTelemetryDataUsed[slotIndex] != 0) {
+                sbusTelemetryData[slotIndex].slotId = sbus2SlotIds[slotIndex];
+                // send
+                serialWriteBuf(telemetrySharedPort, (const uint8_t *)&sbusTelemetryData[slotIndex], sizeof(sbus2_telemetry_frame_t));
+                sbusTelemetryDataLastSent[slotIndex] = currentTimeUs;
             }
         }
     }

diff --git a/src/main/telemetry/sbus2.h b/src/main/telemetry/sbus2.h
@@ -27,6 +27,7 @@
 #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)
 
 #if defined(USE_TELEMETRY) && defined(USE_SBUS2_TELEMETRY)
@@ -105,8 +106,9 @@ typedef struct sbus2_telemetry_frame_s {
 } __attribute__((packed)) sbus2_telemetry_frame_t;
 
 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];
+extern sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_SLOT_COUNT];
+extern uint8_t sbusTelemetryDataUsed[SBUS2_SLOT_COUNT];
+extern timeUs_t sbusTelemetryDataLastSent[SBUS2_SLOT_COUNT];
 
 // refresh telemetry buffers 
 void handleSbus2Telemetry(timeUs_t currentTimeUs);