Ghost MSP support

betaflight · Mar 11, 2022 · feefabe · feefabe
1 parent 427a275
commit feefabe
Show file tree

Hide file tree

Showing 7 changed files with 83 additions and 4 deletions.
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
@@ -95,6 +95,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "D_LPF",
     "VTX_TRAMP",
     "GHST",
+    "GHST_MSP",
     "SCHEDULER_DETERMINISM",
     "TIMING_ACCURACY",
     "RX_EXPRESSLRS_SPI",

diff --git a/src/main/build/debug.h b/src/main/build/debug.h
@@ -93,6 +93,7 @@ typedef enum {
     DEBUG_D_LPF,
     DEBUG_VTX_TRAMP,
     DEBUG_GHST,
+    DEBUG_GHST_MSP,
     DEBUG_SCHEDULER_DETERMINISM,
     DEBUG_TIMING_ACCURACY,
     DEBUG_RX_EXPRESSLRS_SPI,

diff --git a/src/main/rx/ghst.c b/src/main/rx/ghst.c
@@ -129,6 +129,11 @@ void ghstRxSendTelemetryData(void)
     }
 }
 
+uint8_t ghstRxGetTelemetryBufLen(void)
+{
+    return telemetryBufLen;
+}
+
 STATIC_UNIT_TESTED uint8_t ghstFrameCRC(const ghstFrame_t *const pGhstFrame)
 {
     // CRC includes type and payload
@@ -331,6 +336,24 @@ static bool ghstProcessFrame(const rxRuntimeState_t *rxRuntimeState)
                 }
 
             }
+        } else {
+            switch(ghstFrameType) {
+
+#if defined(USE_TELEMETRY_GHST) && defined(USE_MSP_OVER_TELEMETRY)
+                case GHST_UL_MSP_REQ:
+                case GHST_UL_MSP_WRITE: {
+                    static uint8_t mspFrameCounter = 0;
+                    DEBUG_SET(DEBUG_GHST_MSP, 0, ++mspFrameCounter);
+                    if (handleMspFrame(ghstValidatedFrame->frame.payload, ghstValidatedFrame->frame.len - GHST_FRAME_LENGTH_CRC - GHST_FRAME_LENGTH_TYPE, NULL)) {
+                        ghstScheduleMspResponse();
+                    }
+                    break;
+                }
+#endif
+                default:
+                    DEBUG_SET(DEBUG_GHST, DEBUG_GHST_UNKNOWN_FRAMES, ++unknownFrameCount);
+                    break;
+            }
         }
     }
 

diff --git a/src/main/rx/ghst.h b/src/main/rx/ghst.h
@@ -28,6 +28,7 @@ struct rxConfig_s;
 struct rxRuntimeState_s;
 
 void ghstRxWriteTelemetryData(const void *const data, const int len);
+uint8_t ghstRxGetTelemetryBufLen(void);
 void ghstRxSendTelemetryData(void);
 
 bool ghstRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeState_s *rxRuntimeState);

diff --git a/src/main/rx/ghst_protocol.h b/src/main/rx/ghst_protocol.h
@@ -63,6 +63,9 @@ typedef enum {
     GHST_UL_RC_CHANS_HS4_12_RSSI   = 0x33,  // 12 bit primary 4 channel, plus RSSI, LQ, RF Mode, and Tx Power
     GHST_UL_RC_CHANS_HS4_12_LAST   = 0x3f,  // 12 bit last frame type
 
+    // MSP commands
+    GHST_UL_MSP_REQ                = 0x21,  // response request using msp sequence as command
+    GHST_UL_MSP_WRITE              = 0x22,  // write
 } ghstUl_e;
 
 typedef enum {
@@ -72,7 +75,8 @@ typedef enum {
     GHST_DL_PACK_STAT           = 0x23,     // Battery (Pack) Status
     GHST_DL_GPS_PRIMARY         = 0x25,     // Primary GPS data (position)
     GHST_DL_GPS_SECONDARY       = 0x26,
-    GHST_DL_MAGBARO             = 0x27
+    GHST_DL_MAGBARO             = 0x27,
+    GHST_DL_MSP_RESP            = 0x28,     // reply
 } ghstDl_e;
 
 typedef enum {

diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c
@@ -29,6 +29,7 @@
 #include "build/atomic.h"
 #include "build/build_config.h"
 #include "build/version.h"
+#include "build/debug.h"
 
 #include "config/feature.h"
 #include "pg/pg.h"
@@ -69,8 +70,11 @@
 #define GHST_FRAME_PACK_PAYLOAD_SIZE        10
 #define GHST_FRAME_GPS_PAYLOAD_SIZE         10
 #define GHST_FRAME_MAGBARO_PAYLOAD_SIZE     10
-#define GHST_FRAME_LENGTH_CRC               1
-#define GHST_FRAME_LENGTH_TYPE              1
+
+#define GHST_MSP_BUFFER_SIZE    96
+#define GHST_UL_MSP_FRAME_SIZE  10
+#define GHST_DL_MSP_FRAME_SIZE   6
+#define GHST_MSP_LENGTH_OFFSET   1
 
 static bool ghstTelemetryEnabled;
 static uint8_t ghstFrame[GHST_FRAME_SIZE];
@@ -213,6 +217,32 @@ typedef enum {
 static uint8_t ghstScheduleCount;
 static uint8_t ghstSchedule[GHST_SCHEDULE_COUNT_MAX];
 
+static bool mspReplyPending;
+
+void ghstScheduleMspResponse()
+{
+    mspReplyPending = true;
+}
+
+static void ghstSendMspResponse(uint8_t *payload, const uint8_t payloadSize)
+{
+    sbuf_t ghstPayloadBuf;
+    sbuf_t *dst = &ghstPayloadBuf;
+
+    static uint8_t mspFrameCounter = 0;
+    DEBUG_SET(DEBUG_GHST_MSP, 1, ++mspFrameCounter);
+
+    ghstInitializeFrame(dst);                                                           // addr
+    sbufWriteU8(dst, GHST_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);     // lenght
+    sbufWriteU8(dst, GHST_DL_MSP_RESP);                                                 // type
+    sbufWriteData(dst, payload, payloadSize);                                           // payload
+    for(int i = 0; i < GHST_PAYLOAD_SIZE - payloadSize; ++i)                            // payload fill zeroes
+    {
+        sbufWriteU8(dst, 0);
+    }
+    ghstFinalize(dst);                                                                  // crc
+}
+
 static void processGhst(void)
 {
     static uint8_t ghstScheduleIndex = 0;
@@ -260,6 +290,9 @@ void initGhstTelemetry(void)
     }
 
     ghstTelemetryEnabled = false;
+#if defined(USE_MSP_OVER_TELEMETRY)
+    mspReplyPending = false;
+#endif
 
     int index = 0;
     if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
@@ -301,14 +334,25 @@ bool checkGhstTelemetryState(void)
 }
 
 // Called periodically by the scheduler
- void handleGhstTelemetry(timeUs_t currentTimeUs)
+void handleGhstTelemetry(timeUs_t currentTimeUs)
 {
     static timeUs_t ghstLastCycleTime;
 
     if (!ghstTelemetryEnabled) {
         return;
     }
 
+    // Send ad-hoc response frames as soon as possible
+#if defined(USE_MSP_OVER_TELEMETRY)
+    if (mspReplyPending) {
+        ghstLastCycleTime = currentTimeUs;
+        if (ghstRxGetTelemetryBufLen() == 0) {
+            mspReplyPending = sendMspReply(GHST_DL_MSP_FRAME_SIZE, ghstSendMspResponse);
+        }
+        return;
+    }
+#endif
+
     // Ready to send telemetry?
     if (currentTimeUs >= ghstLastCycleTime + (GHST_CYCLETIME_US / ghstScheduleCount)) {
         ghstLastCycleTime = currentTimeUs;

diff --git a/src/main/telemetry/ghst.h b/src/main/telemetry/ghst.h
@@ -26,8 +26,13 @@
 #include "common/time.h"
 
 #include "rx/ghst_protocol.h"
+#include "telemetry/msp_shared.h"
 
 void initGhstTelemetry(void);
 bool checkGhstTelemetryState(void);
 void setGhstTelemetryState(bool state);
 void handleGhstTelemetry(timeUs_t currentTimeUs);
+
+#if defined(USE_MSP_OVER_TELEMETRY)
+void ghstScheduleMspResponse();
+#endif