fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 12:11:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

wip: squashed patch

Browse Source
This commit is contained in:
Thomas Stastny
2022-12-19 13:12:42 +01:00
parent 283aad01fd
commit ebe0a4095c
659 changed files with 23727 additions and 8530 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/camera_capture/camera_capture.cpp
+27 -1
View File
@@ -40,11 +40,15 @@
*/
#include "camera_capture.hpp"
#include <px4_platform_common/events.h>
#include <systemlib/mavlink_log.h>
#include <board_config.h>
#define commandParamToInt(n) static_cast<int>(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
@@ -244,7 +270,7 @@ CameraCapture::Run()
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;