diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index cc5c88fdae..8c667f55aa 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -40,6 +40,7 @@ * @author Mohammed Kabir * @author Kelly Steich * @author Andreas Bircher + * @author Lorenz Meier */ #include @@ -161,8 +162,10 @@ private: float _activation_time; float _interval; + float _min_interval; float _distance; uint32_t _trigger_seq; + hrt_abstime _trigger_timestamp; bool _trigger_enabled; bool _trigger_paused; bool _one_shot; @@ -181,6 +184,7 @@ private: param_t _p_mode; param_t _p_activation_time; param_t _p_interval; + param_t _p_min_interval; param_t _p_distance; param_t _p_interface; param_t _p_cam_cap_fback; @@ -242,8 +246,10 @@ CameraTrigger::CameraTrigger() : _keepalivecall_down {}, _activation_time(0.5f /* ms */), _interval(100.0f /* ms */), + _min_interval(1.0f /* ms */), _distance(25.0f /* m */), _trigger_seq(0), + _trigger_timestamp(0), _trigger_enabled(false), _trigger_paused(false), _one_shot(false), @@ -266,6 +272,7 @@ CameraTrigger::CameraTrigger() : // Parameters _p_interval = param_find("TRIG_INTERVAL"); + _p_min_interval = param_find("TRIG_MIN_TIMING"); _p_distance = param_find("TRIG_DISTANCE"); _p_activation_time = param_find("TRIG_ACT_TIME"); _p_mode = param_find("TRIG_MODE"); @@ -274,6 +281,7 @@ CameraTrigger::CameraTrigger() : param_get(_p_activation_time, &_activation_time); param_get(_p_interval, &_interval); + param_get(_p_min_interval, &_min_interval); param_get(_p_distance, &_distance); param_get(_p_mode, (int32_t *)&_trigger_mode); param_get(_p_interface, (int32_t *)&_camera_interface_mode); @@ -357,32 +365,32 @@ CameraTrigger::update_intervalometer() void CameraTrigger::update_distance() { - if (_turning_on) { + if (_turning_on || !_trigger_enabled || _trigger_paused) { return; } - if (_trigger_enabled) { - vehicle_local_position_s local{}; - _lpos_sub.copy(&local); + vehicle_local_position_s local{}; + _lpos_sub.copy(&local); - if (local.xy_valid) { + if (local.xy_valid) { - // Initialize position if not done yet - matrix::Vector2f current_position(local.x, local.y); + // Initialize position if not done yet + matrix::Vector2f current_position(local.x, local.y); - if (!_valid_position) { - // First time valid position, take first shot - _last_shoot_position = current_position; - _valid_position = local.xy_valid; + if (!_valid_position) { + // First time valid position, take first shot + _last_shoot_position = current_position; + _valid_position = local.xy_valid; + + if (!_one_shot) { shoot_once(); } + } - // Check that distance threshold is exceeded - if (matrix::Vector2f(_last_shoot_position - current_position).length() >= _distance) { - shoot_once(); - _last_shoot_position = current_position; - - } + // Check that distance threshold is exceeded + if (matrix::Vector2f(_last_shoot_position - current_position).length() >= _distance) { + shoot_once(); + _last_shoot_position = current_position; } } } @@ -419,14 +427,12 @@ CameraTrigger::toggle_power() void CameraTrigger::shoot_once() { - if (!_trigger_paused) { - // schedule trigger on and off calls - hrt_call_after(&_engagecall, 0, - (hrt_callout)&CameraTrigger::engage, this); + // schedule trigger on and off calls + hrt_call_after(&_engagecall, 0, + (hrt_callout)&CameraTrigger::engage, this); - hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000), - (hrt_callout)&CameraTrigger::disengage, this); - } + hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000), + (hrt_callout)&CameraTrigger::disengage, this); } bool @@ -526,19 +532,28 @@ CameraTrigger::Run() if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) { need_ack = true; + hrt_abstime now = hrt_absolute_time(); - if (commandParamToInt(cmd.param7) == 1) { - // test shots are not logged or forwarded to GCS for geotagging - _test_shot = true; + if (now - _trigger_timestamp < _min_interval * 1000) { + // triggering too fast, abort + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + + } else { + if (commandParamToInt(cmd.param7) == 1) { + // test shots are not logged or forwarded to GCS for geotagging + _test_shot = true; + + } + + if (commandParamToInt((float)cmd.param5) == 1) { + // Schedule shot + _one_shot = true; + + } + + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } - if (commandParamToInt((float)cmd.param5) == 1) { - // Schedule shot - _one_shot = true; - } - - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; - } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) { need_ack = true; @@ -571,7 +586,7 @@ CameraTrigger::Run() /* * TRANSITIONAL SUPPORT ADDED AS OF 11th MAY 2017 (v1.6 RELEASE) - */ + */ if (cmd.param1 > 0.0f) { _distance = cmd.param1; @@ -579,6 +594,7 @@ CameraTrigger::Run() _trigger_enabled = true; _trigger_paused = false; + _valid_position = false; } else if (commandParamToInt(cmd.param1) == 0) { _trigger_paused = true; @@ -677,6 +693,9 @@ CameraTrigger::Run() } } + // order matters - one_shot has to be handled LAST + // as the other trigger handlers will back off from it + // run every loop iteration and trigger if needed if (_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD || _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) { @@ -718,8 +737,16 @@ CameraTrigger::engage(void *arg) { CameraTrigger *trig = static_cast(arg); + hrt_abstime now = hrt_absolute_time(); + + if ((trig->_trigger_timestamp > 0) && (now - trig->_trigger_timestamp < trig->_min_interval * 1000)) { + return; + } + // Trigger the camera trig->_camera_interface->trigger(true); + // set last timestamp + trig->_trigger_timestamp = now; if (trig->_test_shot) { // do not send messages or increment frame count for test shots @@ -732,7 +759,7 @@ CameraTrigger::engage(void *arg) struct camera_trigger_s trigger = {}; // Set timestamp the instant after the trigger goes off - trigger.timestamp = hrt_absolute_time(); + trigger.timestamp = trig->_trigger_timestamp; timespec tv = {}; px4_clock_gettime(CLOCK_REALTIME, &tv); diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index 35562222db..5f581ea14d 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -37,6 +37,7 @@ * * @author Mohammed Kabir * @author Andreas Bircher + * @author Lorenz Meier */ /** @@ -50,7 +51,6 @@ * @value 4 Generic PWM (IR trigger, servo) * * @reboot_required true -* * @group Camera trigger */ PARAM_DEFINE_INT32(TRIG_INTERFACE, 4); @@ -64,10 +64,26 @@ PARAM_DEFINE_INT32(TRIG_INTERFACE, 4); * @min 4.0 * @max 10000.0 * @decimal 1 + * @reboot_required true * @group Camera trigger */ PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 40.0f); +/** + * Minimum camera trigger interval + * + * This parameter sets the minimum time between two consecutive trigger events + * the specific camera setup is supporting. + * + * @unit ms + * @min 1.0 + * @max 10000.0 + * @decimal 1 + * @reboot_required true + * @group Camera trigger + */ +PARAM_DEFINE_FLOAT(TRIG_MIN_TIMING, 1.0f); + /** * Camera trigger polarity * @@ -77,6 +93,7 @@ PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 40.0f); * @value 1 Active high * @min 0 * @max 1 + * @reboot_required true * @group Camera trigger */ PARAM_DEFINE_INT32(TRIG_POLARITY, 0); @@ -90,6 +107,7 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0); * @min 0.1 * @max 3000 * @decimal 1 + * @reboot_required true * @group Camera trigger */ PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 40.0f); @@ -136,6 +154,7 @@ PARAM_DEFINE_INT32(TRIG_PINS, 56); * @min 0 * @increment 1 * @decimal 1 + * @reboot_required true * @group Camera trigger */ PARAM_DEFINE_FLOAT(TRIG_DISTANCE, 25.0f);