From 82ea67e9f15d428ef38c3d79163a29b0cecf846c Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@outlook.com>
Date: Mon, 3 Feb 2025 20:02:18 +0100
Subject: [PATCH] Potential fix to MSP over CRSF

---
 src/main/rx/crsf.c        | 2 +-
 src/main/telemetry/crsf.c | 7 +++++--
 src/main/telemetry/crsf.h | 2 +-
 3 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
index a791b2e9dd..7fed0a24e6 100755
--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@@ -175,7 +175,7 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData)
                         case CRSF_FRAMETYPE_MSP_WRITE: {
                             uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
                             if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
-                                crsfScheduleMspResponse();
+                                crsfScheduleMspResponse(crsfFrame.frame.payload[1]);
                             }
                             break;
                         }
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 8b7f289db2..05870e5f8a 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -424,10 +424,13 @@ static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
 #if defined(USE_MSP_OVER_TELEMETRY)
 
 static bool mspReplyPending;
+//Id of the last receiver MSP frame over CRSF. Needed to send response with correct frame ID
+static uint8_t mspRequestOriginID = 0;
 
-void crsfScheduleMspResponse(void)
+void crsfScheduleMspResponse(uint8_t requestOriginID)
 {
     mspReplyPending = true;
+    mspRequestOriginID = requestOriginID;
 }
 
 void crsfSendMspResponse(uint8_t *payload)
@@ -438,7 +441,7 @@ void crsfSendMspResponse(uint8_t *payload)
     crsfInitializeFrame(dst);
     sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
     crsfSerialize8(dst, CRSF_FRAMETYPE_MSP_RESP);
-    crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+    crsfSerialize8(dst, mspRequestOriginID);
     crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
     crsfSerializeData(dst, (const uint8_t*)payload, CRSF_FRAME_TX_MSP_FRAME_SIZE);
     crsfFinalize(dst);
diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h
index 6ffa4df358..bfe9b9e4f3 100644
--- a/src/main/telemetry/crsf.h
+++ b/src/main/telemetry/crsf.h
@@ -27,7 +27,7 @@ void initCrsfTelemetry(void);
 bool checkCrsfTelemetryState(void);
 void handleCrsfTelemetry(timeUs_t currentTimeUs);
 void crsfScheduleDeviceInfoResponse(void);
-void crsfScheduleMspResponse(void);
+void crsfScheduleMspResponse(uint8_t requestOriginID);
 int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
 #if defined(USE_MSP_OVER_TELEMETRY)
 void initCrsfMspBuffer(void);