From 9974b6f7475b2c390c2de54c379a4784ae112c6b Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Thu, 7 Jul 2016 11:37:45 +0200 Subject: [PATCH] Camera trigger update (#4998) * updating the camera driver, correct init and keepAlive function * removing debug output --- src/drivers/camera_trigger/camera_trigger.cpp | 72 ++++++++++++++++- .../camera_trigger/camera_trigger_params.c | 21 ++--- .../interfaces/src/camera_interface.h | 5 ++ .../camera_trigger/interfaces/src/pwm.cpp | 77 +++++++++++++++---- .../camera_trigger/interfaces/src/pwm.h | 1 + 5 files changed, 143 insertions(+), 33 deletions(-) diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 521aa796e8..ecd7282a28 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -103,6 +103,11 @@ public: */ void shootOnce(); + /** + * Toggle keep camera alive functionality + */ + void keepAlive(bool on); + /** * Start the task. */ @@ -127,6 +132,9 @@ private: struct hrt_call _engagecall; struct hrt_call _disengagecall; + struct hrt_call _keepalivecall_up; + struct hrt_call _keepalivecall_down; + static struct work_s _work; int _gpio_fd; @@ -167,6 +175,14 @@ private: * Resets trigger */ static void disengage(void *arg); + /** + * Fires trigger + */ + static void keep_alive_up(void *arg); + /** + * Resets trigger + */ + static void keep_alive_down(void *arg); }; @@ -232,6 +248,7 @@ CameraTrigger::CameraTrigger() : } struct camera_trigger_s report = {}; + _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &report); } @@ -269,6 +286,26 @@ CameraTrigger::control(bool on) _trigger_enabled = on; } +void +CameraTrigger::keepAlive(bool on) +{ + if (on) { + // schedule keep-alive up and down calls + hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000), + (hrt_callout)&CameraTrigger::keep_alive_up, this); + + // schedule keep-alive up and down calls + hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000), + (hrt_callout)&CameraTrigger::keep_alive_down, this); + + } else { + // cancel all calls + hrt_cancel(&_keepalivecall_up); + hrt_cancel(&_keepalivecall_down); + } + +} + void CameraTrigger::shootOnce() { @@ -289,6 +326,14 @@ CameraTrigger::start() control(true); } + // Prevent camera from sleeping, if triggering is enabled + if (_mode > 0) { + keepAlive(true); + + } else { + keepAlive(false); + } + // start to monitor at high rate for trigger enable command work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1)); @@ -300,6 +345,8 @@ CameraTrigger::stop() work_cancel(LPWORK, &_work); hrt_cancel(&_engagecall); hrt_cancel(&_disengagecall); + hrt_cancel(&_keepalivecall_up); + hrt_cancel(&_keepalivecall_down); if (camera_trigger::g_camera_trigger != nullptr) { delete(camera_trigger::g_camera_trigger); @@ -390,7 +437,14 @@ CameraTrigger::cycle_trampoline(void *arg) if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) { - // Set trigger to false if the set distance is not positive + // Set trigger to disabled if the set distance is not positive + if (cmd.param1 > 0.0f && !trig->_trigger_enabled) { + trig->_camera_interface->powerOn(); + + } else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) { + trig->_camera_interface->powerOff(); + } + trig->_trigger_enabled = cmd.param1 > 0.0f; trig->_distance = cmd.param1; } @@ -450,6 +504,22 @@ CameraTrigger::disengage(void *arg) trig->_camera_interface->trigger(false); } +void +CameraTrigger::keep_alive_up(void *arg) +{ + CameraTrigger *trig = reinterpret_cast(arg); + + trig->_camera_interface->keep_alive(true); +} + +void +CameraTrigger::keep_alive_down(void *arg) +{ + CameraTrigger *trig = reinterpret_cast(arg); + + trig->_camera_interface->keep_alive(false); +} + void CameraTrigger::info() { diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index 7fcc3817f7..09b3cbabc5 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -36,25 +36,12 @@ * Camera trigger parameters * * @author Mohammed Kabir + * @author Andreas Bircher */ #include #include - /** - * Camera trigger Interface - * - * Selects the trigger interface - * - * @value 1 GPIO - * @value 2 Seagull MAP2 (PWM) - * - * @reboot_required true - * - * @group Camera trigger - */ -PARAM_DEFINE_INT32(TRIG_INTERFACE, 2); - /** * Camera trigger Interface * @@ -102,7 +89,7 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0); * * @unit ms * @min 0.1 - * @max 3 + * @max 3000 * @decimal 1 * @group Camera trigger */ @@ -126,7 +113,9 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0); /** * Camera trigger pin * - * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6) + * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6 on px4fmu-v2 and the rail + * pins on px4fmu-v4). The PWM interface takes two pins per camera, while relay + * triggers on every pin individually. Example: Value 34 would trigger on pins 3 and 4. * * @min 1 * @max 123456 diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h index e6b848884c..6ce9e5011d 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h @@ -24,6 +24,11 @@ public: */ virtual void trigger(bool enable) {}; + /** + * prevent the camera from sleeping + * @param keep alive signal: + */ + virtual void keep_alive(bool signal_on) {}; /** * Display info. diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp index 5dafcc7775..1ce5d91e94 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp +++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp @@ -7,12 +7,14 @@ // PWM levels of the interface to seagull MAP converter to // Multiport (http://www.seagulluav.com/manuals/Seagull_MAP2-Manual.pdf) -#define PWM_CAMERA_DISARMED 90 // TODO(birchera): check here value +#define PWM_CAMERA_DISARMED 900 #define PWM_CAMERA_ON 1100 #define PWM_CAMERA_AUTOFOCUS_SHOOT 1300 #define PWM_CAMERA_NEUTRAL 1500 #define PWM_CAMERA_INSTANT_SHOOT 1700 #define PWM_CAMERA_OFF 1900 +#define PWM_2_CAMERA_KEEP_ALIVE 1700 +#define PWM_2_CAMERA_ON_OFF 1900 CameraInterfacePWM::CameraInterfacePWM(): CameraInterface(), @@ -48,13 +50,18 @@ CameraInterfacePWM::CameraInterfacePWM(): CameraInterfacePWM::~CameraInterfacePWM() { + // Deinitialise pwm channels + // Can currently not be used, because up_pwm_servo_deinit() will deinitialise all pwm channels + // up_pwm_servo_deinit(); } void CameraInterfacePWM::setup() { - for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - if (_pins[i] >= 0) { - // TODO(birchera): use return value to make sure everything is fine + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) { + if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + uint8_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]); + 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)); } } @@ -65,23 +72,52 @@ 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(); + // powerOn(); return; } if (enable) { // Set all valid pins to shoot level - for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - if (_pins[i] >= 0) { - up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000)); + 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)); } } } else { // Set all valid pins back to neutral level - for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - if (_pins[i] >= 0) { + 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)); + } + } + } +} + +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(); + } + + 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)); + } + } + + } else { + // Set channel 2 pin to neutral 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_CAMERA_NEUTRAL, 1000, 2000)); } } @@ -93,9 +129,17 @@ int CameraInterfacePWM::powerOn() // 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++) { - if (_pins[i] >= 0) { - up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_ON, 1000, 2000)); + // 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)); } } @@ -109,9 +153,10 @@ int CameraInterfacePWM::powerOff() // This only starts working upon prearming // Set all valid pins to turn off level - for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - if (_pins[i] >= 0) { - up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_OFF, 1000, 2000)); + 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)); } } diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h index 7310be8b9e..33392d6047 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.h +++ b/src/drivers/camera_trigger/interfaces/src/pwm.h @@ -18,6 +18,7 @@ public: virtual ~CameraInterfacePWM(); void trigger(bool enable); + void keep_alive(bool signal_on); int powerOn(); int powerOff();