AP_MSP: fix DJI FPV temperature and RPM

libraries/AP_MSP/AP_MSP.cpp

@@ -42,9 +42,9 @@ const AP_Param::GroupInfo AP_MSP::var_info[] = {
     // @Param: _OPTIONS
     // @DisplayName: MSP OSD Options
     // @Description: A bitmask to set some MSP specific options
-    // @Bitmask: 0:EnableTelemetryMode
+    // @Bitmask: 0:EnableTelemetryMode, 1: DJIWorkarounds
     // @User: Standard
-    AP_GROUPINFO("_OPTIONS", 2, AP_MSP, _options, 0),
+    AP_GROUPINFO("_OPTIONS", 2, AP_MSP, _options, OPTION_TELEMETRY_DJI_WORKAROUNDS),
 
     AP_GROUPEND
 };
@@ -149,7 +149,7 @@ void AP_MSP::init_osd()
     _osd_item_settings[OSD_HOME_DIST] = &osd->screen[0].home_dist;
     _osd_item_settings[OSD_NUMERICAL_HEADING] = &osd->screen[0].heading;
     _osd_item_settings[OSD_NUMERICAL_VARIO] = &osd->screen[0].vspeed;
-#ifdef HAVE_AP_BLHELI_SUPPORT
+#if HAL_WITH_ESC_TELEM
     _osd_item_settings[OSD_ESC_TMP] = &osd->screen[0].blh_temp;
 #endif
     _osd_item_settings[OSD_RTC_DATETIME] = &osd->screen[0].clk;

libraries/AP_MSP/AP_MSP.h

@@ -59,6 +59,7 @@ private:
 
     enum msp_option_e : uint8_t {
         OPTION_TELEMETRY_MODE = 1U<<0,
+        OPTION_TELEMETRY_DJI_WORKAROUNDS = 1U<<1
     };
 
     AP_MSP_Telem_Backend *_backends[MSP_MAX_INSTANCES];

libraries/AP_MSP/AP_MSP_Telem_DJI.cpp

@@ -104,4 +104,21 @@ MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_fc_variant(sbuf_t *dst)
     return MSP_RESULT_ACK;
 }
 
+MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_esc_sensor_data(sbuf_t *dst)
+{
+#if HAL_WITH_ESC_TELEM
+    const auto msp = AP::msp();
+    if (msp && (msp->_options & AP_MSP::OPTION_TELEMETRY_DJI_WORKAROUNDS)) {
+        AP_ESC_Telem& telem = AP::esc_telem();
+        int16_t highest_temperature = 0;
+        telem.get_highest_motor_temperature(highest_temperature);
+        sbuf_write_u8(dst, uint8_t(highest_temperature / 100));         // deg, report max temperature
+        sbuf_write_u16(dst, uint16_t(telem.get_average_motor_rpm() * 0.1f));  // rpm, report average RPM across all motors
+    } else {
+        return AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(dst);
+    }
+#endif
+    return MSP_RESULT_ACK;
+}
+
 #endif //HAL_MSP_ENABLED

libraries/AP_MSP/AP_MSP_Telem_DJI.h

@@ -57,6 +57,7 @@ public:
     MSP::MSPCommandResult msp_process_out_api_version(MSP::sbuf_t *dst) override;
     MSP::MSPCommandResult msp_process_out_fc_version(MSP::sbuf_t *dst) override;
     MSP::MSPCommandResult msp_process_out_fc_variant(MSP::sbuf_t *dst) override;
+    MSP::MSPCommandResult msp_process_out_esc_sensor_data(MSP::sbuf_t *dst) override;
 
     enum : uint8_t {
         DJI_FLAG_ARM = 0,

libraries/AP_OSD/AP_OSD_Screen.cpp

@@ -344,6 +344,7 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     // @Description: Vertical position on screen
     // @Range: 0 15
     AP_SUBGROUPINFO(vspeed, "VSPEED", 20, AP_OSD_Screen, AP_OSD_Setting),
+
 #if HAL_WITH_ESC_TELEM
     // @Param: BLHTEMP_EN
     // @DisplayName: BLHTEMP_EN