UAVCAN Moving Baseline Working

Set uavcan publisher priorities

Switch to ardupilot rtcm message and add heading accuracy

src/drivers/uavcannode/Publishers/BatteryInfo.hpp

@@ -53,7 +53,9 @@ public:
 		UavcanPublisherBase(uavcan::equipment::power::BatteryInfo::DefaultDataTypeID),
 		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(battery_status)),
 		uavcan::Publisher<uavcan::equipment::power::BatteryInfo>(node)
-	{}
+	{
+		this->setPriority(uavcan::TransferPriority::MiddleLower);
+	}
 
 	void PrintInfo() override
 	{

src/drivers/uavcannode/Publishers/FlowMeasurement.hpp

@@ -63,6 +63,8 @@ public:
 		if (param_get(rot, &val) == PX4_OK) {
 			_rotation = get_rot_matrix((enum Rotation)val);
 		}
+
+		this->setPriority(uavcan::TransferPriority::Default);
 	}
 
 	void PrintInfo() override

src/drivers/uavcannode/Publishers/GnssFix2.hpp

@@ -53,7 +53,9 @@ public:
 		UavcanPublisherBase(uavcan::equipment::gnss::Fix2::DefaultDataTypeID),
 		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_gps)),
 		uavcan::Publisher<uavcan::equipment::gnss::Fix2>(node)
-	{}
+	{
+		this->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
+	}
 
 	void PrintInfo() override
 	{
@@ -118,6 +120,26 @@ public:
 			fix2.covariance.push_back(gps.s_variance_m_s);
 			fix2.covariance.push_back(gps.s_variance_m_s);
 
+			uavcan::equipment::gnss::ECEFPositionVelocity ecefpositionvelocity{};
+			ecefpositionvelocity.velocity_xyz[0] = NAN;
+			ecefpositionvelocity.velocity_xyz[1] = NAN;
+			ecefpositionvelocity.velocity_xyz[2] = NAN;
+
+			// Use ecef_position_velocity for now... There is no heading field
+			if (!isnan(gps.heading)) {
+				ecefpositionvelocity.velocity_xyz[0] = gps.heading;
+
+				if (!isnan(gps.heading_offset)) {
+					ecefpositionvelocity.velocity_xyz[1] = gps.heading_offset;
+				}
+
+				if (!isnan(gps.heading_accuracy)) {
+					ecefpositionvelocity.velocity_xyz[2] = gps.heading_accuracy;
+				}
+
+				fix2.ecef_position_velocity.push_back(ecefpositionvelocity);
+			}
+
 			uavcan::Publisher<uavcan::equipment::gnss::Fix2>::broadcast(fix2);
 
 			// ensure callback is registered

src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp

@@ -53,7 +53,9 @@ public:
 		UavcanPublisherBase(uavcan::equipment::ahrs::MagneticFieldStrength2::DefaultDataTypeID),
 		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_mag)),
 		uavcan::Publisher<uavcan::equipment::ahrs::MagneticFieldStrength2>(node)
-	{}
+	{
+		this->setPriority(uavcan::TransferPriority::Default);
+	}
 
 	void PrintInfo() override
 	{

src/drivers/uavcannode/Publishers/MovingBaselineData.hpp

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   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 "UavcanPublisherBase.hpp"
+
+#include <ardupilot/gnss/MovingBaselineData.hpp>
+
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/gps_inject_data.h>
+
+namespace uavcannode
+{
+
+class MovingBaselineDataPub :
+	public UavcanPublisherBase,
+	public uORB::SubscriptionCallbackWorkItem,
+	private uavcan::Publisher<ardupilot::gnss::MovingBaselineData>
+{
+public:
+	MovingBaselineDataPub(px4::WorkItem *work_item, uavcan::INode &node) :
+		UavcanPublisherBase(ardupilot::gnss::MovingBaselineData::DefaultDataTypeID),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(gps_inject_data)),
+		uavcan::Publisher<ardupilot::gnss::MovingBaselineData>(node)
+	{
+		this->setPriority(uavcan::TransferPriority::NumericallyMax);
+	}
+
+	void PrintInfo() override
+	{
+		if (uORB::SubscriptionCallbackWorkItem::advertised()) {
+			printf("\t%s -> %s:%d\n",
+			       uORB::SubscriptionCallbackWorkItem::get_topic()->o_name,
+			       ardupilot::gnss::MovingBaselineData::getDataTypeFullName(),
+			       id());
+		}
+	}
+
+	void BroadcastAnyUpdates() override
+	{
+		using ardupilot::gnss::MovingBaselineData;
+
+		// gps_inject_data -> ardupilot::gnss::MovingBaselineData
+		gps_inject_data_s inject_data;
+
+		if (uORB::SubscriptionCallbackWorkItem::update(&inject_data)) {
+			// Prevent republishing rtcm data we received from uavcan
+			if (inject_data.device_id > uavcan::NodeID::Max) {
+				ardupilot::gnss::MovingBaselineData movingbaselinedata{};
+
+				const size_t capacity = movingbaselinedata.data.capacity();
+				size_t written = 0;
+				int result = 0;
+
+				while ((result >= 0) && written < inject_data.len) {
+					size_t chunk_size = inject_data.len - written;
+
+					if (chunk_size > capacity) {
+						chunk_size = capacity;
+					}
+
+					for (size_t i = 0; i < chunk_size; ++i) {
+						movingbaselinedata.data.push_back(inject_data.data[written]);
+						written += 1;
+					}
+
+					result = uavcan::Publisher<ardupilot::gnss::MovingBaselineData>::broadcast(movingbaselinedata);
+
+					// ensure callback is registered
+					uORB::SubscriptionCallbackWorkItem::registerCallback();
+
+					movingbaselinedata.data.clear();
+				}
+			}
+		}
+	}
+};
+} // namespace uavcannode

src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp

@@ -53,7 +53,9 @@ public:
 		UavcanPublisherBase(uavcan::equipment::range_sensor::Measurement::DefaultDataTypeID),
 		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(distance_sensor), instance),
 		uavcan::Publisher<uavcan::equipment::range_sensor::Measurement>(node)
-	{}
+	{
+		this->setPriority(uavcan::TransferPriority::Default);
+	}
 
 	void PrintInfo() override
 	{

src/drivers/uavcannode/Publishers/RawAirData.hpp

@@ -53,7 +53,9 @@ public:
 		UavcanPublisherBase(uavcan::equipment::air_data::RawAirData::DefaultDataTypeID),
 		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(differential_pressure)),
 		uavcan::Publisher<uavcan::equipment::air_data::RawAirData>(node)
-	{}
+	{
+		this->setPriority(uavcan::TransferPriority::Default);
+	}
 
 	void PrintInfo() override
 	{

src/drivers/uavcannode/Publishers/SafetyButton.hpp

@@ -53,7 +53,9 @@ public:
 		UavcanPublisherBase(ardupilot::indication::Button::DefaultDataTypeID),
 		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(safety)),
 		uavcan::Publisher<ardupilot::indication::Button>(node)
-	{}
+	{
+		this->setPriority(uavcan::TransferPriority::Default);
+	}
 
 	void PrintInfo() override
 	{

src/drivers/uavcannode/Publishers/StaticPressure.hpp

@@ -53,7 +53,9 @@ public:
 		UavcanPublisherBase(uavcan::equipment::air_data::StaticPressure::DefaultDataTypeID),
 		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_baro)),
 		uavcan::Publisher<uavcan::equipment::air_data::StaticPressure>(node)
-	{}
+	{
+		this->setPriority(uavcan::TransferPriority::Default);
+	}
 
 	void PrintInfo() override
 	{

src/drivers/uavcannode/Publishers/StaticTemperature.hpp

@@ -53,7 +53,9 @@ public:
 		UavcanPublisherBase(uavcan::equipment::air_data::StaticTemperature::DefaultDataTypeID),
 		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_baro)),
 		uavcan::Publisher<uavcan::equipment::air_data::StaticTemperature>(node)
-	{}
+	{
+		this->setPriority(uavcan::TransferPriority::MiddleLower);
+	}
 
 	void PrintInfo() override
 	{

src/drivers/uavcannode/Subscribers/MovingBaselineData.hpp

@@ -35,34 +35,35 @@
 
 #include "UavcanSubscriberBase.hpp"
 
-#include <uavcan/equipment/gnss/RTCMStream.hpp>
+#include <ardupilot/gnss/MovingBaselineData.hpp>
 
+#include <lib/drivers/device/Device.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/gps_inject_data.h>
 
 namespace uavcannode
 {
 
-class RTCMStream;
+class MovingBaselineData;
 
-typedef uavcan::MethodBinder<RTCMStream *,
-	void (RTCMStream::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::RTCMStream>&)>
-	RTCMStreamBinder;
+typedef uavcan::MethodBinder<MovingBaselineData *,
+	void (MovingBaselineData::*)(const uavcan::ReceivedDataStructure<ardupilot::gnss::MovingBaselineData>&)>
+	MovingBaselineDataBinder;
 
-class RTCMStream :
+class MovingBaselineData :
 	public UavcanSubscriberBase,
-	private uavcan::Subscriber<uavcan::equipment::gnss::RTCMStream, RTCMStreamBinder>
+	private uavcan::Subscriber<ardupilot::gnss::MovingBaselineData, MovingBaselineDataBinder>
 {
 public:
-	RTCMStream(uavcan::INode &node) :
-		UavcanSubscriberBase(uavcan::equipment::gnss::RTCMStream::DefaultDataTypeID),
-		uavcan::Subscriber<uavcan::equipment::gnss::RTCMStream, RTCMStreamBinder>(node)
+	MovingBaselineData(uavcan::INode &node) :
+		UavcanSubscriberBase(ardupilot::gnss::MovingBaselineData::DefaultDataTypeID),
+		uavcan::Subscriber<ardupilot::gnss::MovingBaselineData, MovingBaselineDataBinder>(node)
 	{}
 
 	bool init()
 	{
-		if (start(RTCMStreamBinder(this, &RTCMStream::callback)) < 0) {
-			PX4_ERR("uavcan::equipment::gnss::RTCMStream subscription failed");
+		if (start(MovingBaselineDataBinder(this, &MovingBaselineData::callback)) < 0) {
+			PX4_ERR("ardupilot::gnss::MovingBaselineData subscription failed");
 			return false;
 		}
 
@@ -72,23 +73,34 @@ public:
 	void PrintInfo() const override
 	{
 		printf("\t%s:%d -> %s\n",
-		       uavcan::equipment::gnss::RTCMStream::getDataTypeFullName(),
-		       uavcan::equipment::gnss::RTCMStream::DefaultDataTypeID,
+		       ardupilot::gnss::MovingBaselineData::getDataTypeFullName(),
+		       ardupilot::gnss::MovingBaselineData::DefaultDataTypeID,
 		       _gps_inject_data_pub.get_topic()->o_name);
 	}
 
 private:
-	void callback(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::RTCMStream> &msg)
+	void callback(const uavcan::ReceivedDataStructure<ardupilot::gnss::MovingBaselineData> &msg)
 	{
-		gps_inject_data_s gps_inject_data{};
+		// 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();
+			gps_inject_data.len = msg.data.size();
 
-		//gps_inject_data.flags = gps_rtcm_data_msg.flags;
-		memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len);
+			memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len);
 
-		gps_inject_data.timestamp = hrt_absolute_time();
-		_gps_inject_data_pub.publish(gps_inject_data);
+			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_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};

src/drivers/uavcannode/UavcanNode.cpp

@@ -46,13 +46,14 @@
 #include "Publishers/MagneticFieldStrength2.hpp"
 #include "Publishers/RangeSensorMeasurement.hpp"
 #include "Publishers/RawAirData.hpp"
+#include "Publishers/MovingBaselineData.hpp"
 #include "Publishers/SafetyButton.hpp"
 #include "Publishers/StaticPressure.hpp"
 #include "Publishers/StaticTemperature.hpp"
 
 #include "Subscribers/BeepCommand.hpp"
 #include "Subscribers/LightsCommand.hpp"
-#include "Subscribers/RTCMStream.hpp"
+#include "Subscribers/MovingBaselineData.hpp"
 
 using namespace time_literals;
 
@@ -303,13 +304,14 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 	_publisher_list.add(new MagneticFieldStrength2(this, _node));
 	_publisher_list.add(new RangeSensorMeasurement(this, _node));
 	_publisher_list.add(new RawAirData(this, _node));
+	_publisher_list.add(new MovingBaselineDataPub(this, _node));
 	_publisher_list.add(new SafetyButton(this, _node));
 	_publisher_list.add(new StaticPressure(this, _node));
 	_publisher_list.add(new StaticTemperature(this, _node));
 
 	_subscriber_list.add(new BeepCommand(_node));
 	_subscriber_list.add(new LightsCommand(_node));
-	_subscriber_list.add(new RTCMStream(_node));
+	_subscriber_list.add(new MovingBaselineData(_node));
 
 	for (auto &subscriber : _subscriber_list) {
 		subscriber->init();