From efcf4c95fd57203bb45755ad29fc4655a3d703c7 Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Fri, 9 Sep 2022 13:16:09 -0600 Subject: [PATCH] drivers/uavcannode: Add RTCMStream subscriber and fix RTCMStream Publisher (#20056) * Add cannode RTCMStream subscriber * Fix uavcan RTCMStream publisher * Break out CANNODE_SUB_RTCM and CANNODE_SUB_MBD --- boards/ark/can-rtk-gps/init/rc.board_defaults | 3 +- src/drivers/uavcan/sensors/gnss.cpp | 7 +- .../uavcannode/Subscribers/RTCMStream.hpp | 108 ++++++++++++++++++ src/drivers/uavcannode/UavcanNode.cpp | 20 +++- src/drivers/uavcannode/uavcannode_params.c | 23 +++- 5 files changed, 149 insertions(+), 12 deletions(-) create mode 100644 src/drivers/uavcannode/Subscribers/RTCMStream.hpp diff --git a/boards/ark/can-rtk-gps/init/rc.board_defaults b/boards/ark/can-rtk-gps/init/rc.board_defaults index 25bc347662..10419cee6b 100644 --- a/boards/ark/can-rtk-gps/init/rc.board_defaults +++ b/boards/ark/can-rtk-gps/init/rc.board_defaults @@ -4,7 +4,8 @@ #------------------------------------------------------------------------------ param set-default CBRK_IO_SAFETY 0 -param set-default CANNODE_GPS_RTCM 1 +param set-default CANNODE_SUB_MBD 1 +param set-default CANNODE_SUB_RTCM 1 param set-default MBE_ENABLE 1 safety_button start diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 68b5051b86..2eb59d3b26 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -494,10 +494,9 @@ void UavcanGnssBridge::handleInjectDataTopic() bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len) { - using uavcan::equipment::gnss::RTCMStream; + uavcan::equipment::gnss::RTCMStream msg; - RTCMStream msg; - msg.protocol_id = RTCMStream::PROTOCOL_ID_RTCM3; + msg.protocol_id = uavcan::equipment::gnss::RTCMStream::PROTOCOL_ID_RTCM3; const size_t capacity = msg.data.capacity(); size_t written = 0; @@ -517,7 +516,7 @@ bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t result = _pub_rtcm_stream.broadcast(msg) >= 0; perf_count(_rtcm_stream_pub_perf); - msg.data = {}; + msg.data.clear(); } return result; diff --git a/src/drivers/uavcannode/Subscribers/RTCMStream.hpp b/src/drivers/uavcannode/Subscribers/RTCMStream.hpp new file mode 100644 index 0000000000..83e2c84737 --- /dev/null +++ b/src/drivers/uavcannode/Subscribers/RTCMStream.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 "UavcanSubscriberBase.hpp" + +#include + +#include +#include +#include + +namespace uavcannode +{ + +class RTCMStream; + +typedef uavcan::MethodBinder&)> + RTCMStreamBinder; + +class RTCMStream : + public UavcanSubscriberBase, + private uavcan::Subscriber +{ +public: + RTCMStream(uavcan::INode &node) : + UavcanSubscriberBase(uavcan::equipment::gnss::RTCMStream::DefaultDataTypeID), + uavcan::Subscriber(node) + {} + + bool init() + { + if (start(RTCMStreamBinder(this, &RTCMStream::callback)) < 0) { + PX4_ERR("uavcan::equipment::gnss::RTCMStream subscription failed"); + return false; + } + + return true; + } + + void PrintInfo() const override + { + printf("\t%s:%d -> %s\n", + uavcan::equipment::gnss::RTCMStream::getDataTypeFullName(), + uavcan::equipment::gnss::RTCMStream::DefaultDataTypeID, + _gps_inject_data_pub.get_topic()->o_name); + } + +private: + void callback(const uavcan::ReceivedDataStructure &msg) + { + // Don't republish a message from ourselves + if (msg.getSrcNodeID().get() != getNode().getNodeID().get()) { + gps_inject_data_s gps_inject_data{}; + + gps_inject_data.len = msg.data.size(); + + memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len); + + gps_inject_data.timestamp = hrt_absolute_time(); + + union device::Device::DeviceId device_id; + + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN; + device_id.devid_s.address = msg.getSrcNodeID().get(); + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_UAVCAN; + + gps_inject_data.device_id = device_id.devid; + + _gps_inject_data_pub.publish(gps_inject_data); + } + } + + uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index b42b46e9d6..64741a8da8 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -55,6 +55,7 @@ #include "Subscribers/BeepCommand.hpp" #include "Subscribers/LightsCommand.hpp" #include "Subscribers/MovingBaselineData.hpp" +#include "Subscribers/RTCMStream.hpp" using namespace time_literals; @@ -301,12 +302,11 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events _publisher_list.add(new RawAirData(this, _node)); _publisher_list.add(new RelPosHeadingPub(this, _node)); - int32_t enable_movingbaselinedata = 0; - param_get(param_find("CANNODE_GPS_RTCM"), &enable_movingbaselinedata); + int32_t cannode_pub_mbd = 0; + param_get(param_find("CANNODE_PUB_MBD"), &cannode_pub_mbd); - if (enable_movingbaselinedata != 0) { + if (cannode_pub_mbd == 1) { _publisher_list.add(new MovingBaselineDataPub(this, _node)); - } _publisher_list.add(new SafetyButton(this, _node)); @@ -316,10 +316,20 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events _subscriber_list.add(new BeepCommand(_node)); _subscriber_list.add(new LightsCommand(_node)); - if (enable_movingbaselinedata != 0) { + int32_t cannode_sub_mdb = 0; + param_get(param_find("CANNODE_SUB_MDB"), &cannode_sub_mdb); + + if (cannode_sub_mdb == 1) { _subscriber_list.add(new MovingBaselineData(_node)); } + int32_t cannode_sub_rtcm = 0; + param_get(param_find("CANNODE_SUB_RTCM"), &cannode_sub_rtcm); + + if (cannode_sub_rtcm == 1) { + _subscriber_list.add(new RTCMStream(_node)); + } + for (auto &subscriber : _subscriber_list) { subscriber->init(); } diff --git a/src/drivers/uavcannode/uavcannode_params.c b/src/drivers/uavcannode/uavcannode_params.c index 8e9a10e833..8a9e1ba0ee 100644 --- a/src/drivers/uavcannode/uavcannode_params.c +++ b/src/drivers/uavcannode/uavcannode_params.c @@ -61,10 +61,29 @@ PARAM_DEFINE_INT32(CANNODE_BITRATE, 1000000); PARAM_DEFINE_INT32(CANNODE_TERM, 0); /** - * Enable RTCM pub/sub + * Enable MovingBaselineData subscription * * @boolean * @max 1 + * @reboot_required true * @group UAVCAN */ -PARAM_DEFINE_INT32(CANNODE_GPS_RTCM, 0); +PARAM_DEFINE_INT32(CANNODE_SUB_MBD, 0); + +/** + * Enable RTCM subscription + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(CANNODE_SUB_RTCM, 0); + +/** + * Enable MovingBaselineData publication + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(CANNODE_PUB_MBD, 0);