From 983867f9afc8b79594d27400ecde902170e73278 Mon Sep 17 00:00:00 2001 From: honglang Date: Wed, 20 Oct 2021 17:55:44 +0800 Subject: [PATCH] uavcan: support uavcan hygrometer --- src/drivers/uavcan/CMakeLists.txt | 2 + src/drivers/uavcan/libuavcan | 2 +- src/drivers/uavcan/sensors/hygrometer.cpp | 73 ++++++++++++++++++++ src/drivers/uavcan/sensors/hygrometer.hpp | 61 ++++++++++++++++ src/drivers/uavcan/sensors/sensor_bridge.cpp | 23 ++++-- src/drivers/uavcan/uavcan_params.c | 12 ++++ src/drivers/uavcannode/CMakeLists.txt | 1 + 7 files changed, 166 insertions(+), 8 deletions(-) create mode 100755 src/drivers/uavcan/sensors/hygrometer.cpp create mode 100755 src/drivers/uavcan/sensors/hygrometer.hpp diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 087de0cf7e..48bb20a336 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -102,6 +102,7 @@ set(DSDLC_INPUTS "${LIBUAVCAN_DIR}/dsdl/ardupilot" "${LIBUAVCAN_DIR}/dsdl/com" "${LIBUAVCAN_DIR}/dsdl/cuav" + "${LIBUAVCAN_DIR}/dsdl/dronecan" "${LIBUAVCAN_DIR}/dsdl/uavcan" ) set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated") @@ -163,6 +164,7 @@ px4_add_module( sensors/gyro.cpp sensors/cbat.cpp sensors/ice_status.cpp + sensors/hygrometer.cpp MODULE_CONFIG module.yaml diff --git a/src/drivers/uavcan/libuavcan b/src/drivers/uavcan/libuavcan index 76b2c4c338..9c09983f73 160000 --- a/src/drivers/uavcan/libuavcan +++ b/src/drivers/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 76b2c4c33858fcde18c50d29184273a495bdbbd1 +Subproject commit 9c09983f737de0dd224d26025f8f439d6d860b27 diff --git a/src/drivers/uavcan/sensors/hygrometer.cpp b/src/drivers/uavcan/sensors/hygrometer.cpp new file mode 100755 index 0000000000..e22ce31d24 --- /dev/null +++ b/src/drivers/uavcan/sensors/hygrometer.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#include "hygrometer.hpp" + +#include +#include + +const char *const UavcanHygrometerBridge::NAME = "hygrometer_sensor"; + +UavcanHygrometerBridge::UavcanHygrometerBridge(uavcan::INode &node) : + UavcanSensorBridgeBase("uavcan_hygrometer_sensor", ORB_ID(sensor_hygrometer)), + _sub_hygro(node) +{ +} + +int UavcanHygrometerBridge::init() +{ + int res = _sub_hygro.start(HygroCbBinder(this, &UavcanHygrometerBridge::hygro_sub_cb)); + + if (res < 0) { + DEVICE_LOG("failed to start uavcan sub: %d", res); + return res; + } + + return 0; +} + +void UavcanHygrometerBridge::hygro_sub_cb(const uavcan::ReceivedDataStructure + &msg) +{ + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + + sensor_hygrometer_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = 0; // TODO + report.temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; + report.humidity = msg.humidity; + report.timestamp = hrt_absolute_time(); + + publish(msg.getSrcNodeID().get(), &report); +} diff --git a/src/drivers/uavcan/sensors/hygrometer.hpp b/src/drivers/uavcan/sensors/hygrometer.hpp new file mode 100755 index 0000000000..871bdef043 --- /dev/null +++ b/src/drivers/uavcan/sensors/hygrometer.hpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include "sensor_bridge.hpp" + +#include + +class UavcanHygrometerBridge : public UavcanSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanHygrometerBridge(uavcan::INode &node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + void hygro_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder < UavcanHygrometerBridge *, + void (UavcanHygrometerBridge::*) + (const uavcan::ReceivedDataStructure &) > + HygroCbBinder; + + uavcan::Subscriber _sub_hygro; +}; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 6582195caa..89bcfccafd 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -38,18 +38,19 @@ #include "sensor_bridge.hpp" #include -#include "differential_pressure.hpp" +#include "accel.hpp" +#include "airspeed.hpp" #include "baro.hpp" #include "battery.hpp" -#include "airspeed.hpp" -#include "gnss.hpp" +#include "cbat.hpp" +#include "differential_pressure.hpp" #include "flow.hpp" -#include "mag.hpp" -#include "rangefinder.hpp" -#include "accel.hpp" +#include "gnss.hpp" #include "gyro.hpp" -#include "cbat.hpp" +#include "hygrometer.hpp" #include "ice_status.hpp" +#include "mag.hpp" +#include "rangefinder.hpp" /* * IUavcanSensorBridge @@ -107,6 +108,14 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List