mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
feat(gps): add support for UBX_MSG_MON_SPAN (#28173)
* feat(gps): log GNSS info from UBX_MSG_MON_SPAN and UBX_MON_RF for improved GNSS debugging capabilities. Introduction of two new uorb messages to log the content of UBX_MSG_MON_SPAN and UBX_MON_RF on a per-band level per receiver. Gated behind Kconfig GPS_UBX_SPAN and new parameter GPS_UBX_SPECTRUM. * feat(gps): deactivate Kconfig option GPS_UBX_SPAN by default Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> * feat(gps): update submodule * chore(gps): remove unnecessary comment and whitespace * feat(boards/px4_fmu-v6c): activate CONFIG_GPS_UBX_SPAN --------- Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
This commit is contained in:
1 parent
7622c72eb8
commit
37e0cb3f6c
10 files changed
+217
-13
No files matched your search
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -45,6 +45,10 @@
|
||||
#include <uORB/topics/satellite_info.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/sensor_gnss_relative.h>
|
||||
#include <uORB/topics/sensor_gnss_rf.h>
|
||||
#if defined(CONFIG_GPS_UBX_SPAN)
|
||||
#include <uORB/topics/sensor_gnss_spectrum.h>
|
||||
#endif
|
||||
|
||||
#define GPS_INFO(...) PX4_INFO(__VA_ARGS__)
|
||||
#define GPS_WARN(...) PX4_WARN(__VA_ARGS__)
|
||||
|
||||
+1
-1
Submodule src/drivers/gps/devices updated: 1b27986519...928062a625.
+111
-9
@@ -68,7 +68,10 @@
|
||||
#include <uORB/topics/rtcm_data.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/sensor_gnss_relative.h>
|
||||
|
||||
#include <uORB/topics/sensor_gnss_rf.h>
|
||||
#if defined(CONFIG_GPS_UBX_SPAN)
|
||||
#include <uORB/topics/sensor_gnss_spectrum.h>
|
||||
#endif
|
||||
#include <lib/failure_injection/FailureInjection.hpp>
|
||||
#include <lib/gnss/correction_framer.h>
|
||||
#include <systemlib/system_time_source.h>
|
||||
@@ -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_s> _sensor_gps_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<satellite_info_s> _report_sat_info_pub{ORB_ID(satellite_info)}; ///< uORB pub for satellite info
|
||||
uORB::PublicationMulti<sensor_gnss_rf_s> _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_s> _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<satellite_info_s> _report_sat_info_pub {ORB_ID(satellite_info)}; ///< uORB pub for satellite info
|
||||
|
||||
failure_injection::Config _failure_config;
|
||||
failure_injection::Stuck<sensor_gps_s> _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<GPS *> _secondary_instance;
|
||||
|
||||
px4::atomic<int> _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 *> 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 *> 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<sensor_gnss_rf_s *>(data1));
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
#if defined(CONFIG_GPS_UBX_SPAN)
|
||||
|
||||
case GPSCallbackType::gotSpectrumMessage:
|
||||
if (data1 && data2 == sizeof(sensor_gnss_spectrum_s)) {
|
||||
gps->publishSpectrum(*static_cast<sensor_gnss_spectrum_s *>(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<int32_t>(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[])
|
||||
{
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in new issue
Block a user