UAVCAN Moving Baseline Working

Set uavcan publisher priorities

Switch to ardupilot rtcm message and add heading accuracy

src/drivers/gps/gps.cpp

@@ -191,11 +191,12 @@ private:
 
 	const Instance 			_instance;
 
-	uORB::Subscription		_orb_inject_data_sub{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};
-	gps_dump_comm_mode_t _dump_communication_mode{gps_dump_comm_mode_t::Disabled};
+	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};
+	gps_dump_comm_mode_t                 _dump_communication_mode{gps_dump_comm_mode_t::Disabled};
 
 	static px4::atomic_bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1
 	/// and thus we wait until the first one publishes at least one message.
@@ -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,13 +511,16 @@ void GPS::handleInjectDataTopic()
 
 			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);
+				// 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.
+					*/
+					injectData(msg.data, msg.len);
 
-				++_last_rate_rtcm_injection_count;
+					++_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[])
 {