diff --git a/src/drivers/uavcan/sensors/baro.cpp b/src/drivers/uavcan/sensors/baro.cpp index 51c8a4b899..b416e26a12 100644 --- a/src/drivers/uavcan/sensors/baro.cpp +++ b/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 &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 *baro = static_cast *>(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(ORB_ID(sensor_baro)); diff --git a/src/drivers/uavcan/sensors/baro.hpp b/src/drivers/uavcan/sensors/baro.hpp index 882fe35fc8..b1f76c5a76 100644 --- a/src/drivers/uavcan/sensors/baro.hpp +++ b/src/drivers/uavcan/sensors/baro.hpp @@ -40,6 +40,7 @@ #include "sensor_bridge.hpp" #include #include +#include class UavcanBarometerBridge : public UavcanSensorBridgeBase { @@ -56,6 +57,7 @@ private: void air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg); void air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg); + void raw_air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); int init_driver(uavcan_bridge::Channel *channel) override; @@ -69,8 +71,14 @@ private: (const uavcan::ReceivedDataStructure &) > AirTemperatureCbBinder; + typedef uavcan::MethodBinder < UavcanBarometerBridge *, + void (UavcanBarometerBridge::*) + (const uavcan::ReceivedDataStructure &) > + RawAirDataCbBinder; + uavcan::Subscriber _sub_air_pressure_data; uavcan::Subscriber _sub_air_temperature_data; + uavcan::Subscriber _sub_raw_air_data; float _last_temperature_kelvin{NAN}; diff --git a/src/drivers/uavcannode/Publishers/RawAirData.hpp b/src/drivers/uavcannode/Publishers/RawAirData.hpp index 6226a5feb4..9ccd2f882e 100644 --- a/src/drivers/uavcannode/Publishers/RawAirData.hpp +++ b/src/drivers/uavcannode/Publishers/RawAirData.hpp @@ -33,6 +33,8 @@ #pragma once +#include + #include "UavcanPublisherBase.hpp" #include @@ -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::broadcast(raw_air_data);