drivers/gps support ublox basic spectrum analyzer (UBX-MON-SPAN)

msg/CMakeLists.txt

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

msg/SensorGnssSpectrum.msg

@@ -0,0 +1,16 @@
+# GNSS spectrum
+
+uint64 timestamp                  # time since system start (microseconds)
+uint64 timestamp_sample           # time since system start (microseconds)
+
+uint32 device_id                  # unique device ID for the sensor that does not change between power cycles
+
+# The center frequency at each bin: f(i) = center + span * (i - 127) / 256
+uint8[256] spectrum # dB Spectrum data (number of points = span/res)
+uint32 spectrum_span_hz    # Hz Spectrum span
+uint32 resolution_hz       # Hz Resolution of the spectrum
+uint32 center_frequency_hz # Hz Center of spectrum span
+
+uint8 programmable_gain_amplifier_db # dB Programmable gain amplifier
+
+uint8 ORB_QUEUE_LENGTH = 2

src/drivers/gps/definitions.h

@@ -45,6 +45,7 @@
 #include <uORB/topics/satellite_info.h>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/sensor_gnss_relative.h>
+#include <uORB/topics/sensor_gnss_spectrum.h>
 
 #define GPS_INFO(...) PX4_INFO(__VA_ARGS__)
 #define GPS_WARN(...) PX4_WARN(__VA_ARGS__)

src/drivers/gps/gps.cpp

@@ -64,6 +64,7 @@
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/sensor_gnss_relative.h>
+#include <uORB/topics/sensor_gnss_spectrum.h>
 
 #ifndef CONSTRAINED_FLASH
 # include "devices/src/ashtech.h"
@@ -185,6 +186,7 @@ class GPS : public ModuleBase<GPS>, public device::Device
 
 	uORB::PublicationMulti<sensor_gps_s>	_report_gps_pos_pub{ORB_ID(sensor_gps)};	///< uORB pub for gps position
 	uORB::PublicationMulti<sensor_gnss_relative_s> _sensor_gnss_relative_pub{ORB_ID(sensor_gnss_relative)};
+	uORB::PublicationMulti<sensor_gnss_spectrum_s> _sensor_gnss_spectrum_pub{ORB_ID(sensor_gnss_spectrum)};
 
 	uORB::PublicationMulti<satellite_info_s>	_report_sat_info_pub{ORB_ID(satellite_info)};		///< uORB pub for satellite info
 
@@ -228,10 +230,15 @@ class GPS : public ModuleBase<GPS>, public device::Device
 	void 				publishRTCMCorrections(uint8_t *data, size_t len);
 
 	/**
-	 * Publish RTCM corrections
+	 * Publish relative position
 	 */
 	void 				publishRelativePosition(sensor_gnss_relative_s &gnss_relative);
 
+	/**
+	 * Publish spectrum
+	 */
+	void 				publishSpectrum(sensor_gnss_spectrum_s &gnss_spectrum);
+
 	/**
 	 * This is an abstraction for the poll on serial used.
 	 *
@@ -415,6 +422,13 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 
 		break;
 
+	case GPSCallbackType::gotSpectrumMessage:
+		if (data1 && data2 == sizeof(sensor_gnss_spectrum_s)) {
+			gps->publishSpectrum(*static_cast<sensor_gnss_spectrum_s *>(data1));
+		}
+
+		break;
+
 	case GPSCallbackType::surveyInStatus:
 		/* not used */
 		break;
@@ -754,6 +768,13 @@ GPS::run()
 		param_get(handle, &gps_ubx_dynmodel);
 	}
 
+	int32_t gps_ubx_spectrum = 0;
+	handle = param_find("GPS_UBX_SPECTRUM");
+
+	if (handle != PARAM_INVALID) {
+		param_get(handle, &gps_ubx_spectrum);
+	}
+
 	handle = param_find("GPS_UBX_MODE");
 
 	GPSDriverUBX::UBXMode ubx_mode{GPSDriverUBX::UBXMode::Normal};
@@ -857,7 +878,7 @@ GPS::run()
 		/* FALLTHROUGH */
 		case gps_driver_mode_t::UBX:
 			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
-						   gps_ubx_dynmodel, heading_offset, f9p_uart2_baudrate, ubx_mode);
+						   gps_ubx_dynmodel, gps_ubx_spectrum, heading_offset, f9p_uart2_baudrate, ubx_mode);
 			set_device_type(DRV_GPS_DEVTYPE_UBX);
 			break;
 #ifndef CONSTRAINED_FLASH
@@ -1243,6 +1264,14 @@ GPS::publishRelativePosition(sensor_gnss_relative_s &gnss_relative)
 	_sensor_gnss_relative_pub.publish(gnss_relative);
 }
 
+void
+GPS::publishSpectrum(sensor_gnss_spectrum_s &gnss_spectrum)
+{
+	gnss_spectrum.device_id = get_device_id();
+	gnss_spectrum.timestamp = hrt_absolute_time();
+	_sensor_gnss_spectrum_pub.publish(gnss_spectrum);
+}
+
 int
 GPS::custom_command(int argc, char *argv[])
 {

src/drivers/gps/params.c

@@ -81,6 +81,18 @@ PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7);
  */
 PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
 
+/**
+ * Enable spectrum analyzer (if available)
+ *
+ * Enable publication of basic spectrum analyzer, (ORB_ID(sensor_gnss_spectrum)) if possible.
+ * Only available on newer u-blox modules.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_UBX_SPECTRUM, 0);
+
 /**
  * u-blox GPS Mode
  *

src/modules/logger/logged_topics.cpp

@@ -202,6 +202,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("sensor_baro", 1000, 4);
 	add_topic_multi("sensor_gps", 1000, 2);
 	add_topic_multi("sensor_gnss_relative", 1000, 1);
+	add_optional_topic("sensor_gnss_spectrum", 1000);
 	add_optional_topic_multi("sensor_gyro", 1000, 4);
 	add_topic_multi("sensor_mag", 1000, 4);
 	add_topic_multi("sensor_optical_flow", 1000, 2);