diff --git a/boards/px4/fmu-v6c/visionTargetEstStatic.px4board b/boards/px4/fmu-v6c/visionTargetEstStatic.px4board index b6c34fe6c91..b1b3f45cd9e 100644 --- a/boards/px4/fmu-v6c/visionTargetEstStatic.px4board +++ b/boards/px4/fmu-v6c/visionTargetEstStatic.px4board @@ -19,6 +19,7 @@ CONFIG_DRIVERS_GPS=y # CONFIG_GPS_EMLIDREACH is not set # CONFIG_GPS_FEMTOMES is not set # CONFIG_GPS_NMEA is not set +CONFIG_GPS_UBX_SPAN=y CONFIG_DRIVERS_HEATER=y CONFIG_COMMON_INS=n CONFIG_COMMON_LIGHT=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index fcb60c52ad4..c3f71057073 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -205,6 +205,7 @@ set(msg_files SensorCombined.msg SensorCorrection.msg SensorGnssRelative.msg + SensorGnssRf.msg SensorGnssStatus.msg SensorGps.msg SensorGyro.msg @@ -290,6 +291,9 @@ set(msg_files versioned/VtolVehicleStatus.msg versioned/Wind.msg ) +if(CONFIG_GPS_UBX_SPAN) + list(APPEND msg_files SensorGnssSpectrum.msg) +endif() list(SORT msg_files) if(CONFIG_NAVIGATOR_ADSB) diff --git a/msg/SensorGnssRf.msg b/msg/SensorGnssRf.msg new file mode 100644 index 00000000000..47291a5bafd --- /dev/null +++ b/msg/SensorGnssRf.msg @@ -0,0 +1,47 @@ +# GNSS RF status +# +# Reports RF block status for a GNSS receiver, decoded from the u-blox UBX-MON-RF message. +# Includes antenna status/power, jamming/interference indicators, automatic gain control (AGC), +# noise level, and I/Q demodulation metrics. Published by the `gps` driver (UBX protocol only), once +# per RF block, via the block-specific topics below. + +uint64 timestamp # [us] Time since system start +uint64 timestamp_sample # [us] Timestamp of the raw data + +uint32 device_id # Unique device ID for the sensor that does not change between power cycles + +uint8 block_id # [-] Only used for multi-topic publishing + +uint8 antenna_status # [@enum ANTENNA_STATUS] Status of the antenna supervisor state machine +uint8 ANTENNA_STATUS_INIT = 0 # Initializing +uint8 ANTENNA_STATUS_UNKNOWN = 1 # Status unknown +uint8 ANTENNA_STATUS_OK = 2 # Antenna OK +uint8 ANTENNA_STATUS_SHORT = 3 # Short circuit detected +uint8 ANTENNA_STATUS_OPEN = 4 # Open circuit detected + +uint8 antenna_power # [@enum ANTENNA_POWER] Antenna power state +uint8 ANTENNA_POWER_OFF = 0 # Power off +uint8 ANTENNA_POWER_ON = 1 # Power on +uint8 ANTENNA_POWER_UNKNOWN = 2 # Power state unknown + +uint32 post_status # [-] Power-on self-test (POST) status word (vendor-specific bitmask) + +uint16 noise_per_ms # [-] Noise level as measured by the GNSS receiver core +uint16 automatic_gain_control # [-] [@range 0, 8191] Automatic Gain Control (AGC) monitor value (8191 = 100% of max gain) +uint8 jamming_indicator # [-] [@range 0, 255] Continuous wave (CW) jamming indicator (0 = no CW jamming, 255 = strong CW jamming) + +uint8 jamming_state # [@enum JAMMING_STATE] Jamming state +uint8 JAMMING_STATE_UNKNOWN = 0 # Unknown +uint8 JAMMING_STATE_OK = 1 # No jamming detected +uint8 JAMMING_STATE_WARNING = 2 # Interference visible but fix OK +uint8 JAMMING_STATE_CRITICAL = 3 # Interference visible and no fix + +int8 i_offset # [-] [@range -128, 127] Imbalance of the I-part of the complex signal (0 = no imbalance) +uint8 i_magnitude # [-] [@range 0, 255] Magnitude of the I-part of the complex signal (0 = no signal, 255 = max magnitude) +int8 q_offset # [-] [@range -128, 127] Imbalance of the Q-part of the complex signal (0 = no imbalance) +uint8 q_magnitude # [-] [@range 0, 255] Magnitude of the Q-part of the complex signal (0 = no signal, 255 = max magnitude) + +uint32 center_frequency # [Hz] Nominal center frequency of the RF block. 0 = unknown. + +# One topic per RF block ID +# TOPICS sensor_gnss_rf_block0 sensor_gnss_rf_block1 sensor_gnss_rf_block2 diff --git a/msg/SensorGnssSpectrum.msg b/msg/SensorGnssSpectrum.msg new file mode 100644 index 00000000000..95a96442ed6 --- /dev/null +++ b/msg/SensorGnssSpectrum.msg @@ -0,0 +1,22 @@ +# GNSS spectrum analysis +# +# Reports a basic RF spectrum analysis for a GNSS receiver, decoded from the u-blox UBX-MON-SPAN +# message (enabled via the GPS_UBX_SPECTRUM parameter). +# Published by the `gps` driver (UBX protocol only), once per RF block, via the block-specific topics below. +# The center frequency of bin i is: f(i) = center_frequency + spectrum_span * (i - 127) / 256 + +uint64 timestamp # [us] Time since system start +uint64 timestamp_sample # [us] Timestamp of the raw data + +uint32 device_id # Unique device ID for the sensor that does not change between power cycles + +uint8 block_id # [-] Only used for multi-topic publishing. Use center_frequency to determine which RF band it is. + +uint8[256] spectrum # [(2^-2)dB] Spectrum magnitude per bin (number of points = spectrum_span / resolution) +uint32 spectrum_span # [Hz] Frequency span covered by the spectrum +uint32 resolution # [Hz] Frequency resolution of the spectrum (bin width) +uint32 center_frequency # [Hz] Center frequency of the spectrum span +uint8 programmable_gain_amplifier # [dB] Programmable gain amplifier setting + +# One topic per RF block ID +# TOPICS sensor_gnss_spectrum_block0 sensor_gnss_spectrum_block1 sensor_gnss_spectrum_block2 diff --git a/src/drivers/gps/Kconfig b/src/drivers/gps/Kconfig index ddb602c8f38..9be443353af 100644 --- a/src/drivers/gps/Kconfig +++ b/src/drivers/gps/Kconfig @@ -52,6 +52,12 @@ if DRIVERS_GPS Enable on targets that receive a SPARTN correction service. Left off, the SPARTN framer is not built and the receiver only ever sees RTCM3, which is the common RTK setup. + config GPS_UBX_SPAN + bool "Support for UBX_MON_SPAN message handling" + default n + depends on GPS_UBX + ---help--- + Enables reading UBX_MON_SPAN messages from u-blox receivers. endif # DRIVERS_GPS menuconfig USER_GPS diff --git a/src/drivers/gps/definitions.h b/src/drivers/gps/definitions.h index a1f5ac6a175..015cc580c41 100644 --- a/src/drivers/gps/definitions.h +++ b/src/drivers/gps/definitions.h @@ -45,6 +45,10 @@ #include #include #include +#include +#if defined(CONFIG_GPS_UBX_SPAN) +#include +#endif #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 1b279865192..928062a6258 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 1b279865192dd61af62095744366466a0d39ffce +Subproject commit 928062a62583156f10b7416ea10a12d2733ef632 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index d388c2e3f43..4df6312309b 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -68,7 +68,10 @@ #include #include #include - +#include +#if defined(CONFIG_GPS_UBX_SPAN) +#include +#endif #include #include #include @@ -149,7 +152,11 @@ struct GPS_Sat_Info { satellite_info_s _data; }; +#if defined(CONFIG_GPS_UBX_SPAN) +static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(2750); +#else static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(2040); +#endif class GPS : public ModuleBase, public device::Device @@ -242,8 +249,19 @@ private: uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position uORB::PublicationMulti _sensor_gnss_relative_pub{ORB_ID(sensor_gnss_relative)}; - - uORB::PublicationMulti _report_sat_info_pub{ORB_ID(satellite_info)}; ///< uORB pub for satellite info + uORB::PublicationMulti _sensor_gnss_rf_block_pub[kMaxBlocks] { + {ORB_ID(sensor_gnss_rf_block0)}, + {ORB_ID(sensor_gnss_rf_block1)}, + {ORB_ID(sensor_gnss_rf_block2)}, + }; +#if defined(CONFIG_GPS_UBX_SPAN) + uORB::PublicationMulti _sensor_gnss_spectrum_block_pub[kMaxBlocks] { + {ORB_ID(sensor_gnss_spectrum_block0)}, + {ORB_ID(sensor_gnss_spectrum_block1)}, + {ORB_ID(sensor_gnss_spectrum_block2)}, + }; +#endif + uORB::PublicationMulti _report_sat_info_pub {ORB_ID(satellite_info)}; ///< uORB pub for satellite info failure_injection::Config _failure_config; failure_injection::Stuck _stuck; @@ -284,10 +302,13 @@ private: perf_counter_t _rtcm_corrections_injection_perf{perf_alloc(PC_COUNT, MODULE_NAME": rtcm corrections injected")}; perf_counter_t _rtcm_moving_baseline_injection_perf{perf_alloc(PC_COUNT, MODULE_NAME": rtcm moving baseline injected")}; - static px4::atomic_bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1 - static px4::atomic_bool _is_sat_info_main_advertised; ///< for the second gps we want to make sure that it gets instance 1 - /// and thus we wait until the first one publishes at least one message. - + // For the second gps we want to make sure that it gets instance 1 and thus we wait until the first one publishes at least one message. + static px4::atomic_bool _is_gps_main_advertised; + static px4::atomic_bool _is_sat_info_main_advertised; + static px4::atomic_bool _is_rf_block_main_advertised[kMaxBlocks]; +#if defined(CONFIG_GPS_UBX_SPAN) + static px4::atomic_bool _is_spectrum_block_main_advertised[kMaxBlocks]; +#endif static px4::atomic _secondary_instance; px4::atomic _scheduled_reset{(int)GPSRestartType::None}; @@ -309,10 +330,22 @@ private: void publishRTCMCorrections(uint8_t *data, size_t len); /** - * Publish RTCM corrections + * Publish relative position */ void publishRelativePosition(sensor_gnss_relative_s &gnss_relative); + /** + * Publish RF data + */ + void publishRF(sensor_gnss_rf_s &gnss_rf); + +#if defined(CONFIG_GPS_UBX_SPAN) + /** + * Publish spectrum + */ + void publishSpectrum(sensor_gnss_spectrum_s &gnss_spectrum); +#endif + /** * This is an abstraction for the poll on serial used. * @@ -386,7 +419,11 @@ private: px4::atomic_bool GPS::_is_gps_main_advertised{false}; px4::atomic_bool GPS::_is_sat_info_main_advertised{false}; -px4::atomic GPS::_secondary_instance{nullptr}; +px4::atomic_bool GPS::_is_rf_block_main_advertised[kMaxBlocks] {}; +#if defined(CONFIG_GPS_UBX_SPAN) +px4::atomic_bool GPS::_is_spectrum_block_main_advertised[kMaxBlocks] {}; +#endif +px4::atomic GPS::_secondary_instance {nullptr}; ModuleBase::Descriptor GPS::desc{task_spawn, custom_command, print_usage}; /* @@ -560,6 +597,23 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) break; + case GPSCallbackType::gotRFMessage: + if (data1 && data2 == sizeof(sensor_gnss_rf_s)) { + gps->publishRF(*static_cast(data1)); + } + + break; + +#if defined(CONFIG_GPS_UBX_SPAN) + + case GPSCallbackType::gotSpectrumMessage: + if (data1 && data2 == sizeof(sensor_gnss_spectrum_s)) { + gps->publishSpectrum(*static_cast(data1)); + } + + break; +#endif + case GPSCallbackType::surveyInStatus: /* not used */ break; @@ -1054,6 +1108,16 @@ GPS::run() param_get(handle, &jam_det_sensitivity_hi); } +#if defined(CONFIG_GPS_UBX_SPAN) + handle = param_find("GPS_UBX_SPECTRUM"); + int32_t gps_ubx_spectrum = 0; + + if (handle != PARAM_INVALID) { + param_get(handle, &gps_ubx_spectrum); + } + +#endif // CONFIG_GPS_UBX_SPAN + #endif // CONFIG_GPS_UBX int32_t gnssSystemsParam = static_cast(GPSHelper::GNSSSystemsMask::RECEIVER_DEFAULTS); @@ -1159,6 +1223,9 @@ GPS::run() .uart1_baudrate = ubx_uart1_baudrate, .uart2_baudrate = f9p_uart2_baudrate, .ppk_output = ppk_output > 0, +#if defined(CONFIG_GPS_UBX_SPAN) + .spectrum_analyzer = gps_ubx_spectrum > 0, +#endif .jam_det_sensitivity_hi = jam_det_sensitivity_hi > 0, .mode = ubx_mode, }; @@ -1698,6 +1765,41 @@ GPS::publishRelativePosition(sensor_gnss_relative_s &gnss_relative) _sensor_gnss_relative_pub.publish(gnss_relative); } +void +GPS::publishRF(sensor_gnss_rf_s &gnss_rf) +{ + if (gnss_rf.block_id >= kMaxBlocks) { + return; + } + + // impose Main instance to publish first to assign first index + if (_instance == Instance::Main || _is_rf_block_main_advertised[gnss_rf.block_id].load()) { + gnss_rf.device_id = get_device_id(); + gnss_rf.timestamp = hrt_absolute_time(); + + _sensor_gnss_rf_block_pub[gnss_rf.block_id].publish(gnss_rf); + _is_rf_block_main_advertised[gnss_rf.block_id].store(true); + } +} + +#if defined(CONFIG_GPS_UBX_SPAN) +void +GPS::publishSpectrum(sensor_gnss_spectrum_s &gnss_spectrum) +{ + if (gnss_spectrum.block_id >= kMaxBlocks) { + return; + } + + // impose Main instance to publish first to assign first index + if (_instance == Instance::Main || _is_spectrum_block_main_advertised[gnss_spectrum.block_id].load()) { + gnss_spectrum.device_id = get_device_id(); + gnss_spectrum.timestamp = hrt_absolute_time(); + _sensor_gnss_spectrum_block_pub[gnss_spectrum.block_id].publish(gnss_spectrum); + _is_spectrum_block_main_advertised[gnss_spectrum.block_id].store(true); + } +} +#endif + int GPS::custom_command(int argc, char *argv[]) { diff --git a/src/drivers/gps/params.yaml b/src/drivers/gps/params.yaml index c80f5545f54..5558f4c0271 100644 --- a/src/drivers/gps/params.yaml +++ b/src/drivers/gps/params.yaml @@ -94,9 +94,12 @@ parameters: GPS_SAT_INFO: description: short: Enable sat info (if available) - long: |- - Enable publication of satellite info (ORB_ID(satellite_info)) if possible. - Not available on MTK. + type: boolean + default: 0 + reboot_required: true + GPS_UBX_SPECTRUM: + description: + short: Enable spectrum analyzer (if available) type: boolean default: 0 reboot_required: true diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 1b312304c98..7efc75609bb 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -175,9 +175,24 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("sensor_hygrometer", 500, 4); add_optional_topic_multi("sensor_temp", 100, 4); add_optional_topic_multi("rpm", 200); + add_topic_multi("sensor_gnss_rf_block0", 5000, 2); + add_topic_multi("sensor_gnss_rf_block1", 5000, 2); + add_topic_multi("sensor_gnss_rf_block2", 5000, 2); add_topic_multi("timesync_status", 1000, 3); add_topic_multi("telemetry_status", 1000, 4); +#if defined(CONFIG_GPS_UBX_SPAN) + int32_t gps_ubx_spectrum = 0; + param_get(param_find("GPS_UBX_SPECTRUM"), &gps_ubx_spectrum); + + if (gps_ubx_spectrum > 0) { + add_topic_multi("sensor_gnss_spectrum_block0", 5000, 2); + add_topic_multi("sensor_gnss_spectrum_block1", 5000, 2); + add_topic_multi("sensor_gnss_spectrum_block2", 5000, 2); + } + +#endif + // EKF multi topics { // optionally log all estimator* topics at minimal rate