Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 03:19:58 +03:00
Code Issues Wiki Activity

Potential fix to MSP over CRSF

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2025-02-03 20:02:18 +01:00
parent f00cd9ee37
commit 82ea67e9f1
3 changed files with 7 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/rx/crsf.c
View file

@ -175,7 +175,7 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData)
case CRSF_FRAMETYPE_MSP_WRITE: { case CRSF_FRAMETYPE_MSP_WRITE: {
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) { if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
crsfScheduleMspResponse(); crsfScheduleMspResponse(crsfFrame.frame.payload[1]);
} }
break; break;
} }

7
src/main/telemetry/crsf.c
View file

@ -424,10 +424,13 @@ static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
#if defined(USE_MSP_OVER_TELEMETRY) #if defined(USE_MSP_OVER_TELEMETRY)
static bool mspReplyPending; 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; mspReplyPending = true;
mspRequestOriginID = requestOriginID;
} }
void crsfSendMspResponse(uint8_t *payload) void crsfSendMspResponse(uint8_t *payload)
@ -438,7 +441,7 @@ void crsfSendMspResponse(uint8_t *payload)
crsfInitializeFrame(dst); crsfInitializeFrame(dst);
sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_MSP_RESP); crsfSerialize8(dst, CRSF_FRAMETYPE_MSP_RESP);
crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); crsfSerialize8(dst, mspRequestOriginID);
crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
crsfSerializeData(dst, (const uint8_t*)payload, CRSF_FRAME_TX_MSP_FRAME_SIZE); crsfSerializeData(dst, (const uint8_t*)payload, CRSF_FRAME_TX_MSP_FRAME_SIZE);
crsfFinalize(dst); crsfFinalize(dst);

2
src/main/telemetry/crsf.h
View file

@ -27,7 +27,7 @@ void initCrsfTelemetry(void);
bool checkCrsfTelemetryState(void); bool checkCrsfTelemetryState(void);
void handleCrsfTelemetry(timeUs_t currentTimeUs); void handleCrsfTelemetry(timeUs_t currentTimeUs);
void crsfScheduleDeviceInfoResponse(void); void crsfScheduleDeviceInfoResponse(void);
void crsfScheduleMspResponse(void); void crsfScheduleMspResponse(uint8_t requestOriginID);
int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
#if defined(USE_MSP_OVER_TELEMETRY) #if defined(USE_MSP_OVER_TELEMETRY)
void initCrsfMspBuffer(void); void initCrsfMspBuffer(void);