From 26bfedb956cf6d86e92cd68f4b7d7c0107d45937 Mon Sep 17 00:00:00 2001
From: Gone4Dirt <matthewnkear@gmail.com>
Date: Fri, 12 Jan 2024 02:51:48 +0000
Subject: [PATCH] AP_Periph: Send generic equipment temperature data

---
 Tools/AP_Periph/AP_Periph.h     |  9 ++++++
 Tools/AP_Periph/Parameters.cpp  | 11 +++++++
 Tools/AP_Periph/Parameters.h    |  7 ++++-
 Tools/AP_Periph/can.cpp         |  3 ++
 Tools/AP_Periph/temperature.cpp | 54 +++++++++++++++++++++++++++++++++
 5 files changed, 83 insertions(+), 1 deletion(-)
 create mode 100644 Tools/AP_Periph/temperature.cpp

diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index 991ddf2b23..424e341c1c 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -55,6 +55,10 @@
 #endif
 #endif
 
+#if defined(HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE) && !AP_TEMPERATURE_SENSOR_ENABLED
+    #error "HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE requires AP_TEMPERATURE_SENSOR_ENABLED"
+#endif
+
 #include <AP_NMEA_Output/AP_NMEA_Output.h>
 #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS))
     // Needs SerialManager + (AHRS or GPS)
@@ -356,6 +360,11 @@ public:
     
 #if AP_TEMPERATURE_SENSOR_ENABLED
     AP_TemperatureSensor temperature_sensor;
+#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE
+    void temperature_sensor_update();
+    uint32_t temperature_last_send_ms;
+    uint8_t temperature_last_sent_index;
+#endif
 #endif
 
 #if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY)
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index d425ad6c7c..3b00ce8baf 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -650,6 +650,17 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(relay,                 "RELAY", AP_Relay),
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE
+    // @Param: TEMP_MSG_RATE
+    // @DisplayName: Temperature sensor message rate
+    // @Description: This is the rate Temperature sensor data is sent in Hz. Zero means no send. Each sensor with source DroneCAN is sent in turn.
+    // @Units: Hz
+    // @Range: 0 200
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(temperature_msg_rate, "TEMP_MSG_RATE", 0),
+#endif
+
     AP_VAREND
 };
 
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 884f1c4e64..94ac49b39a 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -90,6 +90,7 @@ public:
         k_param_can_terminate2,
         k_param_serial_options,
         k_param_relay,
+        k_param_temperature_msg_rate,
     };
 
     AP_Int16 format_version;
@@ -203,7 +204,11 @@ public:
 #if HAL_PERIPH_CAN_MIRROR
     AP_Int8 can_mirror_ports;
 #endif // HAL_PERIPH_CAN_MIRROR
-    
+
+#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE
+    AP_Int8 temperature_msg_rate;
+#endif
+
 #if HAL_CANFD_SUPPORTED
     AP_Int8 can_fdmode;
     AP_Int8 can_fdbaudrate[HAL_NUM_CAN_IFACES];
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index 3bd020cd2f..1c658463a6 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1827,6 +1827,9 @@ void AP_Periph_FW::can_update()
     #ifdef HAL_PERIPH_ENABLE_EFI
         can_efi_update();
     #endif
+#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE
+        temperature_sensor_update();
+#endif
     }
     const uint32_t now_us = AP_HAL::micros();
     while ((AP_HAL::micros() - now_us) < 1000) {
diff --git a/Tools/AP_Periph/temperature.cpp b/Tools/AP_Periph/temperature.cpp
new file mode 100644
index 0000000000..3377e2b635
--- /dev/null
+++ b/Tools/AP_Periph/temperature.cpp
@@ -0,0 +1,54 @@
+#include "AP_Periph.h"
+
+#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE
+
+#include <dronecan_msgs.h>
+
+// Send temperature message occasionally
+void AP_Periph_FW::temperature_sensor_update(void)
+{
+    if (g.temperature_msg_rate <= 0) {
+        return;
+    }
+
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - temperature_last_send_ms < (1000U / g.temperature_msg_rate)) {
+        return;
+    }
+    temperature_last_send_ms = now_ms;
+
+    {
+        const uint8_t num_sensors = temperature_sensor.num_instances();
+        for (uint8_t i = 0; i < num_sensors; i++) {
+            // Send each sensor in turn
+            const uint8_t index = (temperature_last_sent_index + 1 + i) % num_sensors;
+
+            float temp_deg = 0.0;
+            if ((temperature_sensor.get_source(index) != AP_TemperatureSensor_Params::Source::DroneCAN) ||
+                !temperature_sensor.get_temperature(temp_deg, index)) {
+                // not configured to send or Unhealthy
+                continue;
+            }
+
+            uavcan_equipment_device_Temperature pkt {};
+            pkt.temperature = C_TO_KELVIN(temp_deg);
+
+            // Use source ID from temperature lib
+            pkt.device_id = temperature_sensor.get_source_id(index);
+
+            uint8_t buffer[UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_MAX_SIZE] {};
+            const uint16_t total_size = uavcan_equipment_device_Temperature_encode(&pkt, buffer, !canfdout());
+
+            canard_broadcast(UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE,
+                             UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID,
+                             CANARD_TRANSFER_PRIORITY_LOW,
+                             &buffer[0],
+                             total_size);
+
+            temperature_last_sent_index = index;
+            break;
+        }
+    }
+}
+
+#endif // HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE