From 09de840c8e833559dce2b3ca505a6ab2308f3f83 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tom.pittenger@krausaerospace.com>
Date: Tue, 1 Aug 2023 12:13:22 -0700
Subject: [PATCH] AP_Periph: add support for RawAirData.Pitot_temperature

---
 Tools/AP_Periph/can.cpp | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index cfc5332526..1122b823bb 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -2400,8 +2400,6 @@ void AP_Periph_FW::can_airspeed_update(void)
     }
 
     uavcan_equipment_air_data_RawAirData pkt {};
-    pkt.differential_pressure = press;
-    pkt.static_air_temperature = temp;
 
     // unfilled elements are NaN
     pkt.static_pressure = nanf("");
@@ -2409,6 +2407,21 @@ void AP_Periph_FW::can_airspeed_update(void)
     pkt.differential_pressure_sensor_temperature = nanf("");
     pkt.pitot_temperature = nanf("");
 
+    // populate the elements we have
+    pkt.differential_pressure = press;
+    pkt.static_air_temperature = temp;
+
+    // if a Pitot tube temperature sensor is available, use it
+#if AP_TEMPERATURE_SENSOR_ENABLED
+    for (uint8_t i=0; i<temperature_sensor.num_instances(); i++) {
+        float temp_pitot;
+        if (temperature_sensor.get_source(i) == AP_TemperatureSensor_Params::Source::Pitot_tube && temperature_sensor.get_temperature(temp_pitot, i)) {
+            pkt.pitot_temperature = C_TO_KELVIN(temp_pitot);
+            break;
+        }
+    }
+#endif
+
     uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE] {};
     uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer, !periph.canfdout());