Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

camera_capture: update to use PPS correction for UTC time

Browse Source
This commit is contained in:
Igor Mišić
2021-11-01 15:29:43 +01:00
committed by Beat Küng
parent 8eca35111e
commit c6098ef3dd
2 changed files with 25 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/camera_capture/camera_capture.cpp
+19 -9
View File
@@ -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<CameraCapture *>(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);
}
src/drivers/camera_capture/camera_capture.hpp
+6 -1
View File
@@ -51,6 +51,7 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/pps_capture.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
@@ -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);