feat(pps_capture): allow selecting GPS receiver by device ID (#26719)

* feat(pps_capture): allow selecting GPS receiver by device ID

Add PPS_CAP_GPS_ID parameter to select which GPS receiver's data
is used for PPS timestamp correlation. Matches by device ID rather
than uORB instance index, which avoids dependence on instance ordering.

When set to 0 (default), uses the first available instance for
backward compatibility.

* docs(pps_capture): document PPS_CAP_GPS_ID for multi-GPS setups

* fix(pps_capture): use GPS_MAX_RECEIVERS constant and mark PPS_CAP_GPS_ID reboot-required
This commit is contained in:
Jacob Dahl 2026-03-19 11:25:04 -08:00 committed by GitHub
parent 75a51c19c7
commit 375d540cf8
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
4 changed files with 56 additions and 12 deletions

View File

@ -57,6 +57,17 @@ param set PWM_MAIN_FUNC10 2064
param set PPS_CAP_ENABLE 1
```
#### Multi-GPS Setups
If you have multiple GPS receivers, set `PPS_CAP_GPS_ID` to the device ID of the GPS receiver that emits the PPS signal.
When set to `0` (default), the driver uses the first available GPS instance.
You can find the device ID by running:
```sh
listener sensor_gps
```
### Wiring
The wiring configuration depends on your specific flight controller.
@ -129,5 +140,5 @@ See also:
The PPS signal provides much higher temporal precision than the transmitted time data, which has latency and jitter from serial communication.
::: warning
If the PPS driver does not sending any data for 5 seconds (despite having `PPS_CAP_ENABLE` set to 1), the `SENS_GPS0_DELAY` will be used instead for estimating the latency.
If the PPS driver does not send any data for 5 seconds (despite having `PPS_CAP_ENABLE` set to 1), the corresponding `SENS_GPS*_DELAY` parameter will be used instead for estimating the latency.
:::

View File

@ -48,6 +48,7 @@
ModuleBase::Descriptor PPSCapture::desc{task_spawn, custom_command, print_usage};
PPSCapture::PPSCapture() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
_pps_capture_pub.advertise();
@ -117,9 +118,16 @@ void PPSCapture::Run()
sensor_gps_s sensor_gps;
if (_sensor_gps_sub.update(&sensor_gps)) {
_last_gps_utc_timestamp = sensor_gps.time_utc_usec;
_last_gps_timestamp = sensor_gps.timestamp;
const uint32_t gps_device_id = static_cast<uint32_t>(_param_pps_cap_gps_id.get());
for (auto &sub : _sensor_gps_subs) {
if (sub.update(&sensor_gps)) {
if (gps_device_id == 0 || sensor_gps.device_id == gps_device_id) {
_last_gps_utc_timestamp = sensor_gps.time_utc_usec;
_last_gps_timestamp = sensor_gps.timestamp;
break;
}
}
}
pps_capture_s pps_capture;

View File

@ -36,15 +36,17 @@
#include <drivers/drv_hrt.h>
#include <px4_arch/micro_hal.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/pps_capture.h>
#include <uORB/topics/sensor_gps.h>
using namespace time_literals;
class PPSCapture : public ModuleBase, public px4::ScheduledWorkItem
class PPSCapture : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
{
public:
static Descriptor desc;
@ -71,16 +73,23 @@ public:
private:
void Run() override;
static constexpr int GPS_MAX_RECEIVERS = 2;
int _channel{-1};
uint32_t _pps_capture_gpio{0};
uORB::Publication<pps_capture_s> _pps_capture_pub{ORB_ID(pps_capture)};
uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)};
orb_advert_t _mavlink_log_pub{nullptr};
hrt_abstime _hrt_timestamp{0};
uORB::Publication<pps_capture_s> _pps_capture_pub{ORB_ID(pps_capture)};
uORB::SubscriptionMultiArray<sensor_gps_s, GPS_MAX_RECEIVERS> _sensor_gps_subs{ORB_ID::sensor_gps};
orb_advert_t _mavlink_log_pub{nullptr};
hrt_abstime _hrt_timestamp{0};
hrt_abstime _last_gps_timestamp{0};
uint64_t _last_gps_utc_timestamp{0};
uint8_t _pps_rate_exceeded_counter{0};
px4::atomic<bool> _pps_rate_failure{false};
uint64_t _last_gps_utc_timestamp{0};
uint8_t _pps_rate_exceeded_counter{0};
px4::atomic<bool> _pps_rate_failure{false};
DEFINE_PARAMETERS(
(ParamInt<px4::params::PPS_CAP_GPS_ID>) _param_pps_cap_gps_id
)
};

View File

@ -41,3 +41,19 @@
* @reboot_required true
*/
PARAM_DEFINE_INT32(PPS_CAP_ENABLE, 0);
/**
* PPS capture GPS receiver device ID
*
* Device ID of the GPS receiver that emits the PPS signal captured on the
* configured PWM input pin. When set to 0 (default), the first available
* GPS instance is used.
*
* The device ID can be obtained from the sensor_gps publication
* (e.g. via listener sensor_gps).
*
* @group GPS
* @min 0
* @reboot_required true
*/
PARAM_DEFINE_INT32(PPS_CAP_GPS_ID, 0);