diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 86fa5c9b04..57fd0cc736 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -106,6 +106,11 @@ 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; @@ -119,7 +124,11 @@ CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg) dev->_trigger.chan_index = 0; dev->_trigger.hrt_edge_time = hrt_absolute_time(); - px4_clock_gettime(CLOCK_REALTIME, &dev->_trigger.rtc_edge_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; @@ -191,7 +200,7 @@ CameraCapture::publish_trigger() _rtc_drift_time = pps_capture.rtc_drift_time; } - trigger.timestamp_utc = ts_to_abstime(&_trigger.rtc_edge_time) + _rtc_drift_time; + trigger.timestamp_utc = _trigger.rtc_edge_time + _rtc_drift_time; _trigger_pub.publish(trigger); } diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index 9ad5bbe91e..efa6871643 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -110,7 +110,7 @@ private: struct _trig_s { uint32_t chan_index; hrt_abstime hrt_edge_time; - timespec rtc_edge_time; + uint64_t rtc_edge_time; uint32_t edge_state; uint32_t overflow; } _trigger{};