From 3f4755ea7bb533a2a47bf7c5c49e8f7955d34a9b Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tom.pittenger@krausaerospace.com>
Date: Fri, 2 Sep 2022 14:41:38 -0700
Subject: [PATCH] AP_Periph: add support for AP_TemperatureSensor

---
 Tools/AP_Periph/AP_Periph.cpp  | 8 ++++++++
 Tools/AP_Periph/AP_Periph.h    | 4 ++++
 Tools/AP_Periph/Parameters.cpp | 6 ++++++
 Tools/AP_Periph/Parameters.h   | 1 +
 Tools/AP_Periph/can.cpp        | 4 +++-
 Tools/AP_Periph/wscript        | 1 +
 6 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index 4d228cdab3..0e2efab410 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -228,6 +228,10 @@ void AP_Periph_FW::init()
     }
 #endif
     
+#if AP_TEMPERATURE_SENSOR_ENABLED
+    temperature_sensor.init();
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_NOTIFY
     notify.init();
 #endif
@@ -425,6 +429,10 @@ void AP_Periph_FW::update()
 #endif
     }
 
+#if AP_TEMPERATURE_SENSOR_ENABLED
+    temperature_sensor.update();
+#endif
+
 #if HAL_LOGGING_ENABLED
     logger.periodic_tasks();
 #endif
diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index ccc3d0f171..e5c56c8847 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -14,6 +14,7 @@
 #include <AP_EFI/AP_EFI.h>
 #include <AP_MSP/AP_MSP.h>
 #include <AP_MSP/msp.h>
+#include <AP_TemperatureSensor/AP_TemperatureSensor.h>
 #include "../AP_Bootloader/app_comms.h"
 #include <AP_CheckFirmware/AP_CheckFirmware.h>
 #include "hwing_esc.h"
@@ -230,6 +231,9 @@ public:
     void rcout_handle_safety_state(uint8_t safety_state);
 #endif
 
+#if AP_TEMPERATURE_SENSOR_ENABLED
+    AP_TemperatureSensor temperature_sensor;
+#endif
 
 #if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY)
     void update_rainbow();
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index 61e585f6ea..a7eb3962b8 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -396,6 +396,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
 #endif
 #endif
 
+#if AP_TEMPERATURE_SENSOR_ENABLED
+    // @Group: TEMP
+    // @Path: ../libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp
+    GOBJECT(temperature_sensor,         "TEMP",     AP_TemperatureSensor),
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_MSP
     // @Param: MSP_PORT
     // @DisplayName: MSP Serial Port
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 792c314dc2..5c9de5d642 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -61,6 +61,7 @@ public:
         k_param_efi_baudrate,
         k_param_esc_telem_rate,
         k_param_can_slcan_cport,
+        k_param_temperature_sensor,
     };
 
     AP_Int16 format_version;
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index 73661fde35..1968321342 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1651,7 +1651,9 @@ void AP_Periph_FW::esc_telem_update()
             pkt.current = nan;
         }
         int16_t temperature;
-        if (esc_telem.get_temperature(i, temperature)) {
+        if (esc_telem.get_motor_temperature(i, temperature)) {
+            pkt.temperature = C_TO_KELVIN(temperature*0.01);
+        } else if (esc_telem.get_temperature(i, temperature)) {
             pkt.temperature = C_TO_KELVIN(temperature*0.01);
         } else {
             pkt.temperature = nan;
diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript
index a0919b2ee4..6d298b73b7 100644
--- a/Tools/AP_Periph/wscript
+++ b/Tools/AP_Periph/wscript
@@ -52,6 +52,7 @@ def build(bld):
                      'AP_Airspeed',
                      'AP_RangeFinder',
                      'AP_ROMFS',
+                     'AP_TemperatureSensor',
                      'AP_MSP',
                      'SRV_Channel',
                      'AP_Notify',