diff --git a/docs/en/advanced/pps_time_sync.md b/docs/en/advanced/pps_time_sync.md index b48ee5713c..9fe17a991c 100644 --- a/docs/en/advanced/pps_time_sync.md +++ b/docs/en/advanced/pps_time_sync.md @@ -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. ::: diff --git a/src/drivers/pps_capture/PPSCapture.cpp b/src/drivers/pps_capture/PPSCapture.cpp index 49923baa33..9460fe263e 100644 --- a/src/drivers/pps_capture/PPSCapture.cpp +++ b/src/drivers/pps_capture/PPSCapture.cpp @@ -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(_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; diff --git a/src/drivers/pps_capture/PPSCapture.hpp b/src/drivers/pps_capture/PPSCapture.hpp index 075e198b7e..d598efb0a9 100644 --- a/src/drivers/pps_capture/PPSCapture.hpp +++ b/src/drivers/pps_capture/PPSCapture.hpp @@ -36,15 +36,17 @@ #include #include #include +#include #include #include #include +#include #include #include 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_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_pub{ORB_ID(pps_capture)}; + uORB::SubscriptionMultiArray _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 _pps_rate_failure{false}; + uint64_t _last_gps_utc_timestamp{0}; + uint8_t _pps_rate_exceeded_counter{0}; + px4::atomic _pps_rate_failure{false}; + + DEFINE_PARAMETERS( + (ParamInt) _param_pps_cap_gps_id + ) }; diff --git a/src/drivers/pps_capture/pps_capture_params.c b/src/drivers/pps_capture/pps_capture_params.c index 6f5bdaac64..a8f333504d 100644 --- a/src/drivers/pps_capture/pps_capture_params.c +++ b/src/drivers/pps_capture/pps_capture_params.c @@ -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);