fix: uavcan RawAirData static handling (#27320)

Co-authored-by: Ramon Roche <mrpollo@gmail.com>

src/drivers/uavcan/sensors/baro.cpp

@@ -48,7 +48,8 @@ const char *const UavcanBarometerBridge::NAME = "baro";
 UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
 	UavcanSensorBridgeBase("uavcan_baro", ORB_ID(sensor_baro), node_info_publisher),
 	_sub_air_pressure_data(node),
-	_sub_air_temperature_data(node)
+	_sub_air_temperature_data(node),
+	_sub_raw_air_data(node)
 {
 	set_device_type(DRV_BARO_DEVTYPE_UAVCAN);
 }
@@ -69,6 +70,13 @@ int UavcanBarometerBridge::init()
 		return res;
 	}
 
+	res = _sub_raw_air_data.start(RawAirDataCbBinder(this, &UavcanBarometerBridge::raw_air_data_sub_cb));
+
+	if (res < 0) {
+		DEVICE_LOG("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
 	return 0;
 }
 
@@ -133,6 +141,59 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 	baro->publish(sensor_baro);
 }
 
+void UavcanBarometerBridge::raw_air_data_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &msg)
+{
+	// Only process if static_pressure is a valid atmospheric value.
+	// Old PX4 nodes zero-initialize unused fields, while the spec requires NaN for unused fields.
+	if (!PX4_ISFINITE(msg.static_pressure) || msg.static_pressure <= 0.0f) {
+		return;
+	}
+
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
+
+	if (channel == nullptr) {
+		// Something went wrong - no channel to publish on; return
+		return;
+	}
+
+	// Cast our generic CDev pointer to the sensor-specific driver class
+	uORB::PublicationMulti<sensor_baro_s> *baro = static_cast<uORB::PublicationMulti<sensor_baro_s> *>(channel->h_driver);
+
+	if (baro == nullptr) {
+		return;
+	}
+
+	uint32_t device_id = make_uavcan_device_id(msg);
+
+	// Register barometer capability with NodeInfoPublisher after first successful message
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id,
+				NodeInfoPublisher::DeviceCapability::BAROMETER);
+	}
+
+	// publish
+	sensor_baro_s sensor_baro{};
+	sensor_baro.timestamp_sample = timestamp_sample;
+	sensor_baro.device_id = device_id;
+	sensor_baro.pressure = msg.static_pressure;
+
+	float temperature = msg.static_air_temperature;
+
+	if (PX4_ISFINITE(temperature) && (temperature >= 0.f)) {
+		sensor_baro.temperature = temperature + atmosphere::kAbsoluteNullCelsius;
+
+	} else {
+		sensor_baro.temperature = NAN;
+	}
+
+	sensor_baro.error_count = 0;
+	sensor_baro.timestamp = hrt_absolute_time();
+	baro->publish(sensor_baro);
+}
+
 int UavcanBarometerBridge::init_driver(uavcan_bridge::Channel *channel)
 {
 	channel->h_driver = new uORB::PublicationMulti<sensor_baro_s>(ORB_ID(sensor_baro));

src/drivers/uavcan/sensors/baro.hpp

@@ -40,6 +40,7 @@
 #include "sensor_bridge.hpp"
 #include <uavcan/equipment/air_data/StaticPressure.hpp>
 #include <uavcan/equipment/air_data/StaticTemperature.hpp>
+#include <uavcan/equipment/air_data/RawAirData.hpp>
 
 class UavcanBarometerBridge : public UavcanSensorBridgeBase
 {
@@ -56,6 +57,7 @@ private:
 
 	void air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg);
 	void air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg);
+	void raw_air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &msg);
 
 	int init_driver(uavcan_bridge::Channel *channel) override;
 
@@ -69,8 +71,14 @@ private:
 		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &) >
 		AirTemperatureCbBinder;
 
+	typedef uavcan::MethodBinder < UavcanBarometerBridge *,
+		void (UavcanBarometerBridge::*)
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &) >
+		RawAirDataCbBinder;
+
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data;
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
+	uavcan::Subscriber<uavcan::equipment::air_data::RawAirData, RawAirDataCbBinder> _sub_raw_air_data;
 
 	float _last_temperature_kelvin{NAN};
 

src/drivers/uavcannode/Publishers/RawAirData.hpp

@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include <cmath>
+
 #include "UavcanPublisherBase.hpp"
 
 #include <uavcan/equipment/air_data/RawAirData.hpp>
@@ -76,12 +78,12 @@ public:
 		if (uORB::SubscriptionCallbackWorkItem::update(&diff_press)) {
 			uavcan::equipment::air_data::RawAirData raw_air_data{};
 
-			// raw_air_data.static_pressure =
+			raw_air_data.static_pressure = NAN;
+			raw_air_data.static_pressure_sensor_temperature = NAN;
 			raw_air_data.differential_pressure = diff_press.differential_pressure_pa;
-			// raw_air_data.static_pressure_sensor_temperature =
 			raw_air_data.differential_pressure_sensor_temperature = diff_press.temperature - atmosphere::kAbsoluteNullCelsius;
 			raw_air_data.static_air_temperature = diff_press.temperature - atmosphere::kAbsoluteNullCelsius;
-			// raw_air_data.pitot_temperature
+			raw_air_data.pitot_temperature = NAN;
 			// raw_air_data.covariance
 			uavcan::Publisher<uavcan::equipment::air_data::RawAirData>::broadcast(raw_air_data);