mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-13 11:03:34 +08:00
Merge branch 'main' into pr-MCP23017
This commit is contained in:
Phil-Engljaehringer
@@ -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)
|
||||
|
||||
@@ -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.
|
||||
:::
|
||||
@@ -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)
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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<ILabs *>(context);
|
||||
self->processData(data);
|
||||
}
|
||||
|
||||
@@ -35,6 +35,7 @@
|
||||
|
||||
#include <pthread.h>
|
||||
|
||||
#include <px4_platform_common/atomic.h>
|
||||
#include <px4_platform_common/Serial.hpp>
|
||||
|
||||
#include "data.h"
|
||||
@@ -61,12 +62,15 @@ public:
|
||||
|
||||
private:
|
||||
static void *updateDataThreadHelper(void *context) {
|
||||
if (!context) {
|
||||
return nullptr;
|
||||
}
|
||||
Sensor *sensor = reinterpret_cast<Sensor *>(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};
|
||||
|
||||
@@ -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<MessageHeader *>(_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<int>(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;
|
||||
}
|
||||
|
||||
@@ -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<int64_t>(_params.ekf2_gps_delay * 1000);
|
||||
|
||||
const int64_t time_us = gnss_sample.time_us
|
||||
- static_cast<int64_t>(_params.ekf2_gps_delay * 1000)
|
||||
- delay
|
||||
- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
|
||||
|
||||
if (time_us >= static_cast<int64_t>(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) {
|
||||
|
||||
@@ -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; }
|
||||
|
||||
|
||||
@@ -2452,8 +2452,12 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
const float altitude_amsl = static_cast<float>(vehicle_gps_position.altitude_msl_m);
|
||||
const float altitude_ellipsoid = static_cast<float>(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;
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 <px4_platform_common/log.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -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 <stdint.h>
|
||||
#include <uORB/topics/pps_capture.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
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};
|
||||
};
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -45,8 +45,10 @@
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/pps_capture.h>
|
||||
|
||||
#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<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
|
||||
|
||||
Reference in New Issue
Block a user