gnss(septentrio): implement new MAVLink GNSS_INTEGRITY message

Implement support for the new MAVLink `GNSS_INTEGRITY` message and add
support to the Septentrio driver.

wip

boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp

@@ -1236,6 +1236,10 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
 
 	gps.heading = NAN;
 	gps.heading_offset = NAN;
+	gps.quality_corrections = UINT8_MAX;
+	gps.quality_receiver = UINT8_MAX;
+	gps.quality_gnss_signals = UINT8_MAX;
+	gps.quality_post_processing = UINT8_MAX;
 
 	gps.timestamp = hrt_absolute_time();
 

boards/px4/fmu-v6x/default.px4board

@@ -12,14 +12,11 @@ CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
-CONFIG_DRIVERS_CAMERA_CAPTURE=y
-CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
-CONFIG_DRIVERS_DSHOT=y
+CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
 CONFIG_DRIVERS_GPS=y
-CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y

msg/SensorGps.msg

@@ -30,19 +30,29 @@ float32 vdop			# Vertical dilution of precision
 int32 noise_per_ms		# GPS noise per millisecond
 uint16 automatic_gain_control   # Automatic gain control monitor
 
-uint8 JAMMING_STATE_UNKNOWN  = 0
-uint8 JAMMING_STATE_OK       = 1
-uint8 JAMMING_STATE_WARNING  = 2
-uint8 JAMMING_STATE_CRITICAL = 3
+uint8 JAMMING_STATE_UNKNOWN   = 0
+uint8 JAMMING_STATE_OK        = 1
+uint8 JAMMING_STATE_WARNING   = 2
+uint8 JAMMING_STATE_CRITICAL  = 3
+uint8 JAMMING_STATE_MITIGATED = 4
 uint8 jamming_state		# indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical
 int32 jamming_indicator		# indicates jamming is occurring
 
 uint8 SPOOFING_STATE_UNKNOWN   = 0
 uint8 SPOOFING_STATE_NONE      = 1
 uint8 SPOOFING_STATE_INDICATED = 2
 uint8 SPOOFING_STATE_MULTIPLE  = 3
+uint8 SPOOFING_STATE_MITIGATED = 4
 uint8 spoofing_state		# indicates whether spoofing has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical
 
+# Combined authentication state (e.g. Galileo OSNMA)
+uint8 AUTHENTICATION_STATE_UNKNOWN      = 0
+uint8 AUTHENTICATION_STATE_DISABLED     = 1
+uint8 AUTHENTICATION_STATE_INITIALIZING = 2
+uint8 AUTHENTICATION_STATE_FAILED       = 3
+uint8 AUTHENTICATION_STATE_OK           = 4
+uint8 authentication_state      # GPS signal authentication state
+
 float32 vel_m_s			# GPS ground speed, (metres/sec)
 float32 vel_n_m_s		# GPS North velocity, (metres/sec)
 float32 vel_e_m_s		# GPS East velocity, (metres/sec)
@@ -55,6 +65,16 @@ uint64 time_utc_usec		# Timestamp (microseconds, UTC), this is the timestamp whi
 
 uint8 satellites_used		# Number of satellites used
 
+uint32 SYSTEM_ERROR_OK                   = 0
+uint32 SYSTEM_ERROR_INCOMING_CORRECTIONS = 1
+uint32 SYSTEM_ERROR_CONFIGURATION        = 2
+uint32 SYSTEM_ERROR_SOFTWARE             = 4
+uint32 SYSTEM_ERROR_ANTENNA              = 8
+uint32 SYSTEM_ERROR_EVENT_CONGESTION     = 16
+uint32 SYSTEM_ERROR_CPU_OVERLOAD         = 32
+uint32 SYSTEM_ERROR_OUTPUT_CONGESTION    = 64
+uint32 system_error             # General errors with the connected GPS receiver
+
 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])
@@ -69,4 +89,9 @@ uint8 RTCM_MSG_USED_NOT_USED = 1
 uint8 RTCM_MSG_USED_USED = 2
 uint8 rtcm_msg_used		# Indicates if the RTCM message was used successfully by the receiver
 
+uint8 quality_corrections       # Abstract corrections quality from 0 to 10, or 255 if not available
+uint8 quality_receiver          # Abstract overall receiver operating status from 0 to 10, or 255 if not available
+uint8 quality_gnss_signals      # Abstract quality of GNSS signals from 0 to 10, or 255 if not available
+uint8 quality_post_processing   # Abstract expected post processing quality from 0 to 10, or 255 if not available
+
 # TOPICS sensor_gps vehicle_gps_position

src/drivers/gnss/septentrio/CMakeLists.txt

@@ -37,7 +37,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		# -DDEBUG_BUILD           # Enable during development of the module
 		-DSEP_LOG_ERROR         # Enable module-level error logs
-		# -DSEP_LOG_WARN          # Enable module-level warning logs
+		-DSEP_LOG_WARN          # Enable module-level warning logs
 		# -DSEP_LOG_INFO          # Enable module-level info logs
 		# -DSEP_LOG_TRACE_PARSING # Tracing of parsing steps
 	SRCS

src/drivers/gnss/septentrio/sbf/messages.h

@@ -243,9 +243,14 @@ struct QualityInd {
 };
 
 struct RFBand {
+	enum class InfoMode : uint8_t {
+		Suppressed   = 1,
+		Mitigated    = 2,
+		Interference = 8
+	};
 	uint32_t frequency;
 	uint16_t bandwidth;
-	uint8_t  info_mode: 4;
+	InfoMode info_mode: 4;
 	uint8_t  info_reserved: 2;
 	uint8_t  info_antenna_id: 2;
 };
@@ -261,7 +266,16 @@ struct RFStatus {
 };
 
 struct GALAuthStatus {
-	uint16_t osnma_status_status: 3;
+	enum class OSNMAStatus : uint16_t {
+		Disabled                   = 0,
+		Initializing               = 1,
+		AwaitingTrustedTimeInfo    = 2,
+		InitFailedInconsistentTime = 3,
+		InitFailedKROOTInvalid     = 4,
+		InitFailedInvalidParam     = 5,
+		Authenticating             = 6,
+	};
+	OSNMAStatus osnma_status_status: 3;
 	uint16_t osnma_status_initialization_progress: 8;
 	uint16_t osnma_status_trusted_time_source: 3;
 	uint16_t osnma_status_merkle_tree_busy: 1;

src/drivers/gnss/septentrio/septentrio.cpp

@@ -86,7 +86,7 @@ constexpr int k_max_receiver_read_timeout = 50;
 */
 constexpr size_t k_min_receiver_read_bytes = 32;
 
-constexpr uint8_t k_max_command_size            = 120;
+constexpr uint8_t k_max_command_size            = 140;
 constexpr uint16_t k_timeout_5hz                = 500;
 constexpr uint32_t k_read_buffer_size           = 150;
 constexpr time_t k_gps_epoch_secs               = 1234567890ULL; // TODO: This seems wrong
@@ -106,7 +106,7 @@ constexpr const char *k_command_reset_hot = "erst,soft,none\n";
 constexpr const char *k_command_reset_warm = "erst,soft,PVTData\n";
 constexpr const char *k_command_reset_cold = "erst,hard,SatData\n";
 constexpr const char *k_command_sbf_output_pvt =
-	"sso,Stream%lu,%s,PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler+EndOfPVT+ReceiverStatus,%s\n";
+	"sso,Stream%lu,%s,PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler+EndOfPVT+ReceiverStatus+GALAuthStatus+RFStatus+QualityInd,%s\n";
 constexpr const char *k_command_set_sbf_output =
 	"sso,Stream%lu,%s,%s%s,%s\n";
 constexpr const char *k_command_clear_sbf = "sso,Stream%lu,%s,none,off\n";
@@ -223,6 +223,7 @@ SeptentrioDriver::~SeptentrioDriver()
 	if (_rtcm_decoder) {
 		delete _rtcm_decoder;
 	}
+	PX4_WARN("Device id: %lu", get_device_id());
 }
 
 int SeptentrioDriver::print_status()
@@ -1148,20 +1149,116 @@ int SeptentrioDriver::process_message()
 
 			if (_sbf_decoder.parse(&receiver_status) == PX4_OK) {
 				_message_gps_state.rtcm_msg_used = receiver_status.rx_state_diff_corr_in ? sensor_gps_s::RTCM_MSG_USED_USED : sensor_gps_s::RTCM_MSG_USED_NOT_USED;
+
+				_message_gps_state.system_error = sensor_gps_s::SYSTEM_ERROR_OK;
+
+				if (receiver_status.rx_error_cpu_overload)
+					_message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_CPU_OVERLOAD;
+				if (receiver_status.rx_error_antenna)
+					_message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_ANTENNA;
+				if (receiver_status.ext_error_diff_corr_error)
+					_message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_INCOMING_CORRECTIONS;
+				if (receiver_status.ext_error_setup_error)
+					_message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_CONFIGURATION;
+				if (receiver_status.rx_error_software)
+					_message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_SOFTWARE;
+				if (receiver_status.rx_error_congestion)
+					_message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_OUTPUT_CONGESTION;
+				if (receiver_status.rx_error_missed_event)
+					_message_gps_state.system_error |= sensor_gps_s::SYSTEM_ERROR_EVENT_CONGESTION;
 			}
 
 			break;
 		}
 		case BlockID::QualityInd: {
+			using Type = QualityIndicator::Type;
+
 			SEP_TRACE_PARSING("Processing QualityInd SBF message");
+
+			QualityInd quality_ind;
+
+			if (_sbf_decoder.parse(&quality_ind) == PX4_OK) {
+				_message_gps_state.quality_gnss_signals = 0;
+				for (int i = 0; i < math::min(quality_ind.n, static_cast<uint8_t>(sizeof(quality_ind.indicators) / sizeof(quality_ind.indicators[0]))); i++) {
+					switch (quality_ind.indicators[i].type) {
+					case Type::BaseStationMeasurements:
+						_message_gps_state.quality_corrections = quality_ind.indicators[i].value;
+						break;
+					case Type::Overall:
+						_message_gps_state.quality_receiver = quality_ind.indicators[i].value;
+						break;
+					case Type::RTKPostProcessing:
+						_message_gps_state.quality_post_processing = quality_ind.indicators[i].value;
+						break;
+					case Type::GNSSSignalsMainAntenna:
+						_message_gps_state.quality_gnss_signals += quality_ind.indicators[i].value;
+						break;
+					default:
+						break;
+					}
+				}
+			}
+
 			break;
 		}
 		case BlockID::RFStatus: {
+			using InfoMode = RFBand::InfoMode;
+
 			SEP_TRACE_PARSING("Processing RFStatus SBF message");
+
+			RFStatus rf_status;
+
+			if (_sbf_decoder.parse(&rf_status) == PX4_OK) {
+				_message_gps_state.jamming_state = sensor_gps_s::JAMMING_STATE_OK;
+				_message_gps_state.spoofing_state = sensor_gps_s::SPOOFING_STATE_NONE;
+				for (int i = 0; i < math::min(rf_status.n, static_cast<uint8_t>(sizeof(rf_status.rf_band) / sizeof(rf_status.rf_band[0]))); i++) {
+					switch (rf_status.rf_band[i].info_mode) {
+					case InfoMode::Interference:
+						_message_gps_state.jamming_state = sensor_gps_s::JAMMING_STATE_CRITICAL;
+						_message_gps_state.spoofing_state = sensor_gps_s::SPOOFING_STATE_INDICATED;
+						break;
+					case InfoMode::Mitigated:
+						// Don't report mitigated when there is unmitigated interference in one band.
+						if (_message_gps_state.spoofing_state == sensor_gps_s::SPOOFING_STATE_NONE) {
+							_message_gps_state.jamming_state = sensor_gps_s::JAMMING_STATE_OK;
+							_message_gps_state.spoofing_state = sensor_gps_s::SPOOFING_STATE_MITIGATED;
+						}
+						break;
+					default:
+						break;
+					}
+				}
+			}
+
 			break;
 		}
 		case BlockID::GALAuthStatus: {
+			using OSNMAStatus = GALAuthStatus::OSNMAStatus;
+
 			SEP_TRACE_PARSING("Processing GALAuthStatus SBF message");
+
+			GALAuthStatus gal_auth_status;
+
+			if (_sbf_decoder.parse(&gal_auth_status) == PX4_OK) {
+				switch (gal_auth_status.osnma_status_status) {
+				case OSNMAStatus::Disabled:
+					_message_gps_state.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_DISABLED;
+					break;
+				case OSNMAStatus::AwaitingTrustedTimeInfo:
+				case OSNMAStatus::Initializing:
+					_message_gps_state.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_INITIALIZING;
+					break;
+				case OSNMAStatus::InitFailedInconsistentTime:
+				case OSNMAStatus::InitFailedKROOTInvalid:
+				case OSNMAStatus::InitFailedInvalidParam:
+					_message_gps_state.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_FAILED;
+					break;
+				case OSNMAStatus::Authenticating:
+					_message_gps_state.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_OK;
+					break;
+				}
+			}
+
 			break;
 		}
 		case BlockID::EndOfPVT: {

src/drivers/gps/gps.cpp

@@ -46,6 +46,7 @@
 #endif
 
 #include <cstring>
+#include <cstdint>
 
 #include <drivers/drv_sensor.h>
 #include <lib/drivers/device/Device.hpp>
@@ -315,6 +316,10 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 
 	_report_gps_pos.heading = NAN;
 	_report_gps_pos.heading_offset = NAN;
+	_report_gps_pos.quality_corrections = UINT8_MAX;
+	_report_gps_pos.quality_receiver = UINT8_MAX;
+	_report_gps_pos.quality_gnss_signals = UINT8_MAX;
+	_report_gps_pos.quality_post_processing = UINT8_MAX;
 
 	int32_t enable_sat_info = 0;
 	param_get(param_find("GPS_SAT_INFO"), &enable_sat_info);
@@ -907,6 +912,10 @@ GPS::run()
 			memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
 			_report_gps_pos.heading = NAN;
 			_report_gps_pos.heading_offset = heading_offset;
+			_report_gps_pos.quality_corrections = UINT8_MAX;
+			_report_gps_pos.quality_receiver = UINT8_MAX;
+			_report_gps_pos.quality_gnss_signals = UINT8_MAX;
+			_report_gps_pos.quality_post_processing = UINT8_MAX;
 
 			if (_mode == gps_driver_mode_t::UBX) {
 
@@ -1188,6 +1197,10 @@ GPS::publish()
 		// Heading/yaw data can be updated at a lower rate than the other navigation data.
 		// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
 		_report_gps_pos.heading = NAN;
+		_report_gps_pos.quality_corrections = UINT8_MAX;
+		_report_gps_pos.quality_receiver = UINT8_MAX;
+		_report_gps_pos.quality_gnss_signals = UINT8_MAX;
+		_report_gps_pos.quality_post_processing = UINT8_MAX;
 		_is_gps_main_advertised.store(true);
 
 		if (_report_gps_pos.spoofing_state != _spoofing_state) {

src/drivers/ins/vectornav/VectorNav.cpp

@@ -450,6 +450,13 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 			sensor_gps.hdop = dop.hDOP;
 			sensor_gps.vdop = dop.vDOP;
 
+			sensor_gps.heading = NAN;
+			sensor_gps.heading_offset = NAN;
+			sensor_gps.quality_corrections = UINT8_MAX;
+			sensor_gps.quality_receiver = UINT8_MAX;
+			sensor_gps.quality_gnss_signals = UINT8_MAX;
+			sensor_gps.quality_post_processing = UINT8_MAX;
+
 			sensor_gps.eph = sqrtf(sq(positionUncertaintyGpsNed.c[0]) + sq(positionUncertaintyGpsNed.c[1]));
 			sensor_gps.epv = positionUncertaintyGpsNed.c[2];
 

src/drivers/uavcan/sensors/gnss.cpp

@@ -466,6 +466,10 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	report.heading = heading;
 	report.heading_offset = heading_offset;
 	report.heading_accuracy = heading_accuracy;
+	report.quality_corrections = UINT8_MAX;
+	report.quality_receiver = UINT8_MAX;
+	report.quality_gnss_signals = UINT8_MAX;
+	report.quality_post_processing = UINT8_MAX;
 
 	report.noise_per_ms = noise_per_ms;
 	report.jamming_indicator = jamming_indicator;

src/examples/fake_gps/FakeGps.cpp

@@ -80,6 +80,10 @@ void FakeGps::Run()
 	sensor_gps.timestamp_time_relative = 0;
 	sensor_gps.heading = NAN;
 	sensor_gps.heading_offset = 0.0000;
+	sensor_gps.quality_corrections = UINT8_MAX;
+	sensor_gps.quality_receiver = UINT8_MAX;
+	sensor_gps.quality_gnss_signals = UINT8_MAX;
+	sensor_gps.quality_post_processing = UINT8_MAX;
 	sensor_gps.fix_type = 4;
 	sensor_gps.jamming_state = 0;
 	sensor_gps.spoofing_state = 0;

src/modules/mavlink/mavlink_main.cpp

@@ -1417,6 +1417,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
 		configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
 		configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
+		configure_stream_local("GNSS_INTEGRITY", 1.0f);
 		configure_stream_local("GPS2_RAW", 1.0f);
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS_RAW_INT", 5.0f);
@@ -1486,6 +1487,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
 		configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
 		configure_stream_local("GLOBAL_POSITION_INT", 50.0f);
+		configure_stream_local("GNSS_INTEGRITY", 1.0f);
 		configure_stream_local("GPS2_RAW", unlimited_rate);
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
@@ -1643,6 +1645,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ESTIMATOR_STATUS", 5.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 2.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
+		configure_stream_local("GNSS_INTEGRITY", 1.0f);
 		configure_stream_local("GPS2_RAW", unlimited_rate);
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
@@ -1738,6 +1741,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
+		configure_stream_local("GNSS_INTEGRITY", 1.0f);
 		configure_stream_local("GPS2_RAW", unlimited_rate);
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
 		configure_stream_local("HOME_POSITION", 0.5f);