Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 12:16:17 +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) CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
{ {
_trigger.chan_index = chan_index; _trigger.chan_index = chan_index;
_trigger.edge_time = edge_time; _trigger.hrt_edge_time = edge_time;
_trigger.edge_state = edge_state; _trigger.edge_state = edge_state;
_trigger.overflow = overflow; _trigger.overflow = overflow;
@@ -118,7 +118,8 @@ CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg)
CameraCapture *dev = static_cast<CameraCapture *>(arg); CameraCapture *dev = static_cast<CameraCapture *>(arg);
dev->_trigger.chan_index = 0; 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.edge_state = 0;
dev->_trigger.overflow = 0; dev->_trigger.overflow = 0;
@@ -144,19 +145,20 @@ CameraCapture::publish_trigger()
// MODES 1 and 2 are not fully tested // MODES 1 and 2 are not fully tested
if (_camera_capture_mode == 0 || _gpio_capture) { 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++; trigger.seq = _capture_seq++;
_last_trig_time = trigger.timestamp; _last_trig_time = trigger.timestamp;
publish = true; publish = true;
} else if (_camera_capture_mode == 1) { // Get timestamp of mid-exposure (active high) } else if (_camera_capture_mode == 1) { // Get timestamp of mid-exposure (active high)
if (_trigger.edge_state == 1) { 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) { } 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++; 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; _last_trig_time = trigger.timestamp;
publish = true; publish = true;
_capture_seq++; _capture_seq++;
@@ -164,12 +166,12 @@ CameraCapture::publish_trigger()
} else { // Get timestamp of mid-exposure (active low) } else { // Get timestamp of mid-exposure (active low)
if (_trigger.edge_state == 0) { 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) { } 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++; 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; _last_trig_time = trigger.timestamp;
publish = true; publish = true;
} }
@@ -183,6 +185,14 @@ CameraCapture::publish_trigger()
return; 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); _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/Publication.hpp>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/topics/camera_trigger.h> #include <uORB/topics/camera_trigger.h>
#include <uORB/topics/pps_capture.h>
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h> #include <uORB/topics/vehicle_command_ack.h>
@@ -103,11 +104,13 @@ private:
// Subscribers // Subscribers
uORB::Subscription _command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)};
// Trigger Buffer // Trigger Buffer
struct _trig_s { struct _trig_s {
uint32_t chan_index; uint32_t chan_index;
hrt_abstime edge_time; hrt_abstime hrt_edge_time;
timespec rtc_edge_time;
uint32_t edge_state; uint32_t edge_state;
uint32_t overflow; uint32_t overflow;
} _trigger{}; } _trigger{};
@@ -130,6 +133,8 @@ private:
hrt_abstime _last_trig_time{0}; hrt_abstime _last_trig_time{0};
uint32_t _capture_overflows{0}; uint32_t _capture_overflows{0};
int32_t _rtc_drift_time{0};
// Signal capture callback // Signal capture callback
void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);