From 70cd06bc84d8157af1c52aea84cb87fad5662089 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Tue, 2 Aug 2016 13:59:19 +0200 Subject: [PATCH] initial version camera turn on / off --- src/drivers/camera_trigger/camera_trigger.cpp | 68 +++++++++++- .../camera_trigger/camera_trigger_params.c | 6 +- .../interfaces/src/camera_interface.h | 6 + .../camera_trigger/interfaces/src/pwm.cpp | 103 +++++++++++------- .../camera_trigger/interfaces/src/pwm.h | 11 +- 5 files changed, 145 insertions(+), 49 deletions(-) diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 37299cb312..2cac457366 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -108,6 +108,11 @@ public: */ void keepAlive(bool on); + /** + * Toggle camera on/off functionality + */ + void turnOnOff(); + /** * Start the task. */ @@ -132,6 +137,8 @@ private: struct hrt_call _engagecall; struct hrt_call _disengagecall; + struct hrt_call _engage_turn_on_off_call; + struct hrt_call _disengage_turn_on_off_call; struct hrt_call _keepalivecall_up; struct hrt_call _keepalivecall_down; @@ -175,6 +182,14 @@ private: * Resets trigger */ static void disengage(void *arg); + /** + * Fires on/off + */ + static void engange_turn_on_off(void *arg); + /** + * Resets on/off + */ + static void disengage_turn_on_off(void *arg); /** * Fires trigger */ @@ -197,6 +212,10 @@ CameraTrigger *g_camera_trigger; CameraTrigger::CameraTrigger() : _engagecall {}, _disengagecall {}, + _engage_turn_on_off_call {}, + _disengage_turn_on_off_call {}, + _keepalivecall_up {}, + _keepalivecall_down {}, _gpio_fd(-1), _mode(0), _activation_time(0.5f /* ms */), @@ -306,6 +325,18 @@ CameraTrigger::keepAlive(bool on) } +void +CameraTrigger::turnOnOff() +{ + // schedule trigger on and off calls + hrt_call_after(&_engage_turn_on_off_call, 0, + (hrt_callout)&CameraTrigger::engange_turn_on_off, this); + + // schedule trigger on and off calls + hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000), + (hrt_callout)&CameraTrigger::disengage_turn_on_off, this); +} + void CameraTrigger::shootOnce() { @@ -327,7 +358,8 @@ CameraTrigger::start() } // Prevent camera from sleeping, if triggering is enabled - if (_mode > 0) { + if (_mode > 0 && _mode < 4) { + turnOnOff(); keepAlive(true); } else { @@ -345,6 +377,8 @@ CameraTrigger::stop() work_cancel(LPWORK, &_work); hrt_cancel(&_engagecall); hrt_cancel(&_disengagecall); + hrt_cancel(&_engage_turn_on_off_call); + hrt_cancel(&_disengage_turn_on_off_call); hrt_cancel(&_keepalivecall_up); hrt_cancel(&_keepalivecall_down); @@ -358,6 +392,7 @@ CameraTrigger::test() { struct vehicle_command_s cmd = {}; cmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; + cmd.param5 = 1.0f; orb_advert_t pub; @@ -429,6 +464,8 @@ CameraTrigger::cycle_trampoline(void *arg) if (pos.xy_valid) { + bool turning_on = false; + if (updated && trig->_mode == 4) { // Check update from command @@ -439,10 +476,16 @@ CameraTrigger::cycle_trampoline(void *arg) // Set trigger to disabled if the set distance is not positive if (cmd.param1 > 0.0f && !trig->_trigger_enabled) { - trig->_camera_interface->powerOn(); + trig->turnOnOff(); + trig->keepAlive(true); + poll_interval_usec = 2000000; + turning_on = true; } else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) { - trig->_camera_interface->powerOff(); + hrt_cancel(&(trig->_engagecall)); + hrt_cancel(&(trig->_disengagecall)); + trig->keepAlive(false); + trig->turnOnOff(); } trig->_trigger_enabled = cmd.param1 > 0.0f; @@ -450,7 +493,7 @@ CameraTrigger::cycle_trampoline(void *arg) } } - if (trig->_trigger_enabled || trig->_mode < 4) { + if ((trig->_trigger_enabled || trig->_mode < 4) && !turning_on) { // Initialize position if not done yet math::Vector<2> current_position(pos.x, pos.y); @@ -504,6 +547,23 @@ CameraTrigger::disengage(void *arg) trig->_camera_interface->trigger(false); } +void +CameraTrigger::engange_turn_on_off(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + trig->_camera_interface->turn_on_off(true); +} + +void +CameraTrigger::disengage_turn_on_off(void *arg) +{ + CameraTrigger *trig = reinterpret_cast(arg); + + trig->_camera_interface->turn_on_off(false); +} + void CameraTrigger::keep_alive_up(void *arg) { diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index 09b3cbabc5..421beb8ae9 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -67,7 +67,7 @@ PARAM_DEFINE_INT32(TRIG_INTERFACE, 2); * @decimal 1 * @group Camera trigger */ -PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 40.0f); +PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 3000.0f); /** * Camera trigger polarity @@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0); * @decimal 1 * @group Camera trigger */ -PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 0.5f); +PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 500.0f); /** * Camera trigger mode @@ -123,7 +123,7 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0); * @reboot_required true * @group Camera trigger */ -PARAM_DEFINE_INT32(TRIG_PINS, 6); +PARAM_DEFINE_INT32(TRIG_PINS, 56); /** * Camera trigger distance diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h index 6ce9e5011d..89e23996b6 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h @@ -24,6 +24,12 @@ public: */ virtual void trigger(bool enable) {}; + /** + * turn on/off the camera + * @param enable: + */ + virtual void turn_on_off(bool enable) {}; + /** * prevent the camera from sleeping * @param keep alive signal: diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp index 1ce5d91e94..984968af47 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp +++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp @@ -2,6 +2,7 @@ #include #include +#include #include "drivers/drv_pwm_output.h" #include "pwm.h" @@ -18,6 +19,7 @@ CameraInterfacePWM::CameraInterfacePWM(): CameraInterface(), + _pwm_pub {nullptr, nullptr}, _camera_is_on(false) { _p_pin = param_find("TRIG_PINS"); @@ -34,17 +36,24 @@ CameraInterfacePWM::CameraInterfacePWM(): int single_pin; while ((single_pin = pin_list % 10)) { - _pins[i] = single_pin - 1; if (_pins[i] < 0) { _pins[i] = -1; } + // Wingtra safety check, pins can only be on 5 or 6!!! + if (_pins[i] != 4 && _pins[i] != 5) { + _pins[i] = -1; + } + pin_list /= 10; i++; } + _orb_id[0] = 0; + _orb_id[1] = 1; + setup(); } @@ -63,6 +72,13 @@ void CameraInterfacePWM::setup() up_pwm_servo_init(pin_bitmask); up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000)); up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000)); + + trigger_pwm_s pwm; + pwm.timestamp = hrt_absolute_time(); + pwm.pwm = PWM_CAMERA_DISARMED; + orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT); + pwm.pwm = PWM_CAMERA_DISARMED; + orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT); } } } @@ -72,17 +88,21 @@ void CameraInterfacePWM::trigger(bool enable) // This only starts working upon prearming if (!_camera_is_on) { - // (TODO: powerOn does not work yet) - // Turn camera on and give time to start up - // powerOn(); return; } + + trigger_pwm_s pwm; + pwm.timestamp = hrt_absolute_time(); + if (enable) { // Set all valid pins to shoot level for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000)); + + pwm.pwm = PWM_CAMERA_INSTANT_SHOOT; + orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT); } } @@ -91,6 +111,9 @@ void CameraInterfacePWM::trigger(bool enable) for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); + + pwm.pwm = PWM_CAMERA_NEUTRAL; + orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT); } } } @@ -101,16 +124,20 @@ void CameraInterfacePWM::keep_alive(bool signal_on) // This should alternate between signal_on and !signal_on to keep the camera alive if (!_camera_is_on) { - // (TODO: powerOn does not work yet) - // Turn camera on and give time to start up - powerOn(); + return; } + trigger_pwm_s pwm; + pwm.timestamp = hrt_absolute_time(); + if (signal_on) { // Set channel 2 pin to keep_alive signal for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_KEEP_ALIVE, 1000, 2000)); + + pwm.pwm = PWM_2_CAMERA_KEEP_ALIVE; + orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT); } } @@ -119,50 +146,50 @@ void CameraInterfacePWM::keep_alive(bool signal_on) for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); + + pwm.pwm = PWM_CAMERA_NEUTRAL; + orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT); } } } } -int CameraInterfacePWM::powerOn() +void CameraInterfacePWM::turn_on_off(bool enable) { - // This only starts working upon prearming - // Set all valid pins to turn on level - // for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { - // if (_pins[i] >= 0 && _pins[i + 1] >= 0) { - // up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_ON, 1000, 2000)); - // up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000)); - // } - // } - - // For now, set channel one on neutral upon startup. - for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { - if (_pins[i] >= 0 && _pins[i + 1] >= 0) { - up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); - } - } + trigger_pwm_s pwm; + pwm.timestamp = hrt_absolute_time(); - _camera_is_on = true; + if (enable) { + // For now, set channel one on neutral upon startup. + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { + if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); + up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000)); - return 0; -} + pwm.pwm = PWM_CAMERA_NEUTRAL; + orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT); + pwm.pwm = PWM_2_CAMERA_ON_OFF; + orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT); + } + } -int CameraInterfacePWM::powerOff() -{ - // This only starts working upon prearming + } else { + // For now, set channel one on neutral upon startup. + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { + if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); + up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); - // Set all valid pins to turn off level - for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { - if (_pins[i] >= 0 && _pins[i + 1] >= 0) { - up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_OFF, 1000, 2000)); - up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000)); + pwm.pwm = PWM_CAMERA_NEUTRAL; + orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT); + pwm.pwm = PWM_CAMERA_NEUTRAL; + orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT); + } } - } - _camera_is_on = false; - - return 0; + _camera_is_on = !_camera_is_on; + } } void CameraInterfacePWM::info() diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h index 33392d6047..62ed1838cf 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.h +++ b/src/drivers/camera_trigger/interfaces/src/pwm.h @@ -6,6 +6,7 @@ */ #pragma once +#include #include #include @@ -20,8 +21,7 @@ public: void trigger(bool enable); void keep_alive(bool signal_on); - int powerOn(); - int powerOff(); + void turn_on_off(bool enable); void info(); @@ -29,7 +29,10 @@ public: private: void setup(); - param_t _p_pin; - bool _camera_is_on; + param_t _p_pin; + orb_advert_t _pwm_pub[2]; + int _orb_id[2]; + + bool _camera_is_on; };