From 8959954d37980f557e046e87dccc718fe22a6db0 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Tue, 16 Feb 2016 11:21:12 +0100 Subject: [PATCH] adding third camera triggering mode to trigger based on covered horizontal distance. bench-tested. --- src/drivers/camera_trigger/camera_trigger.cpp | 95 ++++++++++++++++++- .../camera_trigger/camera_trigger_params.c | 13 ++- 2 files changed, 104 insertions(+), 4 deletions(-) diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index fe25657816..fb3f42ba3c 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -54,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -82,6 +84,11 @@ public: */ void control(bool on); + /** + * Trigger just once + */ + void shootOnce(); + /** * Start the task. */ @@ -111,10 +118,15 @@ private: int _mode; float _activation_time; float _interval; + float _distance; uint32_t _trigger_seq; bool _trigger_enabled; + math::Vector<2> _last_shoot_position; + float _last_shoot_time; + bool _valid_position; int _vcommand_sub; + int _vlposition_sub; orb_advert_t _trigger_pub; @@ -122,6 +134,7 @@ private: param_t _p_mode; param_t _p_activation_time; param_t _p_interval; + param_t _p_distance; param_t _p_pin; static constexpr uint32_t _gpios[6] = { @@ -137,6 +150,10 @@ private: * Vehicle command handler */ static void cycle_trampoline(void *arg); + /** + * Distance monitor + */ + static void distance_trampoline(void *arg); /** * Fires trigger */ @@ -168,9 +185,14 @@ CameraTrigger::CameraTrigger() : _mode(0), _activation_time(0.5f /* ms */), _interval(100.0f /* ms */), + _distance(25.0f /* m */), _trigger_seq(0), _trigger_enabled(false), + _last_shoot_position(0.0f, 0.0f), + _last_shoot_time(0.0f), + _valid_position(false), _vcommand_sub(-1), + _vlposition_sub(-1), _trigger_pub(nullptr) { memset(&_work, 0, sizeof(_work)); @@ -178,6 +200,7 @@ CameraTrigger::CameraTrigger() : // Parameters _p_polarity = param_find("TRIG_POLARITY"); _p_interval = param_find("TRIG_INTERVAL"); + _p_distance = param_find("TRIG_DISTANCE"); _p_activation_time = param_find("TRIG_ACT_TIME"); _p_mode = param_find("TRIG_MODE"); _p_pin = param_find("TRIG_PINS"); @@ -185,6 +208,7 @@ CameraTrigger::CameraTrigger() : param_get(_p_polarity, &_polarity); param_get(_p_activation_time, &_activation_time); param_get(_p_interval, &_interval); + param_get(_p_distance, &_distance); param_get(_p_mode, &_mode); int pin_list; param_get(_p_pin, &pin_list); @@ -247,6 +271,18 @@ CameraTrigger::control(bool on) _trigger_enabled = on; } +void +CameraTrigger::shootOnce() +{ + // 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(&_disengagecall, 0 + (_activation_time * 1000), + (hrt_callout)&CameraTrigger::disengage, this); +} + void CameraTrigger::start() { @@ -257,12 +293,18 @@ CameraTrigger::start() } // enable immediate if configured that way - if (_mode > 1) { + if (_mode == 2) { control(true); } - // start to monitor at high rate for trigger enable command - work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1)); + // if in distance mode, start monitoring the position + if (_mode == 3) { + work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::distance_trampoline, this, USEC2TICK(1)); + + } else { + // start to monitor at high rate for trigger enable command + work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1)); + } } void @@ -330,6 +372,51 @@ CameraTrigger::cycle_trampoline(void *arg) camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec)); } +void +CameraTrigger::distance_trampoline(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + int poll_interval_usec = 10000; + + // Set trigger based on covered distance + if (trig->_vlposition_sub < 0) { + trig->_vlposition_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + } + + struct vehicle_local_position_s pos; + + orb_copy(ORB_ID(vehicle_local_position), trig->_vlposition_sub, &pos); + + if (pos.xy_valid) { + + // Initialize position if not done yet + math::Vector<2> current_position(pos.x, pos.y); + + if (!trig->_valid_position) { + trig->_last_shoot_position = current_position; + trig->_valid_position = pos.xy_valid; + } + + float time_now = static_cast(hrt_absolute_time()) / 1.0e6f; + + // Check that distance threshold is exceeded and the time between last shot is large enough + if ((trig->_last_shoot_position - current_position).length() >= trig->_distance && + time_now > trig->_last_shoot_time + trig->_interval / 1000.0f) { + trig->shootOnce(); + trig->_last_shoot_position = current_position; + trig->_last_shoot_time = time_now; + } + + } else { + poll_interval_usec = 100000; + } + + work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::distance_trampoline, + camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec)); +} + void CameraTrigger::engage(void *arg) { @@ -374,7 +461,9 @@ CameraTrigger::info() warnx("state : %s", _trigger_enabled ? "enabled" : "disabled"); warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2], _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); + warnx("mode : %i", _mode); warnx("interval : %.2f", (double)_interval); + warnx("distance : %.2f", (double)_distance); } static void usage() diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index cbdcaf7020..6a9d40c1ee 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -82,7 +82,7 @@ PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 0.5f); * @reboot_required true * * @min 0 - * @max 2 + * @max 3 * @group Camera trigger */ PARAM_DEFINE_INT32(TRIG_MODE, 0); @@ -97,3 +97,14 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0); * @group Camera trigger */ PARAM_DEFINE_INT32(TRIG_PINS, 12); + +/** + * Camera trigger distance + * + * Sets the distance at which to trigger the camera. TRIG_INTERVAL sets an upper bound on the triggering frequency. + * + * @unit meters + * @min 0 + * @group Camera trigger + */ +PARAM_DEFINE_FLOAT(TRIG_DISTANCE, 25.0f);