diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 76baa4d41eb..f87b7f079c7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1674,6 +1674,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } } break; + + case MSP2_INAV_ESC_TELEM: + { + uint8_t motorCount = getMotorCount(); + sbufWriteU8(dst, motorCount); + + for (uint8_t i = 0; i < motorCount; i++){ + const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry + sbufWriteDataSafe(dst, escState, sizeof(escSensorData_t)); + } + } + break; #endif #ifdef USE_EZ_TUNE diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 02c8c979aae..465f5098536 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -89,6 +89,7 @@ #define MSP2_INAV_LOGIC_CONDITIONS_SINGLE 0x203B #define MSP2_INAV_ESC_RPM 0x2040 +#define MSP2_INAV_ESC_TELEM 0x2041 #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049