Septentrio GNSS resilience reporting (PX4#25012)

Co-authored-by: Tory9 <vvpost05@gmail.com>

Author: 2 people

msg/CMakeLists.txt

@@ -195,6 +195,7 @@ set(msg_files
 	SensorCombined.msg
 	SensorCorrection.msg
 	SensorGnssRelative.msg
+	SensorGnssStatus.msg
 	SensorGps.msg
 	SensorGyro.msg
 	SensorGyroFft.msg

msg/SensorGnssStatus.msg

@@ -0,0 +1,10 @@
+# Gnss quality indicators
+
+uint64 timestamp		# time since system start (microseconds)
+uint32 device_id                # unique device ID for the sensor that does not change between power cycles
+
+bool  quality_available         # Set to true if quality indicators are available
+uint8 quality_corrections       # Corrections quality from 0 to 10, or 255 if not available
+uint8 quality_receiver          # Overall receiver operating status from 0 to 10, or 255 if not available
+uint8 quality_gnss_signals      # Quality of GNSS signals from 0 to 10, or 255 if not available
+uint8 quality_post_processing   # Expected post processing quality from 0 to 10, or 255 if not available

msg/SensorGps.msg

@@ -30,18 +30,26 @@ 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		# 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		# indicates whether spoofing has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical
+uint8 JAMMING_STATE_UNKNOWN   = 0 #default
+uint8 JAMMING_STATE_OK        = 1
+uint8 JAMMING_STATE_MITIGATED = 2
+uint8 JAMMING_STATE_DETECTED  = 3
+uint8 jamming_state	      # indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Mitigated, 3: Detected
+int32 jamming_indicator	      # indicates jamming is occurring
+
+uint8 SPOOFING_STATE_UNKNOWN   = 0 #default
+uint8 SPOOFING_STATE_OK        = 1
+uint8 SPOOFING_STATE_MITIGATED = 2
+uint8 SPOOFING_STATE_DETECTED  = 3
+uint8 spoofing_state	       # indicates whether spoofing has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Mitigated, 3: Detected
+
+# Combined authentication state (e.g. Galileo OSNMA)
+uint8 AUTHENTICATION_STATE_UNKNOWN      = 0 #default
+uint8 AUTHENTICATION_STATE_INITIALIZING = 1
+uint8 AUTHENTICATION_STATE_ERROR        = 2
+uint8 AUTHENTICATION_STATE_OK           = 3
+uint8 AUTHENTICATION_STATE_DISABLED     = 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)
@@ -55,6 +63,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 #default
+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])

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/decoder.cpp

@@ -163,7 +163,7 @@ int Decoder::parse(QualityInd *message) const
 
 int Decoder::parse(RFStatus *message) const
 {
-	if (can_parse() && id() == BlockID::PVTGeodetic) {
+	if (can_parse() && id() == BlockID::RFStatus) {
 		static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
 		memcpy(message, _message.payload, sizeof(RFStatus) - sizeof(RFStatus::rf_band));
 

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;
+	uint8_t info_mode: 4;
 	uint8_t  info_reserved: 2;
 	uint8_t  info_antenna_id: 2;
 };
@@ -261,6 +266,15 @@ struct RFStatus {
 };
 
 struct GALAuthStatus {
+	enum class OSNMAStatus : uint16_t {
+		Disabled                   = 0,
+		Initializing               = 1,
+		AwaitingTrustedTimeInfo    = 2,
+		InitFailedInconsistentTime = 3,
+		InitFailedKROOTInvalid     = 4,
+		InitFailedInvalidParam     = 5,
+		Authenticating             = 6,
+	};
 	uint16_t osnma_status_status: 3;
 	uint16_t osnma_status_initialization_progress: 8;
 	uint16_t osnma_status_trusted_time_source: 3;
@@ -271,6 +285,8 @@ struct GALAuthStatus {
 	uint64_t gal_authentic_mask;
 	uint64_t gps_active_mask;
 	uint64_t gps_authentic_mask;
+
+	OSNMAStatus osnmaStatus() const { return static_cast<OSNMAStatus>(osnma_status_status); }
 };
 
 struct VelCovGeodetic {

src/drivers/gnss/septentrio/septentrio.cpp

@@ -92,7 +92,7 @@ constexpr size_t k_min_receiver_read_bytes = 32;
 */
 constexpr uint32_t k_septentrio_receiver_default_baud_rate = 115200;
 
-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
@@ -112,7 +112,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%" PRIu32 ",%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%" PRIu32 ",%s,%s%s,%s\n";
 constexpr const char *k_command_clear_sbf = "sso,Stream%" PRIu32 ",%s,none,off\n";
@@ -967,7 +967,7 @@ SeptentrioDriver::ConfigureResult SeptentrioDriver::configure()
 	}
 
 	// Output a set of SBF blocks on a given connection at a regular interval.
-	snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency);
+	snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, (long unsigned int) _receiver_stream_main, com_port, sbf_frequency);
 	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
 		SEP_WARN("CONFIG: Failed to configure SBF");
 		return ConfigureResult::FailedCompletely;
@@ -1191,20 +1191,138 @@ int SeptentrioDriver::process_message()
 			if (_sbf_decoder.parse(&receiver_status) == PX4_OK) {
 				_sensor_gps.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;
 				_time_synced = receiver_status.rx_state_wn_set && receiver_status.rx_state_tow_set;
+
+				_sensor_gps.system_error = sensor_gps_s::SYSTEM_ERROR_OK;
+
+				if (receiver_status.rx_error_cpu_overload) {
+					_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_CPU_OVERLOAD;
+				}
+				if (receiver_status.rx_error_antenna) {
+					_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_ANTENNA;
+				}
+				if (receiver_status.ext_error_diff_corr_error) {
+					_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_INCOMING_CORRECTIONS;
+				}
+				if (receiver_status.ext_error_setup_error) {
+					_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_CONFIGURATION;
+				}
+				if (receiver_status.rx_error_software) {
+					_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_SOFTWARE;
+				}
+				if (receiver_status.rx_error_congestion) {
+					_sensor_gps.system_error |= sensor_gps_s::SYSTEM_ERROR_OUTPUT_CONGESTION;
+				}
+				if (receiver_status.rx_error_missed_event) {
+					_sensor_gps.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_sensor_gnss_status.quality_available = true;
+				_message_sensor_gnss_status.device_id = get_device_id();
+
+				_message_sensor_gnss_status.quality_corrections = 255;
+				_message_sensor_gnss_status.quality_receiver = 255;
+				_message_sensor_gnss_status.quality_post_processing = 255;
+				_message_sensor_gnss_status.quality_gnss_signals = 255;
+
+				for (int i = 0; i < math::min(quality_ind.n, static_cast<uint8_t>(sizeof(quality_ind.indicators) / sizeof(quality_ind.indicators[0]))); i++) {
+					int quality = quality_ind.indicators[i].value;
+
+					switch (quality_ind.indicators[i].type) {
+					case Type::BaseStationMeasurements:
+						_message_sensor_gnss_status.quality_corrections = quality;
+						break;
+					case Type::Overall:
+						_message_sensor_gnss_status.quality_receiver = quality;
+						break;
+					case Type::RTKPostProcessing:
+						_message_sensor_gnss_status.quality_post_processing = quality;
+						break;
+					case Type::GNSSSignalsMainAntenna:
+						_message_sensor_gnss_status.quality_gnss_signals = quality;
+						break;
+					default:
+						break;
+					}
+				}
+
+
+				_message_sensor_gnss_status.timestamp = hrt_absolute_time();
+				_time_last_qualityind_received = hrt_absolute_time();
+				_sensor_gnss_status_pub.publish(_message_sensor_gnss_status);
+			}
+
 			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) {
+				_sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_OK;
+				_sensor_gps.spoofing_state = sensor_gps_s::SPOOFING_STATE_OK;
+
+				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++) {
+					InfoMode status = static_cast<InfoMode>(rf_status.rf_band[i].info_mode);
+
+					if(status == InfoMode::Interference){
+						_sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_DETECTED;
+						break; // Worst case, we don't need to check the other bands
+					}
+
+					if(status == InfoMode::Suppressed || status == InfoMode::Mitigated){
+						_sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_MITIGATED;
+					}
+				}
+
+				if (rf_status.flags_inauthentic_gnss_signals || rf_status.flags_inauthentic_navigation_message) {
+					_sensor_gps.spoofing_state = sensor_gps_s::SPOOFING_STATE_DETECTED;
+				}
+				_time_last_resilience_received = hrt_absolute_time();
+			}
+
 			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.osnmaStatus()) {
+				case OSNMAStatus::Disabled:
+					_sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_DISABLED;
+					break;
+				case OSNMAStatus::AwaitingTrustedTimeInfo:
+				case OSNMAStatus::Initializing:
+					_sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_INITIALIZING;
+					break;
+				case OSNMAStatus::InitFailedInconsistentTime:
+				case OSNMAStatus::InitFailedKROOTInvalid:
+				case OSNMAStatus::InitFailedInvalidParam:
+					_sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_ERROR;
+					break;
+				case OSNMAStatus::Authenticating:
+					_sensor_gps.authentication_state = sensor_gps_s::AUTHENTICATION_STATE_OK;
+					break;
+				}
+			}
+
 			break;
 		}
 		case BlockID::EndOfPVT: {
@@ -1277,6 +1395,31 @@ int SeptentrioDriver::process_message()
 		}
 		}
 
+		//Check for how recent the resilience data for reciever is, if outdated set to unknown
+		if ((_time_last_resilience_received != 0) && (hrt_elapsed_time(&_time_last_resilience_received) > 5_s)) {
+				_sensor_gps.jamming_state = sensor_gps_s::JAMMING_STATE_UNKNOWN;
+				_sensor_gps.spoofing_state = sensor_gps_s::SPOOFING_STATE_UNKNOWN;
+
+				_time_last_resilience_received = 0; // Reset
+		}
+
+		// Check for how recent the status data for receiver is, if outdated set to unknown
+		if ((_time_last_qualityind_received != 0) && (hrt_elapsed_time(&_time_last_qualityind_received) > 5_s)) {
+				_message_sensor_gnss_status.quality_available = false;
+				_message_sensor_gnss_status.device_id = get_device_id();
+				_message_sensor_gnss_status.timestamp = hrt_absolute_time();
+
+				_message_sensor_gnss_status.quality_corrections = 255;
+				_message_sensor_gnss_status.quality_receiver = 255;
+				_message_sensor_gnss_status.quality_post_processing = 255;
+				_message_sensor_gnss_status.quality_gnss_signals = 255;
+
+
+				_sensor_gnss_status_pub.publish(_message_sensor_gnss_status);
+
+				_time_last_qualityind_received = 0; // Reset
+		}
+
 		break;
 	}
 	case DecodingStatus::RTCMv3: {
@@ -1532,6 +1675,7 @@ void SeptentrioDriver::publish()
 	_sensor_gps.device_id = get_device_id();
 	_sensor_gps.selected_rtcm_instance = _selected_rtcm_instance;
 	_sensor_gps.rtcm_injection_rate = rtcm_injection_frequency();
+	_sensor_gps.timestamp = hrt_absolute_time();
 	_sensor_gps_pub.publish(_sensor_gps);
 }
 

src/drivers/gnss/septentrio/septentrio.h

@@ -53,6 +53,7 @@
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/satellite_info.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_gnss_status.h>
 #include <uORB/topics/gps_dump.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <drivers/drv_hrt.h>
@@ -710,12 +711,16 @@ class SeptentrioDriver : public ModuleBase<SeptentrioDriver>, public device::Dev
 	char                                   _port[20] {};                                                 ///< The path of the used serial device
 	hrt_abstime                            _last_rtcm_injection_time {0};                                ///< Time of last RTCM injection
 	uint8_t                                _selected_rtcm_instance {0};                                  ///< uORB instance that is being used for RTCM corrections
+	uint8_t                                _spoofing_state {0};                                          ///< Receiver spoofing state
+	uint8_t                                _jamming_state {0};                                           ///< Receiver jamming state
 	bool                                   _time_synced {false};                                         ///< Receiver time in sync with GPS time
 	const Instance                         _instance {Instance::Main};                                   ///< The receiver that this instance of the driver controls
 	uint32_t                               _chosen_baud_rate {0};                                        ///< The baud rate requested by the user
 	static px4::atomic<SeptentrioDriver *> _secondary_instance;
 	hrt_abstime                            _sleep_end {0};                                               ///< End time for sleeping
 	State                                  _resume_state {State::OpeningSerialPort};                     ///< Resume state after sleep
+	hrt_abstime                            _time_last_qualityind_received{0};			     ///< Time of last quality message reception
+	hrt_abstime                            _time_last_resilience_received{0};			     ///< Time of last resilience message reception
 
 	// Module configuration
 	float                                  _heading_offset {0.0f};                                       ///< The heading offset given by the `SEP_YAW_OFFS` parameter
@@ -737,14 +742,16 @@ class SeptentrioDriver : public ModuleBase<SeptentrioDriver>, public device::Dev
 	rtcm::Decoder                                  *_rtcm_decoder {nullptr};                    ///< RTCM message decoder
 
 	// uORB topics and subscriptions
-	sensor_gps_s                                   _sensor_gps{};                           ///< uORB topic for position
-	gps_dump_s                                     *_message_data_to_receiver {nullptr};           ///< uORB topic for dumping data to the receiver
-	gps_dump_s                                     *_message_data_from_receiver {nullptr};         ///< uORB topic for dumping data from the receiver
-	satellite_info_s                               *_message_satellite_info {nullptr};             ///< uORB topic for satellite info
-	uORB::PublicationMulti<sensor_gps_s>           _sensor_gps_pub {ORB_ID(sensor_gps)};           ///< uORB publication for gps position
-	uORB::Publication<gps_dump_s>                  _gps_dump_pub {ORB_ID(gps_dump)};               ///< uORB publication for dump GPS data
-	uORB::Publication<gps_inject_data_s>           _gps_inject_data_pub {ORB_ID(gps_inject_data)}; ///< uORB publication for injected data to the receiver
-	uORB::PublicationMulti<satellite_info_s>       _satellite_info_pub {ORB_ID(satellite_info)};   ///< uORB publication for satellite info
+	sensor_gps_s                                   _sensor_gps {};                          		///< uORB topic for position
+	sensor_gnss_status_s                           _message_sensor_gnss_status {};                          ///< uORB topic for gps status
+	gps_dump_s                                     *_message_data_to_receiver {nullptr};           		///< uORB topic for dumping data to the receiver
+	gps_dump_s                                     *_message_data_from_receiver {nullptr};         		///< uORB topic for dumping data from the receiver
+	satellite_info_s                               *_message_satellite_info {nullptr};             		///< uORB topic for satellite info
+	uORB::PublicationMulti<sensor_gps_s>           _sensor_gps_pub {ORB_ID(sensor_gps)};           		///< uORB publication for gps position
+	uORB::PublicationMulti<sensor_gnss_status_s>   _sensor_gnss_status_pub {ORB_ID(sensor_gnss_status)};	///< uORB publication for gnss status
+	uORB::Publication<gps_dump_s>                  _gps_dump_pub {ORB_ID(gps_dump)};              		///< uORB publication for dump GPS data
+	uORB::Publication<gps_inject_data_s>           _gps_inject_data_pub {ORB_ID(gps_inject_data)}; 		///< uORB publication for injected data to the receiver
+	uORB::PublicationMulti<satellite_info_s>       _satellite_info_pub {ORB_ID(satellite_info)};   		///< uORB publication for satellite info
 	uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _gps_inject_data_sub {ORB_ID::gps_inject_data}; ///< uORB subscription about data to inject to the receiver
 
 	// Data about update frequencies of various bits of information like RTCM message injection frequency, received data rate...