From 3ff1f213a45db3e47bb03b5bc11d353de807433a Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Wed, 1 Nov 2023 15:15:17 -0600 Subject: [PATCH] uavcan: add RelPosHeading->sensor_gnss_relative --- msg/SensorGnssRelative.msg | 4 +- src/drivers/uavcan/CMakeLists.txt | 1 + src/drivers/uavcan/Kconfig | 4 + src/drivers/uavcan/sensors/gnss_relative.cpp | 78 ++++++++++++++++++++ src/drivers/uavcan/sensors/gnss_relative.hpp | 65 ++++++++++++++++ src/drivers/uavcan/sensors/sensor_bridge.cpp | 14 ++++ src/drivers/uavcan/uavcan_params.c | 12 +++ 7 files changed, 176 insertions(+), 2 deletions(-) create mode 100644 src/drivers/uavcan/sensors/gnss_relative.cpp create mode 100644 src/drivers/uavcan/sensors/gnss_relative.hpp diff --git a/msg/SensorGnssRelative.msg b/msg/SensorGnssRelative.msg index 0f1736f45f..6d87a344c1 100644 --- a/msg/SensorGnssRelative.msg +++ b/msg/SensorGnssRelative.msg @@ -15,8 +15,8 @@ float32[3] position_accuracy # Accuracy of relative position (m) float32 heading # Heading of the relative position vector (radians) float32 heading_accuracy # Accuracy of heading of the relative position vector (radians) -float32 position_length -float32 accuracy_length +float32 position_length # Length of the position vector (m) +float32 accuracy_length # Accuracy of the position length (m) bool gnss_fix_ok # GNSS valid fix (i.e within DOP & accuracy masks) bool differential_solution # differential corrections were applied diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 37993c97ce..4146edab05 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -167,6 +167,7 @@ px4_add_module( sensors/battery.cpp sensors/airspeed.cpp sensors/flow.cpp + sensors/gnss_relative.cpp sensors/gnss.cpp sensors/mag.cpp sensors/rangefinder.cpp diff --git a/src/drivers/uavcan/Kconfig b/src/drivers/uavcan/Kconfig index d2f064e8ba..1d91acb622 100644 --- a/src/drivers/uavcan/Kconfig +++ b/src/drivers/uavcan/Kconfig @@ -57,6 +57,10 @@ if DRIVERS_UAVCAN bool "Subscribe to GPS: uavcan::equipment::gnss::Auxiliary | uavcan::equipment::gnss::Fix | uavcan::equipment::gnss::Fix2" default y + config UAVCAN_SENSOR_GNSS_RELATIVE + bool "Subscribe to GPS Relative: ardupilot::equipment::gnss::RelPosHeading" + default y + config UAVCAN_SENSOR_HYGROMETER bool "Subscribe to Hygrometer: dronecan::sensors::hygrometer::Hygrometer" default y diff --git a/src/drivers/uavcan/sensors/gnss_relative.cpp b/src/drivers/uavcan/sensors/gnss_relative.cpp new file mode 100644 index 0000000000..1636939163 --- /dev/null +++ b/src/drivers/uavcan/sensors/gnss_relative.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 "gnss_relative.hpp" + +#include +#include + +const char *const UavcanGnssRelativeBridge::NAME = "gnss_relative"; + +UavcanGnssRelativeBridge::UavcanGnssRelativeBridge(uavcan::INode &node) : + UavcanSensorBridgeBase("uavcan_gnss_relative", ORB_ID(sensor_gnss_relative)), + _sub_rel_pos_heading(node) +{ +} + +int +UavcanGnssRelativeBridge::init() +{ + int res = _sub_rel_pos_heading.start(RelPosHeadingCbBinder(this, &UavcanGnssRelativeBridge::rel_pos_heading_sub_cb)); + + if (res < 0) { + DEVICE_LOG("failed to start uavcan sub: %d", res); + return res; + } + + return 0; +} + +void UavcanGnssRelativeBridge::rel_pos_heading_sub_cb(const + uavcan::ReceivedDataStructure &msg) +{ + sensor_gnss_relative_s sensor_gnss_relative{}; + + sensor_gnss_relative.timestamp_sample = uavcan::UtcTime(msg.timestamp).toUSec(); + + sensor_gnss_relative.heading_valid = msg.reported_heading_acc_available; + sensor_gnss_relative.heading = math::radians(msg.reported_heading_deg); + sensor_gnss_relative.heading_accuracy = math::radians(msg.reported_heading_acc_deg); + sensor_gnss_relative.position_length = msg.relative_distance_m; + sensor_gnss_relative.position[2] = msg.relative_down_pos_m; + + sensor_gnss_relative.device_id = get_device_id(); + + sensor_gnss_relative.timestamp = hrt_absolute_time(); + + publish(msg.getSrcNodeID().get(), &sensor_gnss_relative); +} diff --git a/src/drivers/uavcan/sensors/gnss_relative.hpp b/src/drivers/uavcan/sensors/gnss_relative.hpp new file mode 100644 index 0000000000..87c7e22731 --- /dev/null +++ b/src/drivers/uavcan/sensors/gnss_relative.hpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 "sensor_bridge.hpp" + +#include + +#include + +#include + +class UavcanGnssRelativeBridge : public UavcanSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanGnssRelativeBridge(uavcan::INode &node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + + void rel_pos_heading_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder < UavcanGnssRelativeBridge *, + void (UavcanGnssRelativeBridge::*)(const uavcan::ReceivedDataStructure &) > + RelPosHeadingCbBinder; + + uavcan::Subscriber _sub_rel_pos_heading; + +}; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 3f1d424bd1..c219d90f3f 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -60,6 +60,9 @@ #if defined(CONFIG_UAVCAN_SENSOR_GNSS) #include "gnss.hpp" #endif +#if defined(CONFIG_UAVCAN_SENSOR_GNSS_RELATIVE) +#include "gnss_relative.hpp" +#endif #if defined(CONFIG_UAVCAN_SENSOR_HYGROMETER) #include "hygrometer.hpp" #endif @@ -145,6 +148,17 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List