diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index de9c62005d..97024c4b61 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-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 @@ -46,6 +46,7 @@ #include #include #include +#include using namespace time_literals; @@ -57,9 +58,9 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _sub_auxiliary(node), _sub_fix(node), _sub_fix2(node), - _pub_rtcm(node), - _channel_using_fix2(new bool[_max_channels]), - _rtcm_perf(perf_alloc(PC_INTERVAL, "uavcan: gnss: rtcm pub")) + _pub_moving_baseline_data(node), + _pub_rtcm_stream(node), + _channel_using_fix2(new bool[_max_channels]) { for (uint8_t i = 0; i < _max_channels; i++) { _channel_using_fix2[i] = false; @@ -71,7 +72,8 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : UavcanGnssBridge::~UavcanGnssBridge() { delete [] _channel_using_fix2; - perf_free(_rtcm_perf); + perf_free(_rtcm_stream_pub_perf); + perf_free(_moving_baseline_data_pub_perf); } int @@ -98,7 +100,26 @@ UavcanGnssBridge::init() return res; } - _pub_rtcm.setPriority(uavcan::TransferPriority::NumericallyMax); + + // UAVCAN_PUB_RTCM + int32_t uavcan_pub_rtcm = 0; + param_get(param_find("UAVCAN_PUB_RTCM"), &uavcan_pub_rtcm); + + if (uavcan_pub_rtcm == 1) { + _publish_rtcm_stream = true; + _pub_rtcm_stream.setPriority(uavcan::TransferPriority::NumericallyMax); + _rtcm_stream_pub_perf = perf_alloc(PC_INTERVAL, "uavcan: gnss: rtcm stream pub"); + } + + // UAVCAN_PUB_MBD + int32_t uavcan_pub_mbd = 0; + param_get(param_find("UAVCAN_PUB_MBD"), &uavcan_pub_mbd); + + if (uavcan_pub_mbd == 1) { + _publish_moving_baseline_data = true; + _pub_moving_baseline_data.setPriority(uavcan::TransferPriority::NumericallyMax); + _moving_baseline_data_pub_perf = perf_alloc(PC_INTERVAL, "uavcan: gnss: moving baseline data rtcm stream pub"); + } return res; } @@ -446,41 +467,37 @@ void UavcanGnssBridge::update() // to work. void UavcanGnssBridge::handleInjectDataTopic() { - bool updated = false; - // Limit maximum number of GPS injections to 6 since usually // GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo). // Looking at 6 packets thus guarantees, that at least a full injection // data set is evaluated. - const size_t max_num_injections = 6; + static constexpr size_t MAX_NUM_INJECTIONS = 6; + size_t num_injections = 0; + gps_inject_data_s gps_inject_data; - do { - num_injections++; - updated = _orb_inject_data_sub.updated(); - - if (updated) { - gps_inject_data_s msg; - - if (_orb_inject_data_sub.copy(&msg)) { - - /* Write the message to the gps device. Note that the message could be fragmented. - * But as we don't write anywhere else to the device during operation, we don't - * need to assemble the message first. - */ - injectData(msg.data, msg.len); - } + while ((num_injections <= MAX_NUM_INJECTIONS) && _gps_inject_data_sub.update(&gps_inject_data)) { + // Write the message to the gps device. Note that the message could be fragmented. + // But as we don't write anywhere else to the device during operation, we don't + // need to assemble the message first. + if (_publish_rtcm_stream) { + PublishRTCMStream(gps_inject_data.data, gps_inject_data.len); } - } while (updated && num_injections < max_num_injections); + + if (_publish_moving_baseline_data) { + PublishMovingBaselineData(gps_inject_data.data, gps_inject_data.len); + } + + num_injections++; + } } -bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_len) +bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len) { - using ardupilot::gnss::MovingBaselineData; + using uavcan::equipment::gnss::RTCMStream; - perf_count(_rtcm_perf); - - MovingBaselineData msg; + RTCMStream msg; + msg.protocol_id = RTCMStream::PROTOCOL_ID_RTCM3; const size_t capacity = msg.data.capacity(); size_t written = 0; @@ -498,7 +515,36 @@ bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_l written += 1; } - result = _pub_rtcm.broadcast(msg) >= 0; + result = _pub_rtcm_stream.broadcast(msg) >= 0; + perf_count(_rtcm_stream_pub_perf); + msg.data = {}; + } + + return result; +} + +bool UavcanGnssBridge::PublishMovingBaselineData(const uint8_t *data, size_t data_len) +{ + ardupilot::gnss::MovingBaselineData msg; + + const size_t capacity = msg.data.capacity(); + size_t written = 0; + bool result = true; + + while (result && written < data_len) { + size_t chunk_size = data_len - written; + + if (chunk_size > capacity) { + chunk_size = capacity; + } + + for (size_t i = 0; i < chunk_size; ++i) { + msg.data.push_back(data[written]); + written += 1; + } + + result = _pub_moving_baseline_data.broadcast(msg) >= 0; + perf_count(_moving_baseline_data_pub_perf); msg.data.clear(); } @@ -508,5 +554,6 @@ bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_l void UavcanGnssBridge::print_status() const { UavcanSensorBridgeBase::print_status(); - perf_print_counter(_rtcm_perf); + perf_print_counter(_rtcm_stream_pub_perf); + perf_print_counter(_moving_baseline_data_pub_perf); } diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index 620eedfa7f..b60c238f2f 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-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 @@ -54,6 +54,7 @@ #include #include #include +#include #include @@ -61,8 +62,6 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase { - static constexpr unsigned ORB_TO_UAVCAN_FREQUENCY_HZ = 10; - public: static const char *const NAME; @@ -92,7 +91,8 @@ private: const float heading_accuracy); void handleInjectDataTopic(); - bool injectData(const uint8_t *data, size_t data_len); + bool PublishRTCMStream(const uint8_t *data, size_t data_len); + bool PublishMovingBaselineData(const uint8_t *data, size_t data_len); typedef uavcan::MethodBinder < UavcanGnssBridge *, void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > @@ -115,17 +115,23 @@ private: uavcan::Subscriber _sub_auxiliary; uavcan::Subscriber _sub_fix; uavcan::Subscriber _sub_fix2; - uavcan::Publisher _pub_rtcm; + + uavcan::Publisher _pub_moving_baseline_data; + uavcan::Publisher _pub_rtcm_stream; uint64_t _last_gnss_auxiliary_timestamp{0}; float _last_gnss_auxiliary_hdop{0.0f}; float _last_gnss_auxiliary_vdop{0.0f}; - uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; + uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data)}; bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data? bool *_channel_using_fix2; ///< Flag for whether each channel is using Fix2 or Fix msg - perf_counter_t _rtcm_perf; + bool _publish_rtcm_stream{false}; + bool _publish_moving_baseline_data{false}; + + perf_counter_t _rtcm_stream_pub_perf{nullptr}; + perf_counter_t _moving_baseline_data_pub_perf{nullptr}; }; diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index 61d6df4816..d37247d3a9 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/src/drivers/uavcan/uavcan_params.c @@ -194,6 +194,30 @@ PARAM_DEFINE_INT32(UAVCAN_LGT_NAV, 3); */ PARAM_DEFINE_INT32(UAVCAN_LGT_LAND, 0); +/** + * publish RTCM stream + * + * Enable UAVCAN RTCM stream publication + * uavcan::equipment::gnss::RTCMStream + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_PUB_RTCM, 0); + +/** + * publish moving baseline data RTCM stream + * + * Enable UAVCAN RTCM stream publication + * ardupilot::gnss::MovingBaselineData + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_PUB_MBD, 0); + /** * subscription airspeed *