UAVCAN Moving Baseline Working

Set uavcan publisher priorities

Switch to ardupilot rtcm message and add heading accuracy

msg/gps_inject_data.msg

@@ -1,6 +1,9 @@
 uint64 timestamp		# time since system start (microseconds)
+
+uint32 device_id                # unique device ID for the sensor that does not change between power cycles
+
 uint8 len                       # length of data
 uint8 flags                     # LSB: 1=fragmented
-uint8[182] data		# data to write to GPS device (RTCM message)
+uint8[300] data                 # data to write to GPS device (RTCM message)
 
 uint8 ORB_QUEUE_LENGTH = 8

msg/sensor_gps.msg

@@ -38,3 +38,4 @@ uint8 satellites_used		# Number of satellites used
 
 float32 heading			# heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI])
 float32 heading_offset		# heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI])
+float32 heading_accuracy	# heading accuracy (rad, [0, 2PI])

src/drivers/gps/devices

@@ -1 +1 @@
-Subproject commit 6fcf06894973240d45dc49d3b31565917dc8f2f6
+Subproject commit bd72eb6794e8fb4f2ed3e47311a14c2cec69f60b

src/drivers/gps/gps.cpp

@@ -192,6 +192,7 @@ private:
 	const Instance 			_instance;
 
 	uORB::Subscription		     _orb_inject_data_sub{ORB_ID(gps_inject_data)};
+	uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
 	uORB::Publication<gps_dump_s>	     _dump_communication_pub{ORB_ID(gps_dump)};
 	gps_dump_s			     *_dump_to_device{nullptr};
 	gps_dump_s			     *_dump_from_device{nullptr};
@@ -214,6 +215,11 @@ private:
 	 */
 	void 				publishSatelliteInfo();
 
+	/**
+	 * Publish RTCM corrections
+	 */
+	void 				publishRTCMCorrections(uint8_t *data, size_t len);
+
 	/**
 	 * This is an abstraction for the poll on serial used.
 	 *
@@ -383,6 +389,7 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 		return gps->setBaudrate(data2);
 
 	case GPSCallbackType::gotRTCMMessage:
+		gps->publishRTCMCorrections((uint8_t *)data1, (size_t)data2);
 		gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::RTCM, false);
 		break;
 
@@ -504,6 +511,8 @@ void GPS::handleInjectDataTopic()
 
 			if (_orb_inject_data_sub.copy(&msg)) {
 
+				// Prevent injection of data from self
+				if (msg.device_id != get_device_id()) {
 					/* 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.
@@ -513,6 +522,7 @@ void GPS::handleInjectDataTopic()
 					++_last_rate_rtcm_injection_count;
 				}
 			}
+		}
 	} while (updated && num_injections < max_num_injections);
 }
 
@@ -549,6 +559,8 @@ int GPS::setBaudrate(unsigned baud)
 
 	case 460800: speed = B460800; break;
 
+	case 921600: speed = B921600; break;
+
 	default:
 		PX4_ERR("ERR: unknown baudrate: %d", baud);
 		return -EINVAL;
@@ -713,6 +725,20 @@ GPS::run()
 			} else {
 				ubx_mode = GPSDriverUBX::UBXMode::MovingBase;
 			}
+
+		} else if (gps_ubx_mode == 2) {
+			ubx_mode = GPSDriverUBX::UBXMode::MovingBase;
+
+		} else if (gps_ubx_mode == 3) {
+			if (_instance == Instance::Main) {
+				ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1;
+
+			} else {
+				ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1;
+			}
+
+		} else if (gps_ubx_mode == 4) {
+			ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1;
 		}
 	}
 
@@ -869,7 +895,8 @@ GPS::run()
 			int helper_ret;
 			unsigned receive_timeout = TIMEOUT_5HZ;
 
-			if (ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBase) {
+			if ((ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBase)
+			    || (ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1)) {
 				/* The MB rover will wait as long as possible to compute a navigation solution,
 				 * possibly lowering the navigation rate all the way to 1 Hz while doing so. */
 				receive_timeout = TIMEOUT_1HZ;
@@ -1115,6 +1142,41 @@ GPS::publishSatelliteInfo()
 	}
 }
 
+void
+GPS::publishRTCMCorrections(uint8_t *data, size_t len)
+{
+	gps_inject_data_s gps_inject_data{};
+
+	gps_inject_data.timestamp = hrt_absolute_time();
+	gps_inject_data.device_id = get_device_id();
+
+	size_t capacity = (sizeof(gps_inject_data.data) / sizeof(gps_inject_data.data[0]));
+
+	if (len > capacity) {
+		gps_inject_data.flags = 1; //LSB: 1=fragmented
+
+	} else {
+		gps_inject_data.flags = 0;
+	}
+
+	size_t written = 0;
+
+	while (written < len) {
+
+		gps_inject_data.len = len - written;
+
+		if (gps_inject_data.len > capacity) {
+			gps_inject_data.len = capacity;
+		}
+
+		memcpy(gps_inject_data.data, &data[written], gps_inject_data.len);
+
+		_gps_inject_data_pub.publish(gps_inject_data);
+
+		written = written + gps_inject_data.len;
+	}
+}
+
 int
 GPS::custom_command(int argc, char *argv[])
 {

src/drivers/gps/params.c

@@ -76,14 +76,19 @@ PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7);
  * dual GPS without heading.
  *
  * The Heading mode requires 2 F9P devices to be attached. The main GPS will act as rover and output
- * heading information, whereas the secondary will act as moving base, sending RTCM on UART2 to
- * the rover GPS.
+ * heading information, whereas the secondary will act as moving base.
+ * Modes 1 and 2 require each F9P UART1 to be connected to the Autopilot. In addition, UART2 on the
+ * F9P units are connected to each other.
+ * Modes 3 and 4 only require UART1 on each F9P connected to the Autopilot or Can Node. UART RX DMA is required.
  * RTK is still possible with this setup.
  *
  * @min 0
  * @max 1
  * @value 0 Default
- * @value 1 Heading
+ * @value 1 Heading (Rover With Moving Base UART1 Connected To Autopilot, UART2 Connected To Moving Base)
+ * @value 2 Moving Base (UART1 Connected To Autopilot, UART2 Connected To Rover)
+ * @value 3 Heading (Rover With Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
+ * @value 4 Moving Base (Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
  *
  * @reboot_required true
  * @group GPS
@@ -95,7 +100,6 @@ PARAM_DEFINE_INT32(GPS_UBX_MODE, 0);
  * Heading/Yaw offset for dual antenna GPS
  *
  * Heading offset angle for dual antenna GPS setups that support heading estimation.
- * (currently only for the Trimble MB-Two).
  *
  * Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the first antenna is in
  * front. The offset angle increases clockwise.

src/drivers/uavcan/beep.cpp

@@ -45,6 +45,7 @@ UavcanBeep::UavcanBeep(uavcan::INode &node) :
 	_beep_pub(node),
 	_timer(node)
 {
+	_beep_pub.setPriority(uavcan::TransferPriority::Default);
 }
 
 int UavcanBeep::init()

src/drivers/uavcan/safety_state.cpp

@@ -43,6 +43,7 @@ UavcanSafetyState::UavcanSafetyState(uavcan::INode &node) :
 	_safety_state_pub(node),
 	_timer(node)
 {
+	_safety_state_pub.setPriority(uavcan::TransferPriority::Default);
 }
 
 int UavcanSafetyState::init()

src/drivers/uavcan/sensors/gnss.cpp

@@ -98,7 +98,7 @@ UavcanGnssBridge::init()
 		return res;
 	}
 
-	_pub_rtcm.setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+	_pub_rtcm.setPriority(uavcan::TransferPriority::NumericallyMax);
 
 	return res;
 }
@@ -134,7 +134,7 @@ UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
 	float vel_cov[9];
 	msg.velocity_covariance.unpackSquareMatrix(vel_cov);
 
-	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov);
+	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov, NAN, NAN, NAN);
 }
 
 void
@@ -274,14 +274,34 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 		}
 	}
 
-	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances);
+	float heading = NAN;
+	float heading_offset = NAN;
+	float heading_accuracy = NAN;
+
+	// Use ecef_position_velocity for now... There is no heading field
+	if (!msg.ecef_position_velocity.empty()) {
+		heading = msg.ecef_position_velocity[0].velocity_xyz[0];
+
+		if (!isnan(msg.ecef_position_velocity[0].velocity_xyz[1])) {
+			heading_offset = msg.ecef_position_velocity[0].velocity_xyz[1];
+		}
+
+		if (!isnan(msg.ecef_position_velocity[0].velocity_xyz[2])) {
+			heading_accuracy = msg.ecef_position_velocity[0].velocity_xyz[2];
+		}
+	}
+
+	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances, heading, heading_offset,
+		     heading_accuracy);
 }
 
 template <typename FixType>
 void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
 				    uint8_t fix_type,
 				    const float (&pos_cov)[9], const float (&vel_cov)[9],
-				    const bool valid_pos_cov, const bool valid_vel_cov)
+				    const bool valid_pos_cov, const bool valid_vel_cov,
+				    const float heading, const float heading_offset,
+				    const float heading_accuracy)
 {
 	sensor_gps_s report{};
 	report.device_id = get_device_id();
@@ -407,8 +427,9 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 		report.vdop = msg.pdop;
 	}
 
-	report.heading = NAN;
-	report.heading_offset = NAN;
+	report.heading = heading;
+	report.heading_offset = heading_offset;
+	report.heading_accuracy = heading_accuracy;
 
 	publish(msg.getSrcNodeID().get(), &report);
 }
@@ -455,12 +476,11 @@ void UavcanGnssBridge::handleInjectDataTopic()
 
 bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_len)
 {
-	using uavcan::equipment::gnss::RTCMStream;
+	using ardupilot::gnss::MovingBaselineData;
 
 	perf_count(_rtcm_perf);
 
-	RTCMStream msg;
-	msg.protocol_id = RTCMStream::PROTOCOL_ID_RTCM3;
+	MovingBaselineData msg;
 
 	const size_t capacity = msg.data.capacity();
 	size_t written = 0;

src/drivers/uavcan/sensors/gnss.hpp

@@ -53,7 +53,7 @@
 #include <uavcan/equipment/gnss/Auxiliary.hpp>
 #include <uavcan/equipment/gnss/Fix.hpp>
 #include <uavcan/equipment/gnss/Fix2.hpp>
-#include <uavcan/equipment/gnss/RTCMStream.hpp>
+#include <ardupilot/gnss/MovingBaselineData.hpp>
 
 #include <lib/perf/perf_counter.h>
 
@@ -87,7 +87,9 @@ private:
 	void process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
 			  uint8_t fix_type,
 			  const float (&pos_cov)[9], const float (&vel_cov)[9],
-			  const bool valid_pos_cov, const bool valid_vel_cov);
+			  const bool valid_pos_cov, const bool valid_vel_cov,
+			  const float heading, const float heading_offset,
+			  const float heading_accuracy);
 
 	void handleInjectDataTopic();
 	bool injectData(const uint8_t *data, size_t data_len);
@@ -113,7 +115,7 @@ 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<uavcan::equipment::gnss::RTCMStream> _pub_rtcm;
+	uavcan::Publisher<ardupilot::gnss::MovingBaselineData> _pub_rtcm;
 
 	uint64_t	_last_gnss_auxiliary_timestamp{0};
 	float		_last_gnss_auxiliary_hdop{0.0f};

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,24 +73,35 @@ 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)
 	{
+		// 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.flags = gps_rtcm_data_msg.flags;
 			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_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();

src/modules/mavlink/streams/GPS2_RAW.hpp

@@ -90,7 +90,11 @@ private:
 					msg.yaw = 36000; // Use 36000 for north.
 
 				} else {
-					msg.yaw = math::degrees(gps.heading) * 100.f; // centidegrees
+					msg.yaw = math::degrees(matrix::wrap_2pi(gps.heading)) * 100.0f; // centidegrees
+				}
+
+				if (PX4_ISFINITE(gps.heading_accuracy)) {
+					msg.hdg_acc = math::degrees(gps.heading_accuracy) * 1e5f; // Heading / track uncertainty in degE5
 				}
 			}
 

src/modules/mavlink/streams/GPS_RAW_INT.hpp

@@ -85,14 +85,17 @@ private:
 			msg.h_acc = gps.eph * 1e3f;              // position uncertainty in mm
 			msg.v_acc = gps.epv * 1e3f;              // altitude uncertainty in mm
 			msg.vel_acc = gps.s_variance_m_s * 1e3f; // speed uncertainty in mm
-			msg.hdg_acc = math::degrees(gps.c_variance_rad) * 1e5f; // Heading / track uncertainty in degE5
 
 			if (PX4_ISFINITE(gps.heading)) {
 				if (fabsf(gps.heading) < FLT_EPSILON) {
 					msg.yaw = 36000; // Use 36000 for north.
 
 				} else {
-					msg.yaw = math::degrees(gps.heading) * 100.f; // centidegrees
+					msg.yaw = math::degrees(matrix::wrap_2pi(gps.heading)) * 100.0f; // centidegrees
+				}
+
+				if (PX4_ISFINITE(gps.heading_accuracy)) {
+					msg.hdg_acc = math::degrees(gps.heading_accuracy) * 1e5f; // Heading / track uncertainty in degE5
 				}
 			}