dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

uavcan: GNSS optionally publish RTCMStream or MovingBaselineData

This commit is contained in:
Daniel Agar 2022-07-08 16:18:29 -04:00
parent 416610afc3
commit cc7e709597
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
3 changed files with 116 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
src/drivers/uavcan/sensors/gnss.cpp
View File

@ -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 <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include <mathlib/mathlib.h>
#include <lib/parameters/param.h>
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;
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);
}
if (_publish_moving_baseline_data) {
PublishMovingBaselineData(gps_inject_data.data, gps_inject_data.len);
}
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 (updated && num_injections < max_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);
}

20
src/drivers/uavcan/sensors/gnss.hpp
View File

@ -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 <uavcan/equipment/gnss/Fix.hpp>
#include <uavcan/equipment/gnss/Fix2.hpp>
#include <ardupilot/gnss/MovingBaselineData.hpp>
#include <uavcan/equipment/gnss/RTCMStream.hpp>
#include <lib/perf/perf_counter.h>
@ -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<uavcan::equipment::gnss::Auxiliary> &) >
@ -115,17 +115,23 @@ private:
uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxiliaryCbBinder> _sub_auxiliary;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
uavcan::Publisher<ardupilot::gnss::MovingBaselineData> _pub_rtcm;
uavcan::Publisher<ardupilot::gnss::MovingBaselineData> _pub_moving_baseline_data;
uavcan::Publisher<uavcan::equipment::gnss::RTCMStream> _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};
};

24
src/drivers/uavcan/uavcan_params.c
View File

@ -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
*