AP_TemperatureSensor: Support servo telem sources

Add support for updating motor and PCB temperatures in servo telemetry
from temperature sensors.

Author: rob-clarke

libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp

@@ -20,6 +20,7 @@
 
 #include <AP_Logger/AP_Logger.h>
 #include <AP_BattMonitor/AP_BattMonitor.h>
+#include <AP_Servo_Telem/AP_Servo_Telem.h>
 
 /*
   All backends use the same parameter table and set of indices. Therefore, two
@@ -85,6 +86,10 @@ void AP_TemperatureSensor_Backend::update_external_libraries(const float tempera
 #if HAL_WITH_ESC_TELEM
     AP_ESC_Telem_Backend::TelemetryData t;
 #endif
+#if AP_SERVO_TELEM_ENABLED
+    AP_Servo_Telem *servo_telem;
+    AP_Servo_Telem::TelemetryData servo_telem_data;
+#endif
 
     switch ((AP_TemperatureSensor_Params::Source)_params.source.get()) {
 #if HAL_WITH_ESC_TELEM
@@ -111,6 +116,28 @@ void AP_TemperatureSensor_Backend::update_external_libraries(const float tempera
             // Label only, used by AP_Periph
             break;
 
+#if AP_SERVO_TELEM_ENABLED
+        case AP_TemperatureSensor_Params::Source::Servo_Motor:
+            servo_telem = AP_Servo_Telem::get_singleton();
+            if (servo_telem == nullptr) {
+                break;
+            }
+            servo_telem_data.motor_temperature_cdeg = temperature * 100;
+            servo_telem_data.present_types = AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP;
+            servo_telem->update_telem_data(_params.source_id-1, servo_telem_data);
+            break;
+
+        case AP_TemperatureSensor_Params::Source::Servo_PCB:
+            servo_telem = AP_Servo_Telem::get_singleton();
+            if (servo_telem == nullptr) {
+                break;
+            }
+            servo_telem_data.pcb_temperature_cdeg = temperature * 100;
+            servo_telem_data.present_types = AP_Servo_Telem::TelemetryData::Types::PCB_TEMP;
+            servo_telem->update_telem_data(_params.source_id-1, servo_telem_data);
+            break;
+#endif // AP_SERVO_TELEM_ENABLED
+
         case AP_TemperatureSensor_Params::Source::None:
         case AP_TemperatureSensor_Params::Source::Pitot_tube:
         default:

libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp

@@ -58,7 +58,7 @@ const AP_Param::GroupInfo AP_TemperatureSensor_Params::var_info[] = {
     // @Param: SRC
     // @DisplayName: Sensor Source
     // @Description: Sensor Source is used to designate which device's temperature report will be replaced by this temperature sensor's data. If 0 (None) then the data is only available via log. In the future a new Motor temperature report will be created for returning data directly.
-    // @Values: 0: None, 1:ESC, 2:Motor, 3:Battery Index, 4:Battery ID/SerialNumber, 5:CAN based Pitot tube, 6:DroneCAN-out on AP_Periph
+    // @Values: 0: None, 1:ESC, 2:Motor, 3:Battery Index, 4:Battery ID/SerialNumber, 5:CAN based Pitot tube, 6:DroneCAN-out on AP_Periph, 7:Servo motor, 8:Servo PCB
     // @User: Standard
     AP_GROUPINFO("SRC", 4, AP_TemperatureSensor_Params, source, (float)Source::None),
 

libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h

@@ -48,6 +48,8 @@ class AP_TemperatureSensor_Params {
         Battery_ID_SerialNumber     = 4,
         Pitot_tube                  = 5,
         DroneCAN                    = 6,
+        Servo_Motor                 = 7,
+        Servo_PCB                   = 8,
     };
 
     AP_Enum<Type> type;             // 0=disabled, others see frontend enum TYPE