AP_Periph: Send generic equipment temperature data

Tools/AP_Periph/AP_Periph.h

@@ -352,6 +352,9 @@ class AP_Periph_FW {
 
 #if AP_TEMPERATURE_SENSOR_ENABLED
     AP_TemperatureSensor temperature_sensor;
+    void temperature_sensor_update();
+    uint32_t temperature_last_send_ms;
+    uint8_t temperature_last_sent_index;
 #endif
 
 #if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY)

Tools/AP_Periph/Parameters.cpp

@@ -650,6 +650,16 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(relay,                 "RELAY", AP_Relay),
 #endif
 
+#if AP_TEMPERATURE_SENSOR_ENABLED
+    // @Param: TEMP_MAX_RATE
+    // @DisplayName: Temprature Sensor max rate
+    // @Description: This is the maximum rate we send Temprature Sensor data in Hz. Zero means no send.
+    // @Units: Hz
+    // @Range: 0 200
+    // @Increment: 1
+    GSCALAR(temperature_max_rate, "TEMP_MAX_RATE", 0),
+#endif
+
     AP_VAREND
 };
 

Tools/AP_Periph/Parameters.h

@@ -90,6 +90,7 @@ class Parameters {
         k_param_can_terminate2,
         k_param_serial_options,
         k_param_relay,
+        k_param_temperature_max_rate,
     };
 
     AP_Int16 format_version;
@@ -203,7 +204,11 @@ class Parameters {
 #if HAL_PERIPH_CAN_MIRROR
     AP_Int8 can_mirror_ports;
 #endif // HAL_PERIPH_CAN_MIRROR
-
+
+#if AP_TEMPERATURE_SENSOR_ENABLED
+    AP_Int16 temperature_max_rate;
+#endif
+
 #if HAL_CANFD_SUPPORTED
     AP_Int8 can_fdmode;
     AP_Int8 can_fdbaudrate[HAL_NUM_CAN_IFACES];

Tools/AP_Periph/can.cpp

@@ -1820,6 +1820,9 @@ void AP_Periph_FW::can_update()
     #ifdef HAL_PERIPH_ENABLE_EFI
         can_efi_update();
     #endif
+#if AP_TEMPERATURE_SENSOR_ENABLED
+        temperature_sensor_update();
+#endif
     }
     const uint32_t now_us = AP_HAL::micros();
     while ((AP_HAL::micros() - now_us) < 1000) {

Tools/AP_Periph/temperature.cpp

@@ -0,0 +1,54 @@
+#include "AP_Periph.h"
+
+#if AP_TEMPERATURE_SENSOR_ENABLED
+
+#include <dronecan_msgs.h>
+
+// Send temperature message occasionally
+void AP_Periph_FW::temperature_sensor_update(void)
+{
+    if (g.temperature_max_rate <= 0) {
+        return;
+    }
+
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - temperature_last_send_ms < (1000U / g.temperature_max_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_temperature(temp_deg, index) ||
+                (temperature_sensor.get_source(index) != AP_TemperatureSensor_Params::Source::DroneCAN)) {
+                // Unhealthy or not configured to send
+                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 // AP_TEMPERATURE_SENSOR_ENABLED