diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index aae0ec773c..8869842cb8 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -108,6 +108,7 @@ #include "sensors/gyro.h" #include "sensors/sensors.h" #include "sensors/sonar.h" +#include "sensors/esc_sensor.h" #include "telemetry/telemetry.h" @@ -930,6 +931,16 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) break; #endif +#ifdef USE_DSHOT + case MSP_ESC_SENSOR_DATA: + sbufWriteU8(dst, getMotorCount()); + for (int i = 0; i < getMotorCount(); i++) { + sbufWriteU8(dst, getEscSensorData(i)->temperature); + sbufWriteU16(dst, getEscSensorData(i)->rpm); + } + break; +#endif + #ifdef GPS case MSP_GPS_CONFIG: sbufWriteU8(dst, gpsConfig()->provider); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index fbcc14ee4f..e74709330d 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -271,6 +271,7 @@ #define MSP_MOTOR_CONFIG 131 //out message Motor configuration (min/max throttle, etc) #define MSP_GPS_CONFIG 132 //out message GPS configuration #define MSP_COMPASS_CONFIG 133 //out message Compass configuration +#define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM) #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed