From 3772fff721ced6f522dac1211cb327b05adc9a25 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 24 Oct 2022 21:00:35 -0400 Subject: [PATCH] drivers/gps support ublox basic spectrum analyzer (UBX-MON-SPAN) --- msg/CMakeLists.txt | 1 + msg/SensorGnssSpectrum.msg | 16 ++++++++++++++ src/drivers/gps/definitions.h | 1 + src/drivers/gps/devices | 2 +- src/drivers/gps/gps.cpp | 33 ++++++++++++++++++++++++++-- src/drivers/gps/params.c | 12 ++++++++++ src/modules/logger/logged_topics.cpp | 1 + 7 files changed, 63 insertions(+), 3 deletions(-) create mode 100644 msg/SensorGnssSpectrum.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 756f2f454c..8c7a73be58 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -156,6 +156,7 @@ set(msg_files SensorCombined.msg SensorCorrection.msg SensorGnssRelative.msg + SensorGnssSpectrum.msg SensorGps.msg SensorGyro.msg SensorGyroFft.msg diff --git a/msg/SensorGnssSpectrum.msg b/msg/SensorGnssSpectrum.msg new file mode 100644 index 0000000000..1f758da73c --- /dev/null +++ b/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 diff --git a/src/drivers/gps/definitions.h b/src/drivers/gps/definitions.h index a1f5ac6a17..6be3f04635 100644 --- a/src/drivers/gps/definitions.h +++ b/src/drivers/gps/definitions.h @@ -45,6 +45,7 @@ #include #include #include +#include #define GPS_INFO(...) PX4_INFO(__VA_ARGS__) #define GPS_WARN(...) PX4_WARN(__VA_ARGS__) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 1ff87868f6..e73cb45371 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 1ff87868f6008f06e2033ee05dd904ec54109e52 +Subproject commit e73cb4537174693a86f8e0961e9c0f6bbea72e0c diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index d7cdb8f650..55e7f295e5 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -64,6 +64,7 @@ #include #include #include +#include #ifndef CONSTRAINED_FLASH # include "devices/src/ashtech.h" @@ -185,6 +186,7 @@ private: uORB::PublicationMulti _report_gps_pos_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position uORB::PublicationMulti _sensor_gnss_relative_pub{ORB_ID(sensor_gnss_relative)}; + uORB::PublicationMulti _sensor_gnss_spectrum_pub{ORB_ID(sensor_gnss_spectrum)}; uORB::PublicationMulti _report_sat_info_pub{ORB_ID(satellite_info)}; ///< uORB pub for satellite info @@ -228,10 +230,15 @@ private: 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(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[]) { diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c index 256099c485..1933238102 100644 --- a/src/drivers/gps/params.c +++ b/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 * diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 8878673c5a..d06505d810 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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);