From feefabec588ab5779688d9fc429dd14998bb5c47 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C5=A0t=C4=9Bp=C3=A1n=20Daleck=C3=BD?= Date: Tue, 4 Jan 2022 18:38:26 +0100 Subject: [PATCH] Ghost MSP support --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/rx/ghst.c | 23 +++++++++++++++++ src/main/rx/ghst.h | 1 + src/main/rx/ghst_protocol.h | 6 ++++- src/main/telemetry/ghst.c | 50 ++++++++++++++++++++++++++++++++++--- src/main/telemetry/ghst.h | 5 ++++ 7 files changed, 83 insertions(+), 4 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index d3ae5fb3f47..5bea77fec01 100644 --- 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 index 8494550257a..cdac4bef4a4 100644 --- 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 index edb9492e2e4..c04069e2ffc 100644 --- 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 index 48644b3abe9..e3f14e6099d 100644 --- 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 index e6e69da714e..9e656b23b03 100644 --- 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 index 762325c8bc7..a80db5bdfed 100644 --- 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,7 +334,7 @@ bool checkGhstTelemetryState(void) } // Called periodically by the scheduler - void handleGhstTelemetry(timeUs_t currentTimeUs) +void handleGhstTelemetry(timeUs_t currentTimeUs) { static timeUs_t ghstLastCycleTime; @@ -309,6 +342,17 @@ bool checkGhstTelemetryState(void) 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 index 61239a5316e..38a727d1eca 100644 --- 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 \ No newline at end of file