diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 05a17a5fcf..86fa5c9b04 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -105,7 +105,7 @@ void CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) { _trigger.chan_index = chan_index; - _trigger.edge_time = edge_time; + _trigger.hrt_edge_time = edge_time; _trigger.edge_state = edge_state; _trigger.overflow = overflow; @@ -118,7 +118,8 @@ CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg) CameraCapture *dev = static_cast(arg); dev->_trigger.chan_index = 0; - dev->_trigger.edge_time = hrt_absolute_time(); + dev->_trigger.hrt_edge_time = hrt_absolute_time(); + px4_clock_gettime(CLOCK_REALTIME, &dev->_trigger.rtc_edge_time); dev->_trigger.edge_state = 0; dev->_trigger.overflow = 0; @@ -144,19 +145,20 @@ CameraCapture::publish_trigger() // MODES 1 and 2 are not fully tested if (_camera_capture_mode == 0 || _gpio_capture) { - trigger.timestamp = _trigger.edge_time - uint64_t(1000 * _strobe_delay); + trigger.timestamp = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay); trigger.seq = _capture_seq++; _last_trig_time = trigger.timestamp; + publish = true; } else if (_camera_capture_mode == 1) { // Get timestamp of mid-exposure (active high) if (_trigger.edge_state == 1) { - _last_trig_begin_time = _trigger.edge_time - uint64_t(1000 * _strobe_delay); + _last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay); } else if (_trigger.edge_state == 0 && _last_trig_begin_time > 0) { - trigger.timestamp = _trigger.edge_time - ((_trigger.edge_time - _last_trig_begin_time) / 2); + trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2); trigger.seq = _capture_seq++; - _last_exposure_time = _trigger.edge_time - _last_trig_begin_time; + _last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time; _last_trig_time = trigger.timestamp; publish = true; _capture_seq++; @@ -164,12 +166,12 @@ CameraCapture::publish_trigger() } else { // Get timestamp of mid-exposure (active low) if (_trigger.edge_state == 0) { - _last_trig_begin_time = _trigger.edge_time - uint64_t(1000 * _strobe_delay); + _last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay); } else if (_trigger.edge_state == 1 && _last_trig_begin_time > 0) { - trigger.timestamp = _trigger.edge_time - ((_trigger.edge_time - _last_trig_begin_time) / 2); + trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2); trigger.seq = _capture_seq++; - _last_exposure_time = _trigger.edge_time - _last_trig_begin_time; + _last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time; _last_trig_time = trigger.timestamp; publish = true; } @@ -183,6 +185,14 @@ CameraCapture::publish_trigger() return; } + pps_capture_s pps_capture; + + if (_pps_capture_sub.update(&pps_capture)) { + _rtc_drift_time = pps_capture.rtc_drift_time; + } + + trigger.timestamp_utc = ts_to_abstime(&_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 2ee58ae814..9ad5bbe91e 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -103,11 +104,13 @@ private: // Subscribers uORB::Subscription _command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)}; // Trigger Buffer struct _trig_s { uint32_t chan_index; - hrt_abstime edge_time; + hrt_abstime hrt_edge_time; + timespec rtc_edge_time; uint32_t edge_state; uint32_t overflow; } _trigger{}; @@ -130,6 +133,8 @@ private: hrt_abstime _last_trig_time{0}; uint32_t _capture_overflows{0}; + int32_t _rtc_drift_time{0}; + // Signal capture callback void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);