fix compile time

src/drivers/uavcan/Kconfig

@@ -62,8 +62,20 @@ if DRIVERS_UAVCAN
         bool "Subscribe to Fuel Tank Status:            uavcan::equipment::ice::FuelTankStatus"
         default y
 
-    config UAVCAN_SENSOR_GNSS
-        bool "Subscribe to GPS:                         uavcan::equipment::gnss::Auxiliary | uavcan::equipment::gnss::Fix | uavcan::equipment::gnss::Fix2"
+    config UAVCAN_SENSOR_GNSS_AUXILIARY
+        bool "Subscribe to GPS Auxiliary:               uavcan::equipment::gnss::Auxiliary"
+        default y
+
+    config UAVCAN_SENSOR_GNSS_FIX
+        bool "Subscribe to GPS Fix (deprecated):        uavcan::equipment::gnss::Fix"
+        default n
+
+    config UAVCAN_SENSOR_GNSS_FIX2
+        bool "Subscribe to GPS Fix2:                    uavcan::equipment::gnss::Fix2"
+        default y
+
+    config UAVCAN_SENSOR_GNSS_FIX3
+        bool "Subscribe to GPS Fix3 (preferred):        uavcan::equipment::gnss::Fix3"
         default y
 
     config UAVCAN_SENSOR_GNSS_RELATIVE

src/drivers/uavcan/sensors/gnss.cpp

@@ -56,25 +56,51 @@ const char *const UavcanGnssBridge::NAME = "gnss";
 UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
 	UavcanSensorBridgeBase("uavcan_gnss", ORB_ID(sensor_gps), node_info_publisher),
 	_node(node),
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_AUXILIARY)
 	_sub_auxiliary(node),
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX)
 	_sub_fix(node),
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)
 	_sub_fix2(node),
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+	_sub_fix3(node),
+#endif
 	_sub_gnss_heading(node),
 	_sub_moving_baseline_data(node),
 	_pub_moving_baseline_data(node),
-	_pub_rtcm_stream(node),
-	_channel_using_fix2(new bool[_max_channels])
+	_pub_rtcm_stream(node)
 {
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX) && (defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3))
+	_channel_using_fix2 = new bool[_max_channels];
+
 	for (uint8_t i = 0; i < _max_channels; i++) {
 		_channel_using_fix2[i] = false;
 	}
 
+#endif
+#if (defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)) && defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+	_channel_using_fix3 = new bool[_max_channels];
+
+	for (uint8_t i = 0; i < _max_channels; i++) {
+		_channel_using_fix3[i] = false;
+	}
+
+#endif
+
 	set_device_type(DRV_GPS_DEVTYPE_UAVCAN);
 }
 
 UavcanGnssBridge::~UavcanGnssBridge()
 {
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX) && (defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3))
 	delete [] _channel_using_fix2;
+#endif
+#if (defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)) && defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+	delete [] _channel_using_fix3;
+#endif
 	perf_free(_rtcm_stream_pub_perf);
 	perf_free(_moving_baseline_data_pub_perf);
 	perf_free(_moving_baseline_data_sub_perf);
@@ -83,13 +109,19 @@ UavcanGnssBridge::~UavcanGnssBridge()
 int
 UavcanGnssBridge::init()
 {
-	int res = _sub_auxiliary.start(AuxiliaryCbBinder(this, &UavcanGnssBridge::gnss_auxiliary_sub_cb));
+	int res = 0;
+
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_AUXILIARY)
+	res = _sub_auxiliary.start(AuxiliaryCbBinder(this, &UavcanGnssBridge::gnss_auxiliary_sub_cb));
 
 	if (res < 0) {
 		PX4_WARN("GNSS auxiliary sub failed %i", res);
 		return res;
 	}
 
+#endif
+
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX)
 	res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
 
 	if (res < 0) {
@@ -97,6 +129,9 @@ UavcanGnssBridge::init()
 		return res;
 	}
 
+#endif
+
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)
 	res = _sub_fix2.start(Fix2CbBinder(this, &UavcanGnssBridge::gnss_fix2_sub_cb));
 
 	if (res < 0) {
@@ -104,6 +139,18 @@ UavcanGnssBridge::init()
 		return res;
 	}
 
+#endif
+
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+	res = _sub_fix3.start(Fix3CbBinder(this, &UavcanGnssBridge::gnss_fix3_sub_cb));
+
+	if (res < 0) {
+		PX4_WARN("GNSS fix3 sub failed %i", res);
+		return res;
+	}
+
+#endif
+
 	res = _sub_gnss_heading.start(RelPosHeadingCbBinder(this, &UavcanGnssBridge::gnss_relative_sub_cb));
 
 	if (res < 0) {
@@ -143,6 +190,7 @@ UavcanGnssBridge::init()
 	return res;
 }
 
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_AUXILIARY)
 void
 UavcanGnssBridge::gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &msg)
 {
@@ -151,18 +199,30 @@ UavcanGnssBridge::gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavc
 	_last_gnss_auxiliary_hdop = msg.hdop;
 	_last_gnss_auxiliary_vdop = msg.vdop;
 }
+#endif
 
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX)
 void
 UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
 {
-	// Check to see if this node is also publishing a Fix2 message.
+	// Check to see if this node is also publishing a Fix2 or Fix3 message.
 	// If so, ignore the old "Fix" message for this node.
 	const int8_t ch = get_channel_index_for_node(msg.getSrcNodeID().get());
 
-	if (ch > -1 && _channel_using_fix2[ch]) {
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+
+	if (ch > -1 && (_channel_using_fix2[ch]
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+			|| _channel_using_fix3[ch]
+#endif
+		       )) {
 		return;
 	}
 
+#else
+	(void)ch;
+#endif
+
 	uint8_t fix_type = msg.status;
 
 	const bool valid_pos_cov = !msg.position_covariance.empty();
@@ -176,7 +236,9 @@ UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
 
 	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov, NAN, NAN, NAN, -1, -1, 0, 0);
 }
+#endif
 
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)
 void
 UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg)
 {
@@ -184,11 +246,26 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 
 	const int8_t ch = get_channel_index_for_node(msg.getSrcNodeID().get());
 
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+
+	// If this node is using Fix3, ignore Fix2 messages
+	if (ch > -1 && _channel_using_fix3[ch]) {
+		return;
+	}
+
+#endif
+
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX)
+
 	if (ch > -1 && !_channel_using_fix2[ch]) {
 		PX4_WARN("GNSS Fix2 msg detected for ch %d; disabling Fix msg for this node", ch);
 		_channel_using_fix2[ch] = true;
 	}
 
+#else
+	(void)ch;
+#endif
+
 	uint8_t fix_type = msg.status;
 
 	switch (msg.mode) {
@@ -349,6 +426,173 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances, heading, heading_offset,
 		     heading_accuracy, noise_per_ms, jamming_indicator, jamming_state, spoofing_state);
 }
+#endif
+
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+void
+UavcanGnssBridge::gnss_fix3_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix3> &msg)
+{
+	using uavcan::equipment::gnss::Fix3;
+
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)
+	const int8_t ch = get_channel_index_for_node(msg.getSrcNodeID().get());
+
+	if (ch > -1 && !_channel_using_fix3[ch]) {
+		PX4_WARN("GNSS Fix3 msg detected for ch %d; disabling Fix/Fix2 msgs for this node", ch);
+		_channel_using_fix3[ch] = true;
+	}
+
+#endif
+
+	sensor_gps_s sensor_gps{};
+
+	sensor_gps.device_id = make_uavcan_device_id(msg);
+
+	// Register GPS capability with NodeInfoPublisher after first successful message
+	if (_node_info_publisher != nullptr) {
+		_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
+				sensor_gps.device_id,
+				NodeInfoPublisher::DeviceCapability::GPS);
+	}
+
+	sensor_gps.timestamp = hrt_absolute_time();
+
+	// Position - Fix3 uses 1e8 scaling for lat/lon, mm for altitudes
+	sensor_gps.latitude_deg         = msg.latitude_deg_1e8 / 1e8;
+	sensor_gps.longitude_deg        = msg.longitude_deg_1e8 / 1e8;
+	sensor_gps.altitude_msl_m       = msg.altitude_msl / 1e3;        // mm to m
+	sensor_gps.altitude_ellipsoid_m = msg.altitude_ellipsoid / 1e3;  // mm to m
+
+	sensor_gps.eph = (msg.eph >= 0) ? msg.eph : -1.0f;
+	sensor_gps.epv = (msg.epv >= 0) ? msg.epv : -1.0f;
+
+	// Velocity
+	sensor_gps.vel_n_m_s = msg.vel_north;
+	sensor_gps.vel_e_m_s = msg.vel_east;
+	sensor_gps.vel_d_m_s = msg.vel_down;
+	sensor_gps.vel_m_s = (msg.ground_speed >= 0 && !std::isnan(msg.ground_speed)) ?
+			     msg.ground_speed : matrix::Vector3f(msg.vel_north, msg.vel_east, msg.vel_down).norm();
+	sensor_gps.vel_ned_valid = (msg.flags & Fix3::FLAGS_VEL_NED_VALID) != 0;
+
+	// Speed accuracy
+	sensor_gps.s_variance_m_s = (msg.speed_accuracy >= 0) ? (msg.speed_accuracy * msg.speed_accuracy) : -1.0f;
+
+	// Course over ground
+	if (!std::isnan(msg.cog)) {
+		sensor_gps.cog_rad = math::radians(msg.cog);
+
+	} else {
+		sensor_gps.cog_rad = atan2f(sensor_gps.vel_e_m_s, sensor_gps.vel_n_m_s);
+	}
+
+	// Course variance from velocity (same calculation as process_fixx)
+	if (sensor_gps.s_variance_m_s > 0) {
+		float vel_n = msg.vel_north;
+		float vel_e = msg.vel_east;
+		float vel_n_sq = vel_n * vel_n;
+		float vel_e_sq = vel_e * vel_e;
+		float speed_sq = vel_n_sq + vel_e_sq;
+
+		if (speed_sq > 0.01f) { // Only calculate if moving
+			sensor_gps.c_variance_rad = sensor_gps.s_variance_m_s / speed_sq;
+
+		} else {
+			sensor_gps.c_variance_rad = -1.0f;
+		}
+
+	} else {
+		sensor_gps.c_variance_rad = -1.0f;
+	}
+
+	// Fix type mapping
+	sensor_gps.fix_type = msg.fix_type;
+
+	// DOP values
+	sensor_gps.hdop = std::isnan(msg.hdop) ? 0.0f : msg.hdop;
+	sensor_gps.vdop = std::isnan(msg.vdop) ? 0.0f : msg.vdop;
+
+	// Satellite count
+	sensor_gps.satellites_used = msg.sats_used;
+
+	// Time handling
+	sensor_gps.timestamp_time_relative = 0;
+
+	if ((msg.flags & Fix3::FLAGS_TIME_VALID) && msg.gnss_time_usec > 0) {
+		switch (msg.time_standard) {
+		case Fix3::TIME_STANDARD_UTC:
+			sensor_gps.time_utc_usec = msg.gnss_time_usec;
+			break;
+
+		case Fix3::TIME_STANDARD_GPS:
+			if (msg.num_leap_seconds > 0) {
+				sensor_gps.time_utc_usec = msg.gnss_time_usec - (uint64_t)(msg.num_leap_seconds - 9) * 1000000ULL;
+			}
+
+			break;
+
+		case Fix3::TIME_STANDARD_TAI:
+			if (msg.num_leap_seconds > 0) {
+				sensor_gps.time_utc_usec = msg.gnss_time_usec - (uint64_t)(msg.num_leap_seconds + 10) * 1000000ULL;
+			}
+
+			break;
+
+		default:
+			break;
+		}
+
+		// Set system clock if not already done
+		if (sensor_gps.time_utc_usec != 0 && (msg.fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) {
+			timespec ts{};
+			ts.tv_sec = sensor_gps.time_utc_usec / 1000000ULL;
+			ts.tv_nsec = (sensor_gps.time_utc_usec % 1000000ULL) * 1000;
+			px4_clock_settime(CLOCK_REALTIME, &ts);
+			_system_clock_set = true;
+		}
+	}
+
+	// Heading - Fix3 provides direct degrees
+	if ((msg.flags & Fix3::FLAGS_HEADING_VALID) && !std::isnan(msg.heading)) {
+		// Use RelPosHeading if available and we have RTK Fixed solution
+		if (_rel_heading_valid && (msg.fix_type == sensor_gps_s::FIX_TYPE_RTK_FIXED)) {
+			sensor_gps.heading = _rel_heading;
+			sensor_gps.heading_offset = NAN;
+			sensor_gps.heading_accuracy = _rel_heading_accuracy;
+
+			_rel_heading = NAN;
+			_rel_heading_accuracy = NAN;
+			_rel_heading_valid = false;
+
+		} else {
+			sensor_gps.heading = math::radians(msg.heading);
+			sensor_gps.heading_offset = NAN; // Fix3 doesn't have offset field
+			sensor_gps.heading_accuracy = std::isnan(msg.heading_accuracy) ? NAN : math::radians(msg.heading_accuracy);
+		}
+
+	} else {
+		sensor_gps.heading = NAN;
+		sensor_gps.heading_offset = NAN;
+		sensor_gps.heading_accuracy = NAN;
+	}
+
+	// Quality metrics - Fix3 has dedicated fields
+	sensor_gps.noise_per_ms = msg.noise;
+	sensor_gps.automatic_gain_control = msg.agc;
+	sensor_gps.jamming_indicator = msg.jamming_indicator;
+	sensor_gps.jamming_state = msg.jamming_state;
+	sensor_gps.spoofing_state = msg.spoofing_state;
+	sensor_gps.authentication_state = msg.auth_state;
+
+	// System errors
+	sensor_gps.system_error = msg.system_errors;
+
+	// RTCM info
+	sensor_gps.selected_rtcm_instance = _selected_rtcm_instance;
+	sensor_gps.rtcm_injection_rate = _rtcm_injection_rate;
+
+	publish(msg.getSrcNodeID().get(), &sensor_gps);
+}
+#endif
 
 void UavcanGnssBridge::gnss_relative_sub_cb(const
 		uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &msg)
@@ -393,6 +637,7 @@ void UavcanGnssBridge::moving_baseline_data_sub_cb(const
 	}
 }
 
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)
 template <typename FixType>
 void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
 				    uint8_t fix_type,
@@ -522,11 +767,15 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 
 	sensor_gps.satellites_used = msg.sats_used;
 
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_AUXILIARY)
+
 	if (hrt_elapsed_time(&_last_gnss_auxiliary_timestamp) < 2_s) {
 		sensor_gps.hdop = _last_gnss_auxiliary_hdop;
 		sensor_gps.vdop = _last_gnss_auxiliary_vdop;
 
-	} else {
+	} else
+#endif
+	{
 		// Using PDOP for HDOP and VDOP
 		// Relevant discussion: https://github.com/PX4/Firmware/issues/5153
 		sensor_gps.hdop = msg.pdop;
@@ -559,6 +808,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 
 	publish(msg.getSrcNodeID().get(), &sensor_gps);
 }
+#endif
 
 void UavcanGnssBridge::update()
 {

src/drivers/uavcan/sensors/gnss.hpp

@@ -35,8 +35,9 @@
  * @file gnss.hpp
  *
  * UAVCAN <--> ORB bridge for GNSS messages:
- *     uavcan.equipment.gnss.Fix (deprecated, but still supported for backward compatibility)
- *     uavcan.equipment.gnss.Fix2
+ *     uavcan.equipment.gnss.Fix (deprecated, disabled by default via CONFIG_UAVCAN_SENSOR_GNSS_FIX)
+ *     uavcan.equipment.gnss.Fix2 (enabled by default via CONFIG_UAVCAN_SENSOR_GNSS_FIX2)
+ *     uavcan.equipment.gnss.Fix3 (preferred, enabled by default via CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
  *
  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  * @author Andrew Chambers <achamber@gmail.com>
@@ -52,9 +53,18 @@
 #include <uORB/topics/gps_dump.h>
 
 #include <uavcan/uavcan.hpp>
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_AUXILIARY)
 #include <uavcan/equipment/gnss/Auxiliary.hpp>
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX)
 #include <uavcan/equipment/gnss/Fix.hpp>
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)
 #include <uavcan/equipment/gnss/Fix2.hpp>
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+#include <uavcan/equipment/gnss/Fix3.hpp>
+#endif
 #include <ardupilot/gnss/MovingBaselineData.hpp>
 #include <ardupilot/gnss/RelPosHeading.hpp>
 #include <uavcan/equipment/gnss/RTCMStream.hpp>
@@ -81,13 +91,23 @@ private:
 	/**
 	 * GNSS fix message will be reported via this callback.
 	 */
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_AUXILIARY)
 	void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &msg);
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX)
 	void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)
 	void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg);
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+	void gnss_fix3_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix3> &msg);
+#endif
 	void gnss_relative_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &msg);
 	void moving_baseline_data_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::gnss::MovingBaselineData> &msg);
 
 
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)
 	template <typename FixType>
 	void process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
 			  uint8_t fix_type,
@@ -97,22 +117,35 @@ private:
 			  const float heading_accuracy, const int32_t noise_per_ms,
 			  const int32_t jamming_indicator, const uint8_t jamming_state,
 			  const uint8_t spoofing_state);
+#endif
 
 	void handleInjectDataTopic();
 	bool PublishRTCMStream(const uint8_t *data, size_t data_len);
 	bool PublishMovingBaselineData(const uint8_t *data, size_t data_len);
 
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_AUXILIARY)
 	typedef uavcan::MethodBinder < UavcanGnssBridge *,
 		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &) >
 		AuxiliaryCbBinder;
+#endif
 
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX)
 	typedef uavcan::MethodBinder < UavcanGnssBridge *,
 		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &) >
 		FixCbBinder;
+#endif
 
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)
 	typedef uavcan::MethodBinder < UavcanGnssBridge *,
 		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &) >
 		Fix2CbBinder;
+#endif
+
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+	typedef uavcan::MethodBinder < UavcanGnssBridge *,
+		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix3> &) >
+		Fix3CbBinder;
+#endif
 
 	typedef uavcan::MethodBinder<UavcanGnssBridge *,
 		void (UavcanGnssBridge::*)(const uavcan::TimerEvent &)>
@@ -128,9 +161,18 @@ private:
 
 	uavcan::INode &_node;
 
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_AUXILIARY)
 	uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxiliaryCbBinder> _sub_auxiliary;
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX)
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
+#endif
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+	uavcan::Subscriber<uavcan::equipment::gnss::Fix3, Fix3CbBinder> _sub_fix3;
+#endif
 	uavcan::Subscriber<ardupilot::gnss::RelPosHeading, RelPosHeadingCbBinder> _sub_gnss_heading;
 
 	// Used for MSM7 logging for PPK workflows
@@ -139,9 +181,11 @@ private:
 	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};
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_AUXILIARY)
+	uint64_t	_last_gnss_auxiliary_timestamp {0};
 	float		_last_gnss_auxiliary_hdop{0.0f};
 	float		_last_gnss_auxiliary_vdop{0.0f};
+#endif
 
 	uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _orb_inject_data_sub{ORB_ID::gps_inject_data};
 	hrt_abstime		_last_rtcm_injection_time{0};	///< time of last rtcm injection
@@ -151,7 +195,12 @@ private:
 
 	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
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX) && (defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3))
+	bool *_channel_using_fix2 {nullptr}; ///< Flag for whether each channel is using Fix2 or Fix msg
+#endif
+#if (defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2)) && defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
+	bool *_channel_using_fix3 {nullptr}; ///< Flag for whether each channel is using Fix3 (takes priority over Fix2 and Fix)
+#endif
 
 	bool _publish_rtcm_stream{false};
 	bool _publish_moving_baseline_data{false};

src/drivers/uavcan/sensors/sensor_bridge.cpp

@@ -60,7 +60,7 @@
 #if defined(CONFIG_UAVCAN_SENSOR_FUEL_TANK_STATUS)
 #include "fuel_tank_status.hpp"
 #endif
-#if defined(CONFIG_UAVCAN_SENSOR_GNSS)
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_AUXILIARY) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
 #include "gnss.hpp"
 #endif
 #if defined(CONFIG_UAVCAN_SENSOR_GNSS_RELATIVE)
@@ -155,7 +155,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 #endif
 
 	// GPS
-#if defined(CONFIG_UAVCAN_SENSOR_GNSS)
+#if defined(CONFIG_UAVCAN_SENSOR_GNSS_AUXILIARY) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX2) || defined(CONFIG_UAVCAN_SENSOR_GNSS_FIX3)
 	int32_t uavcan_sub_gps = 1;
 	param_get(param_find("UAVCAN_SUB_GPS"), &uavcan_sub_gps);
 

src/drivers/uavcannode/Kconfig

@@ -30,10 +30,18 @@ if DRIVERS_UAVCANNODE
         bool "Include flow measurement"
         default n
 
-    config UAVCANNODE_GNSS_FIX
-        bool "Include GNSS fix"
+    config UAVCANNODE_GNSS_AUXILIARY
+        bool "Include GNSS Auxiliary publisher"
         default n
 
+    config UAVCANNODE_GNSS_FIX2
+        bool "Include GNSS Fix2 publisher"
+        default n
+
+    config UAVCANNODE_GNSS_FIX3
+        bool "Include GNSS Fix3 publisher (preferred)"
+        default y
+
     config UAVCANNODE_HYGROMETER_MEASUREMENT
         bool "Include hygrometer measurement"
         default n

src/drivers/uavcannode/Publishers/GnssFix3.hpp

@@ -0,0 +1,208 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-2024 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 <cmath>
+
+#include "UavcanPublisherBase.hpp"
+
+#include <uavcan/equipment/gnss/Fix3.hpp>
+
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/sensor_gps.h>
+
+namespace uavcannode
+{
+
+class GnssFix3 :
+	public UavcanPublisherBase,
+	public uORB::SubscriptionCallbackWorkItem,
+	private uavcan::Publisher<uavcan::equipment::gnss::Fix3>
+{
+public:
+	GnssFix3(px4::WorkItem *work_item, uavcan::INode &node) :
+		UavcanPublisherBase(uavcan::equipment::gnss::Fix3::DefaultDataTypeID),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_gps)),
+		uavcan::Publisher<uavcan::equipment::gnss::Fix3>(node)
+	{
+		this->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
+	}
+
+	void PrintInfo() override
+	{
+		if (uORB::SubscriptionCallbackWorkItem::advertised()) {
+			printf("\t%s -> %s:%d\n",
+			       uORB::SubscriptionCallbackWorkItem::get_topic()->o_name,
+			       uavcan::equipment::gnss::Fix3::getDataTypeFullName(),
+			       id());
+		}
+	}
+
+	void BroadcastAnyUpdates() override
+	{
+		using uavcan::equipment::gnss::Fix3;
+
+		// sensor_gps -> uavcan::equipment::gnss::Fix3
+		sensor_gps_s gps;
+
+		if (uORB::SubscriptionCallbackWorkItem::update(&gps)) {
+			uavcan::equipment::gnss::Fix3 fix3{};
+
+			// Time
+			fix3.gnss_time_usec = gps.time_utc_usec;
+			fix3.time_standard = Fix3::TIME_STANDARD_UTC;
+			fix3.num_leap_seconds = 0; // Unknown
+
+			// Position - scale to 1e8 and mm
+			fix3.latitude_deg_1e8 = (int64_t)(gps.latitude_deg * 1e8);
+			fix3.longitude_deg_1e8 = (int64_t)(gps.longitude_deg * 1e8);
+			fix3.altitude_msl = (int32_t)(gps.altitude_msl_m * 1e3);        // m to mm
+			fix3.altitude_ellipsoid = (int32_t)(gps.altitude_ellipsoid_m * 1e3);  // m to mm
+
+			// Velocity
+			fix3.vel_north = gps.vel_n_m_s;
+			fix3.vel_east = gps.vel_e_m_s;
+			fix3.vel_down = gps.vel_d_m_s;
+			fix3.ground_speed = gps.vel_m_s;
+			fix3.eph = gps.eph;
+			fix3.epv = gps.epv;
+
+			// Speed accuracy - convert variance to std dev
+			fix3.speed_accuracy = (gps.s_variance_m_s > 0) ? sqrtf(gps.s_variance_m_s) : -1.0f;
+
+			// Heading - convert rad to deg
+			if (!std::isnan(gps.heading)) {
+				fix3.heading = math::degrees(gps.heading);
+				fix3.heading_accuracy = std::isnan(gps.heading_accuracy) ? NAN : math::degrees(gps.heading_accuracy);
+
+			} else {
+				fix3.heading = NAN;
+				fix3.heading_accuracy = NAN;
+			}
+
+			// Course over ground - convert rad to deg
+			fix3.cog = math::degrees(gps.cog_rad);
+
+			// DOP
+			fix3.hdop = gps.hdop;
+			fix3.vdop = gps.vdop;
+			fix3.pdop = (gps.hdop > gps.vdop) ? gps.hdop : gps.vdop; // Approximate PDOP
+
+			// Fix type
+			fix3.fix_type = gps.fix_type;
+
+			// Fix quality - derive from fix type if not available
+			switch (gps.fix_type) {
+			case sensor_gps_s::FIX_TYPE_NONE:
+				fix3.fix_quality = 0;
+				break;
+
+			case sensor_gps_s::FIX_TYPE_2D:
+				fix3.fix_quality = 30;
+				break;
+
+			case sensor_gps_s::FIX_TYPE_3D:
+				fix3.fix_quality = 60;
+				break;
+
+			case sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL:
+				fix3.fix_quality = 75;
+				break;
+
+			case sensor_gps_s::FIX_TYPE_RTK_FLOAT:
+				fix3.fix_quality = 85;
+				break;
+
+			case sensor_gps_s::FIX_TYPE_RTK_FIXED:
+				fix3.fix_quality = 100;
+				break;
+
+			default:
+				fix3.fix_quality = 0;
+				break;
+			}
+
+			// Satellite counts
+			fix3.sats_used = gps.satellites_used;
+			fix3.sats_visible = gps.satellites_used; // We don't have visible count separately
+
+			// Differential correction age - not available in sensor_gps
+			fix3.diff_age = 0xFFFF; // Unavailable
+
+			// Signal quality metrics
+			fix3.noise = gps.noise_per_ms;
+			fix3.agc = gps.automatic_gain_control;
+			fix3.jamming_state = gps.jamming_state;
+			fix3.jamming_indicator = gps.jamming_indicator;
+			fix3.spoofing_state = gps.spoofing_state;
+			fix3.auth_state = gps.authentication_state;
+
+			// System errors
+			fix3.system_errors = gps.system_error;
+
+			// Flags
+			fix3.flags = 0;
+
+			if (gps.vel_ned_valid) {
+				fix3.flags |= Fix3::FLAGS_VEL_NED_VALID;
+			}
+
+			if (!std::isnan(gps.heading)) {
+				fix3.flags |= Fix3::FLAGS_HEADING_VALID;
+			}
+
+			if (gps.time_utc_usec != 0) {
+				fix3.flags |= Fix3::FLAGS_TIME_VALID;
+			}
+
+			if (gps.fix_type >= sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL) {
+				fix3.flags |= Fix3::FLAGS_DIFF_CORRECTIONS;
+			}
+
+			// Assume receiver is healthy if we're getting data
+			fix3.flags |= Fix3::FLAGS_RECEIVER_HEALTHY;
+
+			// Armable if we have at least a 3D fix
+			if (gps.fix_type >= sensor_gps_s::FIX_TYPE_3D) {
+				fix3.flags |= Fix3::FLAGS_ARMABLE;
+			}
+
+			uavcan::Publisher<uavcan::equipment::gnss::Fix3>::broadcast(fix3);
+
+			// ensure callback is registered
+			uORB::SubscriptionCallbackWorkItem::registerCallback();
+		}
+	}
+};
+} // namespace uavcannode

src/drivers/uavcannode/UavcanNode.cpp

@@ -56,10 +56,17 @@
 #include "Publishers/HygrometerMeasurement.hpp"
 #endif // UAVCANNODE_HYGROMETER_MEASUREMENT
 
-#if defined(CONFIG_UAVCANNODE_GNSS_FIX)
-#include "Publishers/GnssFix2.hpp"
+#if defined(CONFIG_UAVCANNODE_GNSS_AUXILIARY)
 #include "Publishers/GnssAuxiliary.hpp"
-#endif // CONFIG_UAVCANNODE_GNSS_FIX
+#endif // CONFIG_UAVCANNODE_GNSS_AUXILIARY
+
+#if defined(CONFIG_UAVCANNODE_GNSS_FIX2)
+#include "Publishers/GnssFix2.hpp"
+#endif // CONFIG_UAVCANNODE_GNSS_FIX2
+
+#if defined(CONFIG_UAVCANNODE_GNSS_FIX3)
+#include "Publishers/GnssFix3.hpp"
+#endif // CONFIG_UAVCANNODE_GNSS_FIX3
 
 #if defined(CONFIG_UAVCANNODE_INDICATED_AIR_SPEED)
 #include "Publishers/IndicatedAirspeed.hpp"
@@ -378,10 +385,17 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 	_publisher_list.add(new HygrometerMeasurement(this, _node));
 #endif // UAVCANNODE_HYGROMETER_MEASUREMENT
 
-#if defined(CONFIG_UAVCANNODE_GNSS_FIX)
-	_publisher_list.add(new GnssFix2(this, _node));
+#if defined(CONFIG_UAVCANNODE_GNSS_FIX3)
+	_publisher_list.add(new GnssFix3(this, _node));
+#endif // CONFIG_UAVCANNODE_GNSS_FIX3
+
+#if defined(CONFIG_UAVCANNODE_GNSS_FIX2)
+	_publisher_list.add(new GnssFix2(this, _node)); // Keep Fix2 for backwards compatibility
+#endif // CONFIG_UAVCANNODE_GNSS_FIX2
+
+#if defined(CONFIG_UAVCANNODE_GNSS_AUXILIARY)
 	_publisher_list.add(new GnssAuxiliary(this, _node));
-#endif // CONFIG_UAVCANNODE_GNSS_FIX
+#endif // CONFIG_UAVCANNODE_GNSS_AUXILIARY
 
 #if defined(CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH)
 	int32_t cannode_pub_mag = 1;