From 5ad8b84decf1760a0f0c2181849d406814687f17 Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Thu, 11 Nov 2021 11:05:03 +0100 Subject: [PATCH] Fix PPS based UTC timestamp in camera trigger and capture messages The existing implementation has about 100ms difference to a reference clock. With the changes this error less than 25us. - Use sensor_gps messages with hrt timestamps as RTC reference and not the system realtime clock. The PPS interrupt can then be aligned with the GPS clock system. - Keep fallback based on system RTC when no PPS pulse was captured --- msg/pps_capture.msg | 5 ++-- src/drivers/camera_capture/camera_capture.cpp | 27 ++++++++++--------- src/drivers/camera_capture/camera_capture.hpp | 4 +-- src/drivers/camera_trigger/camera_trigger.cpp | 19 +++++++++---- src/drivers/pps_capture/PPSCapture.cpp | 27 ++++++++++++++----- src/drivers/pps_capture/PPSCapture.hpp | 8 +++++- 6 files changed, 60 insertions(+), 30 deletions(-) diff --git a/msg/pps_capture.msg b/msg/pps_capture.msg index 0444d1a572..b9ec949a9c 100644 --- a/msg/pps_capture.msg +++ b/msg/pps_capture.msg @@ -1,3 +1,2 @@ -uint64 timestamp # time since system start (microseconds) -uint64 rtc_timestamp # captured time in microseconds from real-time clock at PPS event -int32 rtc_drift_time # time drift between RTC and PPS event in microseconds \ No newline at end of file +uint64 timestamp # time since system start (microseconds) at PPS capture event +uint64 rtc_timestamp # Corrected GPS UTC timestamp at PPS capture event \ No newline at end of file diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 57fd0cc736..c47d27ba67 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -106,11 +106,6 @@ CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint { _trigger.chan_index = chan_index; _trigger.hrt_edge_time = edge_time; - - timespec tv{}; - px4_clock_gettime(CLOCK_REALTIME, &tv); - _trigger.rtc_edge_time = ts_to_abstime(&tv) - hrt_elapsed_time(&edge_time); - _trigger.edge_state = edge_state; _trigger.overflow = overflow; @@ -124,11 +119,6 @@ CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg) dev->_trigger.chan_index = 0; dev->_trigger.hrt_edge_time = hrt_absolute_time(); - - timespec tv{}; - px4_clock_gettime(CLOCK_REALTIME, &tv); - dev->_trigger.rtc_edge_time = ts_to_abstime(&tv); - dev->_trigger.edge_state = 0; dev->_trigger.overflow = 0; @@ -197,10 +187,23 @@ CameraCapture::publish_trigger() pps_capture_s pps_capture; if (_pps_capture_sub.update(&pps_capture)) { - _rtc_drift_time = pps_capture.rtc_drift_time; + _pps_hrt_timestamp = pps_capture.timestamp; + _pps_rtc_timestamp = pps_capture.rtc_timestamp; } - trigger.timestamp_utc = _trigger.rtc_edge_time + _rtc_drift_time; + + if (_pps_hrt_timestamp > 0) { + // Current RTC time (RTC time captured by the PPS module + elapsed time since capture) + uint64_t gps_utc_time = _pps_rtc_timestamp + hrt_elapsed_time(&_pps_hrt_timestamp); + // Current RTC time - elapsed time since capture interrupt event + trigger.timestamp_utc = gps_utc_time - hrt_elapsed_time(&trigger.timestamp); + + } else { + // No PPS capture received, use RTC clock as fallback + timespec tv{}; + px4_clock_gettime(CLOCK_REALTIME, &tv); + trigger.timestamp_utc = ts_to_abstime(&tv) - hrt_elapsed_time(&trigger.timestamp); + } _trigger_pub.publish(trigger); } diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index efa6871643..2618cf73ac 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -110,7 +110,6 @@ private: struct _trig_s { uint32_t chan_index; hrt_abstime hrt_edge_time; - uint64_t rtc_edge_time; uint32_t edge_state; uint32_t overflow; } _trigger{}; @@ -133,7 +132,8 @@ private: hrt_abstime _last_trig_time{0}; uint32_t _capture_overflows{0}; - int32_t _rtc_drift_time{0}; + uint64_t _pps_hrt_timestamp{0}; + uint64_t _pps_rtc_timestamp{0}; // Signal capture callback void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 260e8d19e8..0e0fbfa91e 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -174,7 +174,8 @@ private: bool _turning_on{false}; matrix::Vector2f _last_shoot_position{0.f, 0.f}; bool _valid_position{false}; - int32_t _rtc_drift_time{0}; + uint64_t _pps_hrt_timestamp{0}; + uint64_t _pps_rtc_timestamp{0}; //Camera Auto Mount Pivoting Oblique Survey (CAMPOS) uint32_t _CAMPOS_num_poses{0}; @@ -828,16 +829,24 @@ CameraTrigger::engage(void *arg) pps_capture_s pps_capture; if (trig->_pps_capture_sub.update(&pps_capture)) { - trig->_rtc_drift_time = pps_capture.rtc_drift_time; + trig->_pps_hrt_timestamp = pps_capture.timestamp; + trig->_pps_rtc_timestamp = pps_capture.rtc_timestamp; } // Send camera trigger message. This messages indicates that we sent // the camera trigger request. Does not guarantee capture. camera_trigger_s trigger{}; - timespec tv{}; - px4_clock_gettime(CLOCK_REALTIME, &tv); - trigger.timestamp_utc = ts_to_abstime(&tv) + trig->_rtc_drift_time; + if (trig->_pps_hrt_timestamp > 0) { + // Current RTC time (RTC time captured by the PPS module + elapsed time since capture) + trigger.timestamp_utc = trig->_pps_rtc_timestamp + hrt_elapsed_time(&trig->_pps_hrt_timestamp); + + } else { + // No PPS capture received, use RTC clock as fallback + timespec tv{}; + px4_clock_gettime(CLOCK_REALTIME, &tv); + trigger.timestamp_utc = ts_to_abstime(&tv); + } trigger.seq = trig->_trigger_seq; trigger.feedback = false; diff --git a/src/drivers/pps_capture/PPSCapture.cpp b/src/drivers/pps_capture/PPSCapture.cpp index db09dd031f..c151f99a35 100644 --- a/src/drivers/pps_capture/PPSCapture.cpp +++ b/src/drivers/pps_capture/PPSCapture.cpp @@ -89,14 +89,27 @@ void PPSCapture::Run() return; } + 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; + } + pps_capture_s pps_capture; - pps_capture.timestamp = hrt_absolute_time(); - pps_capture.rtc_timestamp = ts_to_abstime(&_rtc_system_time); - uint32_t microseconds_part = pps_capture.rtc_timestamp % USEC_IN_1_SEC; + pps_capture.timestamp = _hrt_timestamp; + // GPS UTC time when the GPIO interrupt was triggered + // UTC time + elapsed time since the UTC time was received - eleapsed time since the PPS interrupt + // event + uint64_t gps_utc_time = _last_gps_utc_timestamp + hrt_elapsed_time(&_last_gps_timestamp) + - hrt_elapsed_time(&_hrt_timestamp); + + // (For ubx F9P) The rising edge of the PPS pulse is aligned to the top of second GPS time base. + // So, remove the fraction of second and shift to the next second. The interrupt is triggered + // before the matching timestamp tranfered, which means the last received GPS time is always + // behind. + pps_capture.rtc_timestamp = gps_utc_time - (gps_utc_time % USEC_PER_SEC) + USEC_PER_SEC; - /* max drift time to detect with PPS is in the range between -0.5 s to +0.5 s */ - pps_capture.rtc_drift_time = (microseconds_part >= MAX_POSITIVE_DRIFT) ? ((-1 * USEC_IN_1_SEC) + microseconds_part) - : microseconds_part; _pps_capture_pub.publish(pps_capture); } @@ -104,7 +117,7 @@ int PPSCapture::gpio_interrupt_callback(int irq, void *context, void *arg) { PPSCapture *instance = static_cast(arg); - px4_clock_gettime(CLOCK_REALTIME, &(instance->_rtc_system_time)); + instance->_hrt_timestamp = hrt_absolute_time(); instance->ScheduleNow(); // schedule work queue to publish PPS captured time return PX4_OK; diff --git a/src/drivers/pps_capture/PPSCapture.hpp b/src/drivers/pps_capture/PPSCapture.hpp index 3e46614166..0f1d812019 100644 --- a/src/drivers/pps_capture/PPSCapture.hpp +++ b/src/drivers/pps_capture/PPSCapture.hpp @@ -38,7 +38,9 @@ #include #include #include +#include #include +#include using namespace time_literals; @@ -69,10 +71,14 @@ private: uint32_t _pps_capture_gpio{0}; uORB::Publication _pps_capture_pub{ORB_ID(pps_capture)}; + uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)}; static constexpr unsigned USEC_IN_1_SEC{1000000}; // microseconds in 1 second static constexpr unsigned MAX_POSITIVE_DRIFT{USEC_IN_1_SEC / 2}; // microseconds - timespec _rtc_system_time{0}; + hrt_abstime _hrt_timestamp{0}; + + uint64_t _last_gps_timestamp{0}; + uint64_t _last_gps_utc_timestamp{0}; };