From e529a5d8f2da96c9e3457bc1d6625d17aba5724c Mon Sep 17 00:00:00 2001 From: thomas Date: Thu, 13 Aug 2026 20:10:45 +0200 Subject: [PATCH] support logging three rf blocks for X20 compatibility --- msg/SensorGnssRf.msg | 2 +- msg/SensorGnssSpectrum.msg | 2 +- src/drivers/gps/devices | 2 +- src/drivers/gps/gps.cpp | 44 +++++++++++++++++++++++++++++++++++++- 4 files changed, 46 insertions(+), 4 deletions(-) diff --git a/msg/SensorGnssRf.msg b/msg/SensorGnssRf.msg index 55b1cd284c8..23179f56537 100644 --- a/msg/SensorGnssRf.msg +++ b/msg/SensorGnssRf.msg @@ -49,4 +49,4 @@ uint8 RF_BAND_L3 = 3 # L3 band uint8 RF_BAND_L5 = 4 # L5 band # One topic per RF block ID -# TOPICS sensor_gnss_rf_block0 sensor_gnss_rf_block1 +# TOPICS sensor_gnss_rf_block0 sensor_gnss_rf_block1 sensor_gnss_rf_block2 diff --git a/msg/SensorGnssSpectrum.msg b/msg/SensorGnssSpectrum.msg index 36fb7f779f4..95a96442ed6 100644 --- a/msg/SensorGnssSpectrum.msg +++ b/msg/SensorGnssSpectrum.msg @@ -19,4 +19,4 @@ 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 +# TOPICS sensor_gnss_spectrum_block0 sensor_gnss_spectrum_block1 sensor_gnss_spectrum_block2 diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index dcd7540e145..2922bc94a08 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit dcd7540e145b5202c27e16ffe9f2144b3482dcfc +Subproject commit 2922bc94a0844a9488b2e153278c7d3c4ff16bb4 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 43ed3423b18..d1a17471f1d 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -150,7 +150,7 @@ struct GPS_Sat_Info { satellite_info_s _data; }; -static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(2300); +static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(2750); class GPS : public ModuleBase, public device::Device @@ -245,8 +245,10 @@ private: uORB::PublicationMulti _sensor_gnss_relative_pub{ORB_ID(sensor_gnss_relative)}; uORB::PublicationMulti _sensor_gnss_rf_block0_pub{ORB_ID(sensor_gnss_rf_block0)}; uORB::PublicationMulti _sensor_gnss_rf_block1_pub{ORB_ID(sensor_gnss_rf_block1)}; + uORB::PublicationMulti _sensor_gnss_rf_block2_pub{ORB_ID(sensor_gnss_rf_block2)}; uORB::PublicationMulti _sensor_gnss_spectrum_block0_pub{ORB_ID(sensor_gnss_spectrum_block0)}; uORB::PublicationMulti _sensor_gnss_spectrum_block1_pub{ORB_ID(sensor_gnss_spectrum_block1)}; + uORB::PublicationMulti _sensor_gnss_spectrum_block2_pub{ORB_ID(sensor_gnss_spectrum_block2)}; uORB::PublicationMulti _report_sat_info_pub{ORB_ID(satellite_info)}; ///< uORB pub for satellite info @@ -294,8 +296,10 @@ private: static px4::atomic_bool _is_sat_info_main_advertised; static px4::atomic_bool _is_rf_block0_main_advertised; static px4::atomic_bool _is_rf_block1_main_advertised; + static px4::atomic_bool _is_rf_block2_main_advertised; static px4::atomic_bool _is_spectrum_block0_main_advertised; static px4::atomic_bool _is_spectrum_block1_main_advertised; + static px4::atomic_bool _is_spectrum_block2_main_advertised; static px4::atomic _secondary_instance; @@ -327,6 +331,7 @@ private: void publishRF(sensor_gnss_rf_s &gnss_rf); void publishRFBlock0(sensor_gnss_rf_s &gnss_rf); void publishRFBlock1(sensor_gnss_rf_s &gnss_rf); + void publishRFBlock2(sensor_gnss_rf_s &gnss_rf); /** * Publish spectrum @@ -334,6 +339,7 @@ private: void publishSpectrum(sensor_gnss_spectrum_s &gnss_spectrum); void publishSpectrumBlock0(sensor_gnss_spectrum_s &gnss_spectrum); void publishSpectrumBlock1(sensor_gnss_spectrum_s &gnss_spectrum); + void publishSpectrumBlock2(sensor_gnss_spectrum_s &gnss_spectrum); /** * This is an abstraction for the poll on serial used. @@ -408,8 +414,10 @@ px4::atomic_bool GPS::_is_gps_main_advertised{false}; px4::atomic_bool GPS::_is_sat_info_main_advertised{false}; px4::atomic_bool GPS::_is_rf_block0_main_advertised{false}; px4::atomic_bool GPS::_is_rf_block1_main_advertised{false}; +px4::atomic_bool GPS::_is_rf_block2_main_advertised{false}; px4::atomic_bool GPS::_is_spectrum_block0_main_advertised{false}; px4::atomic_bool GPS::_is_spectrum_block1_main_advertised{false}; +px4::atomic_bool GPS::_is_spectrum_block2_main_advertised{false}; px4::atomic GPS::_secondary_instance{nullptr}; ModuleBase::Descriptor GPS::desc{task_spawn, custom_command, print_usage}; @@ -1696,6 +1704,10 @@ GPS::publishRF(sensor_gnss_rf_s &gnss_rf) else if (gnss_rf.block_id == 1) { publishRFBlock1(gnss_rf); } + + else if (gnss_rf.block_id == 2) { + publishRFBlock2(gnss_rf); + } } void @@ -1724,6 +1736,19 @@ GPS::publishRFBlock1(sensor_gnss_rf_s &gnss_rf) } } +void +GPS::publishRFBlock2(sensor_gnss_rf_s &gnss_rf) +{ + if (_instance == Instance::Main || _is_rf_block2_main_advertised.load()) { + gnss_rf.device_id = get_device_id(); + gnss_rf.timestamp = hrt_absolute_time(); + + _sensor_gnss_rf_block2_pub.publish(gnss_rf); + // impose Main instance to publish sat_info first to assign first index + _is_rf_block2_main_advertised.store(true); + } +} + void GPS::publishSpectrum(sensor_gnss_spectrum_s &gnss_spectrum) { @@ -1734,6 +1759,10 @@ GPS::publishSpectrum(sensor_gnss_spectrum_s &gnss_spectrum) else if (gnss_spectrum.block_id == 1) { publishSpectrumBlock1(gnss_spectrum); } + + else if (gnss_spectrum.block_id == 2) { + publishSpectrumBlock2(gnss_spectrum); + } } void @@ -1762,6 +1791,19 @@ GPS::publishSpectrumBlock1(sensor_gnss_spectrum_s &gnss_spectrum) } } +void +GPS::publishSpectrumBlock2(sensor_gnss_spectrum_s &gnss_spectrum) +{ + if (_instance == Instance::Main || _is_spectrum_block2_main_advertised.load()) { + gnss_spectrum.device_id = get_device_id(); + gnss_spectrum.timestamp = hrt_absolute_time(); + _sensor_gnss_spectrum_block2_pub.publish(gnss_spectrum); + + // impose Main instance to publish sat_info first to assign first index + _is_spectrum_block2_main_advertised.store(true); + } +} + int GPS::custom_command(int argc, char *argv[]) {