From 98d3a2141a794b9c297e8b5d6ca92b86c0e5eff5 Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Wed, 29 Oct 2025 15:23:55 -0400 Subject: [PATCH 1/2] drivers/ins: InertialLabs INS driver bugfix --- src/drivers/ins/ilabs/ILabs.cpp | 26 +++-- src/drivers/ins/ilabs/ILabs.h | 4 + .../ins/ilabs/libilabs/include/sensor.h | 15 ++- src/drivers/ins/ilabs/libilabs/src/sensor.cpp | 104 +++++++++++------- 4 files changed, 94 insertions(+), 55 deletions(-) diff --git a/src/drivers/ins/ilabs/ILabs.cpp b/src/drivers/ins/ilabs/ILabs.cpp index 98f169357d..1c81a818ab 100644 --- a/src/drivers/ins/ilabs/ILabs.cpp +++ b/src/drivers/ins/ilabs/ILabs.cpp @@ -236,34 +236,42 @@ void ILabs::Run() { if (!result) { PX4_ERR("Sensor initializing error"); - ScheduleDelayed(1_s); + _sensor.deinit(); + _time_initialized.store(0); + _time_last_valid_imu_data.store(0); + ScheduleDelayed(3_s); return; } _time_initialized.store(hrt_absolute_time()); } - // check for timeout const hrt_abstime time_initialized = _time_initialized.load(); const hrt_abstime time_last_valid_imu_data = _time_last_valid_imu_data.load(); - if (_param_ilabs_mode.get() == ILabsMode::FULL_INS && time_last_valid_imu_data != 0 && - hrt_elapsed_time(&time_last_valid_imu_data) < 3_s) { - // update sensor_selection if configured in INS mode + // Update sensor_selection for FULL_INS mode + if (_param_ilabs_mode.get() == ILabsMode::FULL_INS && time_initialized != 0 && time_last_valid_imu_data != 0 && + hrt_elapsed_time(&time_last_valid_imu_data) < 3_s) { + if ((_px4_accel.get_device_id() != 0) && (_px4_gyro.get_device_id() != 0)) { sensor_selection_s sensor_selection{}; sensor_selection.accel_device_id = _px4_accel.get_device_id(); sensor_selection.gyro_device_id = _px4_gyro.get_device_id(); - sensor_selection.timestamp = _time_initialized.load(); + sensor_selection.timestamp = time_initialized; _sensor_selection_pub.publish(sensor_selection); } else { PX4_ERR("Sensor not initialized"); } } - if (time_initialized != 0 && hrt_elapsed_time(&time_last_valid_imu_data) > 5_s && - time_last_valid_imu_data != 0 && hrt_elapsed_time(&time_last_valid_imu_data) > 1_s) { - PX4_ERR("Timeout, reinitializing"); + // Missing data handling + if (time_initialized != 0 && time_last_valid_imu_data != 0 && + hrt_elapsed_time(&time_last_valid_imu_data) > 3_s) { + PX4_ERR("Timeout: no new data from sensor. Reinitializing"); _sensor.deinit(); + _time_initialized.store(0); + _time_last_valid_imu_data.store(0); + ScheduleDelayed(3_s); + return; } ScheduleDelayed(100_ms); diff --git a/src/drivers/ins/ilabs/ILabs.h b/src/drivers/ins/ilabs/ILabs.h index 82b94541db..3754dce982 100644 --- a/src/drivers/ins/ilabs/ILabs.h +++ b/src/drivers/ins/ilabs/ILabs.h @@ -80,6 +80,10 @@ private: void Run() override; void processData(InertialLabs::SensorsData *sensordata); static void processDataProxy(void *context, InertialLabs::SensorsData *data) { + if (!context || !data) { + return; + } + ILabs *self = static_cast(context); self->processData(data); } diff --git a/src/drivers/ins/ilabs/libilabs/include/sensor.h b/src/drivers/ins/ilabs/libilabs/include/sensor.h index f8c29bef0a..504cabbc53 100644 --- a/src/drivers/ins/ilabs/libilabs/include/sensor.h +++ b/src/drivers/ins/ilabs/libilabs/include/sensor.h @@ -35,6 +35,7 @@ #include +#include #include #include "data.h" @@ -61,12 +62,15 @@ public: private: static void *updateDataThreadHelper(void *context) { + if (!context) { + return nullptr; + } Sensor *sensor = reinterpret_cast(context); sensor->updateData(); return nullptr; } - void resetSerial(); + bool initSerialPort(const char *serialDeviceName); bool moveToBufferStart(const uint8_t *pos); bool skipPackageInBufferStart(); bool movePackageHeaderToBufferStart(); @@ -74,15 +78,16 @@ private: bool parseUDDPayload(); - device::Serial *_serial{nullptr}; - pthread_t _threadId; - bool _processInThread{false}; + device::Serial *_serial{nullptr}; + pthread_t _threadId{0}; + px4::atomic_bool _processInThread{false}; // callback. C-style class method pointer void *_context{nullptr}; DataHandler _dataHandler{nullptr}; - bool _isInitialized{false}; + px4::atomic_bool _isInitialized{false}; + px4::atomic_bool _isDeinitInProcess{false}; uint8_t _buf[BUFFER_SIZE]{}; uint16_t _bufOffset{0}; diff --git a/src/drivers/ins/ilabs/libilabs/src/sensor.cpp b/src/drivers/ins/ilabs/libilabs/src/sensor.cpp index fa79551188..ba6427536c 100644 --- a/src/drivers/ins/ilabs/libilabs/src/sensor.cpp +++ b/src/drivers/ins/ilabs/libilabs/src/sensor.cpp @@ -86,72 +86,72 @@ Sensor::~Sensor() { } bool Sensor::init(const char *serialDeviceName, void *context, DataHandler dataHandler) { - if (_isInitialized) { - PX4_ERR("Serial device already initialized. Deinitialize first."); + if (_isInitialized.load()) { + PX4_WARN("Serial device already initialized. Deinitialize first"); + return false; + } + if (_isDeinitInProcess.load()) { + PX4_WARN("Deinitialization is in progress. Please wait until it completes before re-initializing"); return false; } - if (serialDeviceName[0] == '\0') { PX4_ERR("Empty serial device name"); return false; } - if (!context || !dataHandler) { PX4_ERR("Empty data handler callback"); return false; } - _serial = new device::Serial(serialDeviceName, BAUDRATE); - if (_serial == nullptr) { - PX4_ERR("Error creating serial device: %s", serialDeviceName); - px4_sleep(1); // NOLINT(concurrency-mt-unsafe) + if (!initSerialPort(serialDeviceName)) { return false; } - if (!_serial->isOpen() && !_serial->open()) { - // Open the UART. If this is successful then the UART is ready to use. - if (!_serial->open()) { - PX4_ERR("Error opening serial device: %s", serialDeviceName); - px4_sleep(1); // NOLINT(concurrency-mt-unsafe) - resetSerial(); - return false; - } - - PX4_INFO("Serial device opened sucessfully: %s", serialDeviceName); - - _serial->flush(); - } _context = context; _dataHandler = dataHandler; - _processInThread = true; + _processInThread.store(true); const int result = pthread_create(&_threadId, nullptr, &Sensor::updateDataThreadHelper, this); if (result) { PX4_ERR("Cant create working thread"); - _processInThread = false; - px4_sleep(1); // NOLINT(concurrency-mt-unsafe) - resetSerial(); + deinit(); return false; } - pthread_detach(_threadId); - _isInitialized = true; + _isInitialized.store(true); return true; } void Sensor::deinit() { - _processInThread = false; - _isInitialized = false; - resetSerial(); + if (_isDeinitInProcess.load()) { + PX4_WARN("Deinitialization already in process"); + return; + } + + _isDeinitInProcess.store(true); + _isInitialized.store(false); + _processInThread.store(false); + // Wait to be shure, that operations with UART are finished + px4_sleep(1); // NOLINT(concurrency-mt-unsafe) + if (_serial) { + (void)_serial->close(); + delete _serial; + _serial = nullptr; + } + + _context = nullptr; + _dataHandler = nullptr; + _bufOffset = 0; + _isDeinitInProcess.store(false); } bool Sensor::isInitialized() const { - return _isInitialized; + return _isInitialized.load(); } void Sensor::updateData() { - while (_processInThread) { + while (_processInThread.load()) { bool result = moveValidPackageToBufferStart(); if (!result) { continue; @@ -162,18 +162,40 @@ void Sensor::updateData() { continue; } - _dataHandler(_context, &_sensorData); + if (_dataHandler) { + _dataHandler(_context, &_sensorData); + } skipPackageInBufferStart(); } } -void Sensor::resetSerial() { +bool Sensor::initSerialPort(const char *serialDeviceName) { if (_serial) { - (void)_serial->close(); - delete _serial; - _serial = nullptr; + PX4_WARN("Serial port already initialized. Deinitialize first"); + return false; } + + _serial = new device::Serial(serialDeviceName, BAUDRATE); + if (_serial == nullptr) { + PX4_ERR("Error creating serial device: %s", serialDeviceName); + px4_sleep(1); // NOLINT(concurrency-mt-unsafe) + return false; + } + + if (!_serial->isOpen()) { + _serial->open(); + if (!_serial->isOpen()) { + PX4_ERR("Error opening serial device: %s", serialDeviceName); + deinit(); + return false; + } + PX4_INFO("Serial device opened sucessfully: %s", serialDeviceName); + } else { + PX4_INFO("Serial device already opened: %s", serialDeviceName); + } + _serial->flush(); + return true; } bool Sensor::moveToBufferStart(const uint8_t *pos) { @@ -201,7 +223,7 @@ bool Sensor::moveToBufferStart(const uint8_t *pos) { bool Sensor::skipPackageInBufferStart() { const MessageHeader *packageHeader = reinterpret_cast(_buf); if (!isMessageHeaderCorrect(packageHeader)) { - PX4_ERR("Incorrect message header in buffer start. Can't skip the package correctly."); + PX4_ERR("Incorrect message header in buffer start. Can't skip the package correctly"); return false; } @@ -239,7 +261,7 @@ bool Sensor::moveValidPackageToBufferStart() { return false; } - const ssize_t nRead = _serial->readAtLeast(&_buf[_bufOffset], freeBufByteCount, freeBufByteCount, 1000); + const ssize_t nRead = _serial->readAtLeast(&_buf[_bufOffset], freeBufByteCount, freeBufByteCount, 500); if (nRead != freeBufByteCount) { PX4_ERR("Can't read requested data from the serial device. Bytes requested: %d. Bytes read: %d", freeBufByteCount, static_cast(nRead)); @@ -546,8 +568,8 @@ bool Sensor::parseUDDPayload() { break; } default: { - PX4_ERR("Unknown message type: %d. Further package parsing result will be incorrect!", - messageType); + PX4_ERR("Unknown message type: %d. Further package parsing result will be incorrect", + messageType); messageLength = 0; break; } From 5eab16c17c1bc78237ca9af49de71b1450da3616 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Tue, 11 Nov 2025 20:56:56 +0100 Subject: [PATCH 2/2] PPS time corection capability for GNSS measurements (#25838) ** add PPS time corection capability for GNSS measurements * * add documentation * add comment for 'future check' * replace url with relative link * Update docs/en/advanced/pps_time_sync.md Co-authored-by: Silvan Fuhrer * Subedit and add to sidebar * Apply suggestions from code review * Update docs/en/SUMMARY.md * remove offset jump check, clean up * add comment pps_compensation activation condition. move documentation to hardware integration --------- Co-authored-by: Hamish Willee Co-authored-by: Silvan Fuhrer --- docs/en/SUMMARY.md | 1 + docs/en/advanced/pps_time_sync.md | 133 ++++++++++++++++++ docs/en/gps_compass/index.md | 2 + src/modules/ekf2/EKF/estimator_interface.cpp | 6 +- src/modules/ekf2/EKF/estimator_interface.h | 2 +- src/modules/ekf2/EKF2.cpp | 8 +- src/modules/ekf2/params_gnss.yaml | 4 +- .../vehicle_gps_position/CMakeLists.txt | 2 + .../vehicle_gps_position/PpsTimeSync.cpp | 104 ++++++++++++++ .../vehicle_gps_position/PpsTimeSync.hpp | 67 +++++++++ .../VehicleGPSPosition.cpp | 7 + .../VehicleGPSPosition.hpp | 5 + 12 files changed, 335 insertions(+), 6 deletions(-) create mode 100644 docs/en/advanced/pps_time_sync.md create mode 100644 src/modules/sensors/vehicle_gps_position/PpsTimeSync.cpp create mode 100644 src/modules/sensors/vehicle_gps_position/PpsTimeSync.hpp diff --git a/docs/en/SUMMARY.md b/docs/en/SUMMARY.md index 73cefc0ed1..030b856d55 100644 --- a/docs/en/SUMMARY.md +++ b/docs/en/SUMMARY.md @@ -485,6 +485,7 @@ - [UART/Serial Ports](uart/index.md) - [Port-Configurable Serial Drivers](uart/user_configurable_serial_driver.md) - [RTK GPS (Integration)](advanced/rtk_gps.md) + - [PPS Time Synchronization](advanced/pps_time_sync.md) - [Middleware](middleware/index.md) - [uORB Messaging](middleware/uorb.md) - [uORB Graph](middleware/uorb_graph.md) diff --git a/docs/en/advanced/pps_time_sync.md b/docs/en/advanced/pps_time_sync.md new file mode 100644 index 0000000000..fcd4291f70 --- /dev/null +++ b/docs/en/advanced/pps_time_sync.md @@ -0,0 +1,133 @@ +# PPS Time Synchronization (PX4 Integration) + +[Pulse Per Second](https://en.wikipedia.org/wiki/Pulse-per-second_signal) (PPS) time synchronization provides high-precision timing for GNSS receivers. +This page explains how PPS is integrated into PX4 and how to configure it. + +## Overview + +PPS (Pulse Per Second) is a timing signal provided by GNSS receivers that outputs an electrical pulse once per second, synchronized to UTC time. +The PPS signal provides a highly accurate timing reference that PX4 can use to: + +- Refine GNSS time measurements and compensate for clock drift +- Provide precise UTC timestamps for camera capture events (for photogrammetry and mapping applications) +- Enable offline position refinement through accurate time correlation + +## Supported Hardware + +PPS time synchronization can be supported on flight controllers that have a hardware timer input pin that can be configured for PPS capture, by [enabling the PPS capture driver](#enable-pps-driver-in-board-configuration) in the board configuration. + +Supported boards include (at time of writing): + +- [Ark FMUv6x](../flight_controller/ark_v6x.md) +- Auterion FMUv6x +- Auterion FMUv6s + +## Setup + +### Enable PPS Driver in Board Configuration + +The [PPS capture driver](../modules/modules_driver.md#pps-capture) must be enabled in the board configuration. +This is done by adding the following to your board's configuration: + +```ini +CONFIG_DRIVERS_PPS_CAPTURE=y +``` + +### Configure PPS Parameters + +The configuration varies depending on your flight controller hardware. + +#### FMUv6X + +For FMUv6X-based flight controllers, configure PWM AUX Timer 3 and Function 9: + +```sh +param set PWM_AUX_TIM3 -2 +param set PWM_AUX_FUNC9 2064 +param set PPS_CAP_ENABLE 1 +``` + +#### FMUv6S + +For FMUv6S-based flight controllers, configure PWM MAIN Timer 3 and Function 10: + +```sh +param set PWM_MAIN_TIM3 -2 +param set PWM_MAIN_FUNC10 2064 +param set PPS_CAP_ENABLE 1 +``` + +### Wiring + +The wiring configuration depends on your specific flight controller. + +#### Skynode X (FMUv6x) + +Connect the PPS signal from your GNSS module to the flight controller using the 11-pin or 6-pin GPS connector: + +For detailed pinout information, refer to: + +- [Skynode GPS Peripherals - Pinouts](https://docs.auterion.com/hardware-integration/skynode/peripherals/gps#pinouts) + +#### Skynode S (FMUv6S) + +For FMUv6S, you need to route the PPS signal separately: + +1. Connect your GNSS module using the standard 6-pin GPS connector: [Skynode S GPS Interface](https://docs.auterion.com/hardware-integration/skynode-s/interfaces#gps) +2. Connect the PPS signal from your GNSS module to the **PPM_IN** pin: [Skynode S Extras 1 Interface](https://docs.auterion.com/hardware-integration/skynode-s/interfaces#extras-1) + +#### ARK Jetson Carrier Board (FMUv6x) + +For ARK FMUv6X on the Jetson carrier board: + +1. Connect your GNSS module using either the 10-pin or 6-pin GPS connector: [ARK PAB GPS1 Interface](../flight_controller/ark_pab#gps1) +2. Connect the PPS signal to the **FMU_CAP** pin: [ARK PAB ADIO Interface](../flight_controller/ark_pab.md#adio) + +## Verification + +After configuring PPS, you can verify that it is working correctly: + +1. Connect to the [PX4 System Console](../debug/system_console.md) (via MAVLink shell or serial console). +2. Wait for GNSS fix. +3. Check the PPS capture status to confirm it is up and running: + + ```sh + pps_capture status + ``` + +4. You can also check the [PpsCapture](../msg_docs/PpsCapture.md) uORB topic + + ```sh + listener pps_capture + ``` + + Where you should see: `timestamp`, `rtc_timestamp`, and `pps_rate_exceeded_counter`. + +### PPS Capture Driver + +The PPS capture driver is located in `src/drivers/pps_capture` and uses hardware timer input capture to precisely measure the arrival time of each PPS pulse. + +Key features: + +- Sub-microsecond pulse capture precision (hardware-dependent) +- Automatic drift calculation and compensation +- Integration with the GNSS driver for refined time stamping + +See also: + +- [PPS Capture Driver Documentation](../modules/modules_driver.md#pps-capture) +- [PpsCapture Message](../msg_docs/PpsCapture.md) + +### Time Synchronization Flow + +1. GNSS module sends position/time data at ~1-20 Hz. +2. GNSS module outputs PPS pulse at 1 Hz, precisely aligned to UTC second boundary. +3. PPS capture driver measures the exact time of the PPS pulse arrival using hardware timer. +4. Driver calculates the offset between GNSS time (from UART data) and autopilot clock (from PPS measurement). +5. This offset is used to correct GNSS timestamps and improve sensor fusion accuracy. + +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 `EKF2_GPS_DELAY` will be used instead for estimating the latency. +::: diff --git a/docs/en/gps_compass/index.md b/docs/en/gps_compass/index.md index dbd9514aec..6b965d2fea 100644 --- a/docs/en/gps_compass/index.md +++ b/docs/en/gps_compass/index.md @@ -200,7 +200,9 @@ EPH/EPV values therefore provide a more immediate and practical estimate of the - GPS/RTK-GPS - [RTK-GPS](../advanced/rtk_gps.md) + - [PPS Time Synchronization](../advanced/pps_time_sync.md) - [GPS driver](../modules/modules_driver.md#gps) + - [PPS driver](../modules/modules_driver.md#pps-capture) - [DroneCAN Example](../dronecan/index.md) - Compass - [Driver source code](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/magnetometer) (Compasses) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 5c30c72afc..64ee74f1d4 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -159,7 +159,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample) #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS) -void EstimatorInterface::setGpsData(const gnssSample &gnss_sample) +void EstimatorInterface::setGpsData(const gnssSample &gnss_sample, const bool pps_compensation) { if (!_initialised) { return; @@ -177,8 +177,10 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample) } } + const int64_t delay = pps_compensation ? 0 : static_cast(_params.ekf2_gps_delay * 1000); + const int64_t time_us = gnss_sample.time_us - - static_cast(_params.ekf2_gps_delay * 1000) + - delay - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 if (time_us >= static_cast(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 64d3e4ffc3..b87472f919 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -89,7 +89,7 @@ public: void setIMUData(const imuSample &imu_sample); #if defined(CONFIG_EKF2_GNSS) - void setGpsData(const gnssSample &gnss_sample); + void setGpsData(const gnssSample &gnss_sample, const bool pps_compensation = false); const gnssSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f93f59085f..7a59a0e7a1 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2452,8 +2452,12 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) const float altitude_amsl = static_cast(vehicle_gps_position.altitude_msl_m); const float altitude_ellipsoid = static_cast(vehicle_gps_position.altitude_ellipsoid_m); + // if pps_compensation is active but not valid, the timestamp_sample will be equal to timestamp + const bool pps_compensation = vehicle_gps_position.timestamp_sample > 0 + && vehicle_gps_position.timestamp_sample != vehicle_gps_position.timestamp; + gnssSample gnss_sample{ - .time_us = vehicle_gps_position.timestamp, + .time_us = pps_compensation ? vehicle_gps_position.timestamp_sample : vehicle_gps_position.timestamp, .lat = vehicle_gps_position.latitude_deg, .lon = vehicle_gps_position.longitude_deg, .alt = altitude_amsl, @@ -2471,7 +2475,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) .spoofed = vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_DETECTED, }; - _ekf.setGpsData(gnss_sample); + _ekf.setGpsData(gnss_sample, pps_compensation); const float geoid_height = altitude_ellipsoid - altitude_amsl; diff --git a/src/modules/ekf2/params_gnss.yaml b/src/modules/ekf2/params_gnss.yaml index bcc186e0ba..b8168e494a 100644 --- a/src/modules/ekf2/params_gnss.yaml +++ b/src/modules/ekf2/params_gnss.yaml @@ -29,7 +29,9 @@ parameters: default: 0 EKF2_GPS_DELAY: description: - short: GPS measurement delay relative to IMU measurements + short: GPS measurement delay relative to IMU measurement + long: GPS measurement delay relative to IMU measurement if PPS time correction + is not available/enabled (PPS_CAP_ENABLE). type: float default: 110 min: 0 diff --git a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt index 12b625ead6..c82a636118 100644 --- a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt +++ b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt @@ -36,6 +36,8 @@ px4_add_library(vehicle_gps_position VehicleGPSPosition.hpp gps_blending.cpp gps_blending.hpp + PpsTimeSync.cpp + PpsTimeSync.hpp ) target_link_libraries(vehicle_gps_position PRIVATE diff --git a/src/modules/sensors/vehicle_gps_position/PpsTimeSync.cpp b/src/modules/sensors/vehicle_gps_position/PpsTimeSync.cpp new file mode 100644 index 0000000000..c139484456 --- /dev/null +++ b/src/modules/sensors/vehicle_gps_position/PpsTimeSync.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file PpsTimeSync.cpp + * + * PPS-based time synchronization implementation + */ + +#include "PpsTimeSync.hpp" +#include +#include + +void PpsTimeSync::process_pps(const pps_capture_s &pps) +{ + if (pps.timestamp == 0 || pps.rtc_timestamp == 0) { + return; + } + + _pps_hrt_timestamp = pps.timestamp; + _pps_rtc_timestamp = pps.rtc_timestamp; + _time_offset = (int64_t)pps.rtc_timestamp - (int64_t)pps.timestamp; + _initialized = true; + _updated = true; +} + +uint64_t PpsTimeSync::correct_gps_timestamp(uint64_t gps_fc_timestamp, uint64_t gps_utc_timestamp) +{ + if (!is_valid()) { + return gps_fc_timestamp; + } + + const int64_t corrected_fc_timestamp = (int64_t)gps_utc_timestamp - _time_offset; + + if (_updated) { + const int64_t correction_amount = corrected_fc_timestamp - (int64_t)gps_fc_timestamp; + + if (math::abs_t(correction_amount) > kPpsMaxCorrectionUs) { + PX4_DEBUG("PPS: Correction too large: %" PRId64 " us (%.1f ms), rejecting", + correction_amount, (double)correction_amount / 1000.0); + return gps_fc_timestamp; + } + + // Additional sanity check: corrected timestamp should not be too far in the future (0.1s) + const uint64_t now = hrt_absolute_time(); + + if ((uint64_t)corrected_fc_timestamp > now + 100000) { + return gps_fc_timestamp; + } + + _updated = false; + } + + return (uint64_t)corrected_fc_timestamp; +} + +bool PpsTimeSync::is_valid() const +{ + if (!_initialized) { + return false; + } + + uint64_t now = hrt_absolute_time(); + + if (now < _pps_hrt_timestamp) { + now = UINT64_MAX; + } + + if (now - _pps_hrt_timestamp > kPpsStaleTimeoutUs) { + return false; + } + + return true; +} diff --git a/src/modules/sensors/vehicle_gps_position/PpsTimeSync.hpp b/src/modules/sensors/vehicle_gps_position/PpsTimeSync.hpp new file mode 100644 index 0000000000..433d13154b --- /dev/null +++ b/src/modules/sensors/vehicle_gps_position/PpsTimeSync.hpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file PpsTimeSync.hpp + * + * PPS-based time synchronization for GPS timestamp correction + */ + +#pragma once + +#include +#include +#include + +class PpsTimeSync +{ +public: + PpsTimeSync() = default; + ~PpsTimeSync() = default; + + void process_pps(const pps_capture_s &pps); + uint64_t correct_gps_timestamp(uint64_t gps_fc_timestamp, uint64_t gps_utc_timestamp); + +private: + bool is_valid() const; + + uint64_t _pps_hrt_timestamp{0}; // FC time when PPS pulse arrived (usec since boot) + uint64_t _pps_rtc_timestamp{0}; // GPS UTC time at PPS pulse (usec since Unix epoch) + int64_t _time_offset{0}; + + static constexpr uint64_t kPpsStaleTimeoutUs = 5'000'000; + static constexpr int64_t kPpsMaxCorrectionUs = 300'000; // max delay (max of EKF2_GPS_DELAY) + + bool _initialized{false}; + bool _updated{false}; +}; diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 964b5fdecc..417001846c 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -104,6 +104,12 @@ void VehicleGPSPosition::Run() perf_begin(_cycle_perf); ParametersUpdate(); + pps_capture_s pps_capture; + + if (_pps_capture_sub.update(&pps_capture)) { + _pps_time_sync.process_pps(pps_capture); + } + // Check all GPS instance bool any_gps_updated = false; bool gps_updated = false; @@ -136,6 +142,7 @@ void VehicleGPSPosition::Run() gps_output.device_id = 0; } + gps_output.timestamp_sample = _pps_time_sync.correct_gps_timestamp(gps_output.timestamp, gps_output.time_utc_usec); _vehicle_gps_position_pub.publish(gps_output); } } diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index 971f24ae4a..c2d60e6db3 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -45,8 +45,10 @@ #include #include #include +#include #include "gps_blending.hpp" +#include "PpsTimeSync.hpp" using namespace time_literals; @@ -88,9 +90,12 @@ private: {this, ORB_ID(sensor_gps), 1}, }; + uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)}; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; GpsBlending _gps_blending; + PpsTimeSync _pps_time_sync; DEFINE_PARAMETERS( (ParamInt) _param_sens_gps_mask,