feat(ubx): add support for UBX_MSG_MON_SPAN and full support of UBX_MON_RF

README.md

@@ -9,7 +9,7 @@ This repository contains user-space gps drivers, used as a submodule in
 All platform-specific stuff is done via a callback function and a
 `definitions.h` header file.
 
-In order for the project to build, `definitions.h` must include definitions for `sensor_gnss_relative_s`, `sensor_gps_s` and `satellite_info_s`.
+In order for the project to build, `definitions.h` must include definitions for `sensor_gnss_relative_s`, `sensor_gps_s`, `satellite_info_s`, `sensor_gnss_rf_s` (and `sensor_gnss_spectrum_s` when applicable).
 For example, check the implementation in [PX4 Autopilot](https://github.com/PX4/PX4-Autopilot/blob/master/src/drivers/gps/definitions.h) or [QGroundControl](https://github.com/mavlink/qgroundcontrol/blob/master/src/GPS/definitions.h).
 
 

src/gps_helper.h

@@ -114,6 +114,24 @@ enum class GPSCallbackType {
 	 * return: ignored
 	 */
 	setClock,
+
+	/**
+	 * Got an RF message from the device.
+	 * data1: pointer to the message
+	 * data2: message length
+	 * return: ignored
+	 */
+	gotRFMessage,
+
+#if defined(CONFIG_GPS_UBX_SPAN)
+	/**
+	 * Got a spectrum message from the device.
+	 * data1: pointer to the message
+	 * data2: message length
+	 * return: ignored
+	 */
+	gotSpectrumMessage,
+#endif
 };
 
 enum class GPSRestartType {
@@ -320,6 +338,20 @@ class GPSHelper
 		_callback(GPSCallbackType::gotRelativePositionMessage, &gnss_relative, sizeof(sensor_gnss_relative_s), _callback_user);
 	}
 
+	/** got an RF message from the device */
+	void gotRFMessage(sensor_gnss_rf_s &gnss_rf)
+	{
+		_callback(GPSCallbackType::gotRFMessage, &gnss_rf, sizeof(sensor_gnss_rf_s), _callback_user);
+	}
+
+#if defined(CONFIG_GPS_UBX_SPAN)
+	/** got a spectrum message from the device */
+	void gotSpectrumMessage(sensor_gnss_spectrum_s &gnss_spectrum)
+	{
+		_callback(GPSCallbackType::gotSpectrumMessage, &gnss_spectrum, sizeof(sensor_gnss_spectrum_s), _callback_user);
+	}
+#endif
+
 	void setClock(timespec &t)
 	{
 		_callback(GPSCallbackType::setClock, &t, 0, _callback_user);

src/ubx.cpp

@@ -108,7 +108,11 @@ GPSDriverUBX::GPSDriverUBX(Interface gpsInterface, GPSCallbackPtr callback, void
 	_uart1_baudrate(settings.uart1_baudrate),
 	_uart2_baudrate(settings.uart2_baudrate),
 	_ppk_output(settings.ppk_output),
+#if defined(CONFIG_GPS_UBX_SPAN)
+	_spectrum_analyzer(settings.spectrum_analyzer),
+#endif
 	_jam_det_sensitivity_hi(settings.jam_det_sensitivity_hi)
+
 {
 	decodeInit();
 }
@@ -1367,6 +1371,19 @@ int GPSDriverUBX::configureDevice(const GPSConfig &config, const int32_t uart2_b
 		}
 	}
 
+#if defined(CONFIG_GPS_UBX_SPAN)
+
+	// UBX_MSG_MON_SPAN
+	if (_spectrum_analyzer) {
+		UBX_DEBUG("Configuration spectrum analyzer");
+		initCfgValset();
+		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_MON_SPAN_UART1, 1);
+
+		sendCfgValsetAcked(true);
+	}
+
+#endif
+
 	return 0;
 }
 
@@ -2107,8 +2124,9 @@ GPSDriverUBX::payloadRxInit()
 		break;
 
 	case UBX_MSG_MON_RF:
-		if (_rx_payload_length < sizeof(ubx_payload_rx_mon_rf_t) ||
-		    (_rx_payload_length - 4) % sizeof(ubx_payload_rx_mon_rf_t::ubx_payload_rx_mon_rf_block_t) != 0) {
+		if (_rx_payload_length > sizeof(ubx_payload_rx_mon_rf_t) ||
+		    _rx_payload_length <= kHeaderSizeMonRFSpan ||
+		    (_rx_payload_length - kHeaderSizeMonRFSpan) % sizeof(ubx_payload_rx_mon_rf_t::ubx_payload_rx_mon_rf_block_t) != 0) {
 
 			_rx_state = UBX_RXMSG_ERROR_LENGTH;
 
@@ -2128,6 +2146,22 @@ GPSDriverUBX::payloadRxInit()
 
 		break;
 
+#if defined(CONFIG_GPS_UBX_SPAN)
+
+	case UBX_MSG_MON_SPAN:
+		if ((_rx_payload_length > sizeof(ubx_payload_rx_mon_span_t)) ||
+		    (_rx_payload_length <= kHeaderSizeMonRFSpan) ||
+		    (_rx_payload_length - kHeaderSizeMonRFSpan) % sizeof(ubx_payload_rx_mon_span_t::ubx_payload_rx_mon_span_block_t) != 0) {
+
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+
+		} else if (!_configured) {
+			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
+		}
+
+		break;
+#endif
+
 	case UBX_MSG_RXM_RTCM:
 		if (_rx_payload_length != sizeof(ubx_payload_rx_rxm_rtcm_t)) {
 			_rx_state = UBX_RXMSG_ERROR_LENGTH;
@@ -2784,7 +2818,7 @@ GPSDriverUBX::payloadRxDone()
 	case UBX_MSG_NAV_SVINFO:
 		UBX_TRACE_RXMSG("Rx NAV-SVINFO");
 
-		// _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp
+		// _satellite_info already populated by payloadRxAddNavSvinfo(), just add a timestamp
 		_satellite_info->timestamp = gps_absolute_time();
 
 		ret = 2;
@@ -3023,6 +3057,94 @@ GPSDriverUBX::payloadRxDone()
 			_gps_position->jamming_state = _buf.payload_rx_mon_rf.block[0].flags & 0x03;
 		}
 
+		{
+			const uint64_t timestamp_sample = gps_absolute_time(); // TODO: adjust with delay estimate
+
+			int rf_blocks = _buf.payload_rx_mon_rf.nBlocks;
+
+			if (rf_blocks > kMaxBlocks) {
+				rf_blocks = kMaxBlocks;
+			}
+
+			// Extract all RF metrics
+			for (int i = 0; i < rf_blocks; i++) {
+				sensor_gnss_rf_s gnss_rf{};
+				gnss_rf.timestamp_sample = timestamp_sample;
+				gnss_rf.block_id = _buf.payload_rx_mon_rf.block[i].blockId;
+				gnss_rf.antenna_status = _buf.payload_rx_mon_rf.block[i].antStatus;
+				gnss_rf.antenna_power = _buf.payload_rx_mon_rf.block[i].antPower;
+				gnss_rf.post_status = _buf.payload_rx_mon_rf.block[i].postStatus;
+				gnss_rf.noise_per_ms = _buf.payload_rx_mon_rf.block[i].noisePerMS;
+				gnss_rf.automatic_gain_control = _buf.payload_rx_mon_rf.block[i].agcCnt;
+				gnss_rf.jamming_indicator = _buf.payload_rx_mon_rf.block[i].jamInd;
+				gnss_rf.jamming_state = _buf.payload_rx_mon_rf.block[i].flags;
+				gnss_rf.i_offset = _buf.payload_rx_mon_rf.block[i].ofsI;
+				gnss_rf.i_magnitude = _buf.payload_rx_mon_rf.block[i].magI;
+				gnss_rf.q_offset = _buf.payload_rx_mon_rf.block[i].ofsQ;
+				gnss_rf.q_magnitude = _buf.payload_rx_mon_rf.block[i].magQ;
+
+				static constexpr uint32_t kNominalL1Freq = 1575420000; // 1575.42 MHz
+				static constexpr uint32_t kNominalL2Freq = 1227600000; // 1227.6 MHz
+				static constexpr uint32_t kNominalL3Freq = 1202025000; // 1202.025 MHz
+				static constexpr uint32_t kNominalL5Freq = 1176450000; // 1176.45 MHz
+
+				if (_board == Board::u_blox_X20) {
+					// Fill the center frequency with the nominal values
+					uint8_t rf_block_band = _buf.payload_rx_mon_rf.block[i].rfBlockGnssBand;
+
+					switch (rf_block_band) {
+					case 1: // L1
+						gnss_rf.center_frequency = kNominalL1Freq;
+						break;
+
+					case 2: // L2
+						gnss_rf.center_frequency = kNominalL2Freq;
+						break;
+
+					case 3: // L3
+						gnss_rf.center_frequency = kNominalL3Freq;
+						break;
+
+					case 4: // L5
+						gnss_rf.center_frequency = kNominalL5Freq;
+						break;
+
+					case 0:
+					default: // unknown or unsupported
+						gnss_rf.center_frequency = 0;
+						break;
+					}
+
+				} else if (_board == Board::u_blox9_F9P_L1L2) {
+					if (i == 0) { // L1
+						gnss_rf.center_frequency = kNominalL1Freq;
+
+					} else if (i == 1) { // L2
+						gnss_rf.center_frequency = kNominalL2Freq;
+
+					} else { // should never be the case
+						gnss_rf.center_frequency = 0;
+					}
+
+				} else if (_board == Board::u_blox9_F9P_L1L5 || _board == Board::u_blox10_L1L5) {
+					if (i == 0) { // L1
+						gnss_rf.center_frequency = kNominalL1Freq;
+
+					} else if (i == 1) { // L5
+						gnss_rf.center_frequency = kNominalL5Freq;
+
+					} else { // should never be the case
+						gnss_rf.center_frequency = 0;
+					}
+
+				} else { // unknown or unsupported for other modules
+					gnss_rf.center_frequency = 0;
+				}
+
+				gotRFMessage(gnss_rf);
+			}
+		}
+
 		ret = 1;
 		break;
 
@@ -3078,6 +3200,39 @@ GPSDriverUBX::payloadRxDone()
 		ret = 1;
 		break;
 
+#if defined(CONFIG_GPS_UBX_SPAN)
+
+	case UBX_MSG_MON_SPAN:
+		UBX_TRACE_RXMSG("Rx MON-SPAN");
+
+		if (_spectrum_analyzer) {
+			const uint64_t timestamp_sample = gps_absolute_time(); // TODO: adjust with delay estimate
+
+			int rf_blocks = _buf.payload_rx_mon_span.numRfBlocks;
+
+			if (rf_blocks > kMaxBlocks) {
+				rf_blocks = kMaxBlocks;
+			}
+
+			for (int i = 0; i < rf_blocks; i++) {
+				sensor_gnss_spectrum_s gnss_spectrum{};
+				gnss_spectrum.timestamp_sample = timestamp_sample;
+				gnss_spectrum.block_id = i; // no blockId sent by gnss
+				memcpy(gnss_spectrum.spectrum, _buf.payload_rx_mon_span.block[i].spectrum,
+				       sizeof(ubx_payload_rx_mon_span_t::ubx_payload_rx_mon_span_block_t::spectrum));
+				gnss_spectrum.spectrum_span = _buf.payload_rx_mon_span.block[i].span;
+				gnss_spectrum.resolution = _buf.payload_rx_mon_span.block[i].res;
+				gnss_spectrum.center_frequency = _buf.payload_rx_mon_span.block[i].center;
+				gnss_spectrum.programmable_gain_amplifier = _buf.payload_rx_mon_span.block[i].pga;
+
+				gotSpectrumMessage(gnss_spectrum);
+			}
+		}
+
+		ret = 1;
+		break;
+#endif
+
 	case UBX_MSG_RXM_RTCM:
 		UBX_TRACE_RXMSG("Rx RXM-RTCM");
 

src/ubx.h

@@ -118,6 +118,7 @@
 #define UBX_ID_CFG_VALDEL     0x8C
 #define UBX_ID_MON_VER        0x04
 #define UBX_ID_MON_HW         0x09 // deprecated in protocol version >= 27 -> use MON_RF
+#define UBX_ID_MON_SPAN       0x31
 #define UBX_ID_MON_RF         0x38
 #define UBX_ID_SEC_SIG        0x09
 
@@ -178,6 +179,7 @@
 #define UBX_MSG_MON_VER       ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
 #define UBX_MSG_MON_RF        ((UBX_CLASS_MON) | UBX_ID_MON_RF << 8)
 #define UBX_MSG_SEC_SIG       ((UBX_CLASS_SEC) | UBX_ID_SEC_SIG << 8)
+#define UBX_MSG_MON_SPAN      ((UBX_CLASS_MON) | UBX_ID_MON_SPAN << 8)
 #define UBX_MSG_RTCM3_1005    ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1005 << 8)
 #define UBX_MSG_RTCM3_1077    ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1077 << 8)
 #define UBX_MSG_RTCM3_1087    ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1087 << 8)
@@ -406,6 +408,8 @@
 #define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_I2C  0x20910303
 #define UBX_CFG_KEY_MSGOUT_UBX_NAV_TIMEGPS_I2C	 0x20910047
 
+#define UBX_CFG_KEY_MSGOUT_UBX_MON_SPAN_UART1 0x2091038c
+
 #define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART1  0x209102ff
 #define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_1_UART1  0x20910382
 #define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_UART1    0x209102cd
@@ -714,30 +718,34 @@ typedef struct {
 	uint8_t reserved0[56];
 } ubx_payload_rx_mon_hw_deprecated_t;
 
+static constexpr uint8_t kMaxBlocks = 3;  ///< handle up to 3 blocks
+static constexpr uint16_t kHeaderSizeMonRFSpan = 4; ///< header size of MON-RF and MON-SPAN messages
+
 /* Rx MON-RF (replaces MON-HW, protocol 27+) */
 typedef struct {
 	uint8_t version;
 	uint8_t nBlocks;         /**< number of RF blocks included */
-	uint8_t reserved1[2];
+	uint8_t reserved0[2];
 
 	struct ubx_payload_rx_mon_rf_block_t {
-		uint8_t  blockId;       /**< RF block id */
-		uint8_t  flags;         /**< jammingState */
-		uint8_t  antStatus;     /**< Status of the antenna superior state machine */
-		uint8_t  antPower;      /**< Current power status of antenna */
-		uint32_t postStatus;    /**< POST status word */
-		uint8_t  reserved2[4];
-		uint16_t noisePerMS;    /**< Noise level as measured by the GPS core */
-		uint16_t agcCnt;        /**< AGC Monitor (counts SIGI xor SIGLO, range 0 to 8191 */
-		uint8_t  jamInd;        /**< CW jamming indicator, scaled (0=no CW jamming, 255=strong CW jamming) */
-		int8_t   ofsI;          /**< Imbalance of I-part of complex signal */
-		uint8_t  magI;          /**< Magnitude of I-part of complex signal (0=no signal, 255=max magnitude) */
-		int8_t   ofsQ;          /**< Imbalance of Q-part of complex signal */
-		uint8_t  magQ;          /**< Magnitude of Q-part of complex signal (0=no signal, 255=max magnitude) */
-		uint8_t  reserved3[3];
+		uint8_t  blockId;        /**< RF block id */
+		uint8_t  flags;          /**< jammingState */
+		uint8_t  antStatus;      /**< Status of the antenna superior state machine */
+		uint8_t  antPower;       /**< Current power status of antenna */
+		uint32_t postStatus;     /**< POST status word */
+		uint8_t  reserved1[4];
+		uint16_t noisePerMS;     /**< Noise level as measured by the GPS core */
+		uint16_t agcCnt;         /**< AGC Monitor (counts SIGI xor SIGLO, range 0 to 8191 */
+		uint8_t  jamInd;         /**< CW jamming indicator, scaled (0=no CW jamming, 255=strong CW jamming) */
+		int8_t   ofsI;           /**< Imbalance of I-part of complex signal */
+		uint8_t  magI;           /**< Magnitude of I-part of complex signal (0=no signal, 255=max magnitude) */
+		int8_t   ofsQ;           /**< Imbalance of Q-part of complex signal */
+		uint8_t  magQ;           /**< Magnitude of Q-part of complex signal (0=no signal, 255=max magnitude) */
+		uint8_t rfBlockGnssBand; /**< GNSS band associated with the reported RF block */
+		uint8_t  reserved2[2];
 	};
 
-	ubx_payload_rx_mon_rf_block_t block[1]; ///< only read out the first block
+	ubx_payload_rx_mon_rf_block_t block[kMaxBlocks];
 } ubx_payload_rx_mon_rf_t;
 
 /* Rx SEC-SIG v2/v3 header (v1 jamFlags is at offset 4). Repeating
@@ -750,6 +758,26 @@ typedef struct {
 	uint8_t jamFlags;        /**< v1 only */
 } ubx_payload_rx_sec_sig_t;
 
+#if defined(CONFIG_GPS_UBX_SPAN)
+/* Rx MON-SPAN */
+typedef struct {
+	uint8_t version;
+	uint8_t numRfBlocks;         /**< Number of RF blocks included */
+	uint8_t reserved0[2];
+
+	struct ubx_payload_rx_mon_span_block_t {
+		uint8_t spectrum[256];  /**< dB Spectrum data (number of points = span/res) */
+		uint32_t span;          /**< Hz Spectrum span */
+		uint32_t res;           /**< Hz Resolution of the spectrum */
+		uint32_t center;        /**< Hz Center of spectrum span */
+		uint8_t pga;            /**< dB Programmable gain amplifier */
+		uint8_t reserved1[3];   /**< Reserved */
+	};
+
+	ubx_payload_rx_mon_span_block_t block[kMaxBlocks];
+} ubx_payload_rx_mon_span_t;
+#endif
+
 /* Rx MON-VER Part 1 */
 typedef struct {
 	uint8_t swVersion[30];
@@ -1004,6 +1032,9 @@ typedef union {
 	ubx_payload_rx_mon_hw_deprecated_t ubx_payload_rx_mon_hw_deprecated;
 	ubx_payload_rx_mon_rf_t           payload_rx_mon_rf;
 	ubx_payload_rx_sec_sig_t          payload_rx_sec_sig;
+#if defined(CONFIG_GPS_UBX_SPAN)
+	ubx_payload_rx_mon_span_t         payload_rx_mon_span;
+#endif
 	ubx_payload_rx_mon_ver_part1_t    payload_rx_mon_ver_part1;
 	ubx_payload_rx_mon_ver_part2_t    payload_rx_mon_ver_part2;
 	ubx_payload_rx_rxm_rtcm_t         payload_rx_rxm_rtcm;
@@ -1084,6 +1115,9 @@ class GPSDriverUBX : public GPSBaseStationSupport
 		int32_t uart1_baudrate;
 		int32_t uart2_baudrate;
 		bool ppk_output;
+#if defined(CONFIG_GPS_UBX_SPAN)
+		bool spectrum_analyzer;
+#endif
 		bool jam_det_sensitivity_hi;
 		UBXMode mode;
 	};
@@ -1384,6 +1418,8 @@ class GPSDriverUBX : public GPSBaseStationSupport
 	const int32_t _uart1_baudrate {};
 	const int32_t _uart2_baudrate {};
 	const bool _ppk_output {};
+#if defined(CONFIG_GPS_UBX_SPAN)
+	const bool _spectrum_analyzer {};
+#endif
 	const bool _jam_det_sensitivity_hi {};
 };
-