fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 08:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix PPS based UTC timestamp in camera trigger and capture messages

Browse Source 
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
This commit is contained in:
Michael Schaeuble
2021-12-17 07:56:08 +01:00
committed by Beat Küng
parent ebb657bcf4
commit 5ad8b84dec
6 changed files with 61 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/camera_capture/camera_capture.cpp
+15 -12
View File
@@ -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);
}
src/drivers/camera_capture/camera_capture.hpp
+2 -2
View File
@@ -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);