From d682ddb5104ec4b3f3d4914ec15db136a93cccbe Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Mon, 6 Apr 2020 22:15:31 -0600 Subject: [PATCH] UAVCAN differential pressure sensor support * added airspeed handling (differential pressure) to uavcan and uavcannode Co-authored-by: Jacob Crabill --- .../px4_work_queue/WorkQueueManager.hpp | 2 +- src/drivers/uavcan/CMakeLists.txt | 7 +- .../uavcan/sensors/differential_pressure.cpp | 113 ++++++++++++++++++ .../uavcan/sensors/differential_pressure.hpp | 74 ++++++++++++ src/drivers/uavcan/sensors/sensor_bridge.cpp | 8 +- src/drivers/uavcannode/UavcanNode.cpp | 21 ++++ src/drivers/uavcannode/UavcanNode.hpp | 6 +- 7 files changed, 222 insertions(+), 9 deletions(-) create mode 100644 src/drivers/uavcan/sensors/differential_pressure.cpp create mode 100644 src/drivers/uavcan/sensors/differential_pressure.hpp diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 3b7c6cd36f..30a7d123b2 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -69,7 +69,7 @@ static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 7200, -13}; static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -14}; -static constexpr wq_config_t uavcan{"wq:uavcan", 2800, -15}; +static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -15}; static constexpr wq_config_t UART0{"wq:UART0", 1400, -16}; static constexpr wq_config_t UART1{"wq:UART1", 1400, -17}; diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 3cd60a4470..b176d8bf8b 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -124,11 +124,12 @@ px4_add_module( # Sensors sensors/sensor_bridge.cpp + sensors/differential_pressure.cpp + sensors/baro.cpp + sensors/battery.cpp + sensors/flow.cpp sensors/gnss.cpp sensors/mag.cpp - sensors/baro.cpp - sensors/flow.cpp - sensors/battery.cpp DEPENDS px4_uavcan_dsdlc diff --git a/src/drivers/uavcan/sensors/differential_pressure.cpp b/src/drivers/uavcan/sensors/differential_pressure.cpp new file mode 100644 index 0000000000..d08b7c3d60 --- /dev/null +++ b/src/drivers/uavcan/sensors/differential_pressure.cpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Jacob Crabill + */ + +#include "differential_pressure.hpp" + +#include +#include +#include +#include +#include + +const char *const UavcanDifferentialPressureBridge::NAME = "differential_pressure"; + +UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode &node) : + UavcanCDevSensorBridgeBase("uavcan_differential_pressure", "/dev/uavcan/differential_pressure", + AIRSPEED_BASE_DEVICE_PATH, + ORB_ID(differential_pressure)), + _sub_air(node) +{ +} + +int UavcanDifferentialPressureBridge::init() +{ + int res = device::CDev::init(); + + if (res < 0) { + return res; + } + + // Initialize the calibration offset + param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); + + res = _sub_air.start(AirCbBinder(this, &UavcanDifferentialPressureBridge::air_sub_cb)); + + if (res < 0) { + DEVICE_LOG("failed to start uavcan sub: %d", res); + return res; + } + + return 0; +} + +int UavcanDifferentialPressureBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case AIRSPEEDIOCSSCALE: { + struct airspeed_scale *s = (struct airspeed_scale *)arg; + _diff_pres_offset = s->offset_pa; + return PX4_OK; + } + + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanDifferentialPressureBridge::air_sub_cb(const + uavcan::ReceivedDataStructure + &msg) +{ + _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_UAVCAN; + _device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF; + + float diff_press_pa = msg.differential_pressure; + float temperature_c = msg.static_air_temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; + + differential_pressure_s report = { + .timestamp = hrt_absolute_time(), + .error_count = 0, + .differential_pressure_raw_pa = diff_press_pa - _diff_pres_offset, + .differential_pressure_filtered_pa = _filter.apply(diff_press_pa) - _diff_pres_offset, /// TODO: Create filter + .temperature = temperature_c, + .device_id = _device_id.devid + }; + + publish(msg.getSrcNodeID().get(), &report); +} diff --git a/src/drivers/uavcan/sensors/differential_pressure.hpp b/src/drivers/uavcan/sensors/differential_pressure.hpp new file mode 100644 index 0000000000..7c8ab6fcfd --- /dev/null +++ b/src/drivers/uavcan/sensors/differential_pressure.hpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Jacob Crabill + */ + +#pragma once + +#include +#include +#include + +#include "sensor_bridge.hpp" + +#include + +class UavcanDifferentialPressureBridge : public UavcanCDevSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanDifferentialPressureBridge(uavcan::INode &node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + float _diff_pres_offset {0.0f}; + + math::LowPassFilter2p _filter{10.f, 1.1f}; /// Adapted from MS5525 driver + + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void air_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder < UavcanDifferentialPressureBridge *, + void (UavcanDifferentialPressureBridge::*) + (const uavcan::ReceivedDataStructure &) > + AirCbBinder; + + uavcan::Subscriber _sub_air; +}; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 6c1544636d..a46ac68d07 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -38,11 +38,12 @@ #include "sensor_bridge.hpp" #include -#include "gnss.hpp" -#include "mag.hpp" +#include "differential_pressure.hpp" #include "baro.hpp" -#include "flow.hpp" #include "battery.hpp" +#include "gnss.hpp" +#include "flow.hpp" +#include "mag.hpp" /* * IUavcanSensorBridge @@ -54,6 +55,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List uavcan::equipment::air_data::RawAirData + if (_diff_pressure_sub.updated()) { + differential_pressure_s diff_press; + + if (_diff_pressure_sub.copy(&diff_press)) { + + uavcan::equipment::air_data::RawAirData raw_air_data{}; + + // raw_air_data.static_pressure = + raw_air_data.differential_pressure = diff_press.differential_pressure_raw_pa; + // raw_air_data.static_pressure_sensor_temperature = + raw_air_data.differential_pressure_sensor_temperature = diff_press.temperature; + raw_air_data.static_air_temperature = diff_press.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; + // raw_air_data.pitot_temperature + // raw_air_data.covariance + _raw_air_data_publisher.broadcast(raw_air_data); + } + } + // sensor_baro -> uavcan::equipment::air_data::StaticTemperature if (_sensor_baro_sub.updated()) { sensor_baro_s baro; diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index e818c66b7f..f2fa9c4eb6 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/src/drivers/uavcannode/UavcanNode.hpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -71,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -170,13 +172,13 @@ private: uavcan::Publisher _power_battery_info_publisher; uavcan::Publisher _air_data_static_pressure_publisher; uavcan::Publisher _air_data_static_temperature_publisher; + uavcan::Publisher _raw_air_data_publisher; hrt_abstime _last_static_temperature_publish{0}; - - uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::SubscriptionCallbackWorkItem _battery_status_sub{this, ORB_ID(battery_status)}; + uORB::SubscriptionCallbackWorkItem _diff_pressure_sub{this, ORB_ID(differential_pressure)}; uORB::SubscriptionCallbackWorkItem _sensor_baro_sub{this, ORB_ID(sensor_baro)}; uORB::SubscriptionCallbackWorkItem _sensor_mag_sub{this, ORB_ID(sensor_mag)}; uORB::SubscriptionCallbackWorkItem _vehicle_gps_position_sub{this, ORB_ID(vehicle_gps_position)};