diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 7d1183b2df..0066979a1b 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -40,11 +40,15 @@ */ #include "camera_capture.hpp" +#include +#include #include #define commandParamToInt(n) static_cast(n >= 0 ? n + 0.5f : n - 0.5f) +using namespace time_literals; + namespace camera_capture { CameraCapture *g_camera_capture{nullptr}; @@ -104,6 +108,21 @@ CameraCapture::~CameraCapture() void CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) { + // Maximum acceptable rate is 5kHz + if ((edge_time - _trigger.hrt_edge_time) < 200_us) { + ++_trigger_rate_exceeded_counter; + + if (_trigger_rate_exceeded_counter > 100) { + + // Trigger rate too high, stop future interrupts + up_input_capture_set(_capture_channel, Disabled, 0, nullptr, nullptr); + _trigger_rate_failure.store(true); + } + + } else if (_trigger_rate_exceeded_counter > 0) { + --_trigger_rate_exceeded_counter; + } + _trigger.chan_index = chan_index; _trigger.hrt_edge_time = edge_time; _trigger.edge_state = edge_state; @@ -140,6 +159,13 @@ CameraCapture::publish_trigger() { bool publish = false; + if (_trigger_rate_failure.load()) { + mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: Camera capture disabled\t"); + events::send(events::ID("camera_capture_trigger_rate_exceeded"), + events::Log::Error, "Hardware fault: Camera capture disabled"); + _trigger_rate_failure.store(false); + } + camera_trigger_s trigger{}; // MODES 1 and 2 are not fully tested diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index baeebb21a3..92dd2790d1 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -134,6 +134,10 @@ private: hrt_abstime _pps_hrt_timestamp{0}; uint64_t _pps_rtc_timestamp{0}; + uint8_t _trigger_rate_exceeded_counter{0}; + px4::atomic _trigger_rate_failure{false}; + + orb_advert_t _mavlink_log_pub{nullptr}; // Signal capture callback void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);