From 24f57b00a60a02bf210c391a3d7c4ea822656775 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Thu, 13 Apr 2017 11:39:02 +0200 Subject: [PATCH] camera_trigger : consolidate camera power control and fix camelCase --- src/drivers/camera_trigger/camera_trigger.cpp | 68 ++++++++++--------- .../interfaces/src/camera_interface.h | 27 +++----- .../interfaces/src/seagull_map2.cpp | 35 ++++++---- .../interfaces/src/seagull_map2.h | 7 +- 4 files changed, 74 insertions(+), 63 deletions(-) diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 97d6dc03d3..a1666c1c98 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -100,19 +100,19 @@ public: void control(bool on); /** - * Trigger just once + * Trigger the camera just once */ - void shootOnce(); + void shoot_once(); /** * Toggle keep camera alive functionality */ - void keepAlive(bool on); + void enable_keep_alive(bool on); /** - * Toggle camera on/off functionality + * Toggle camera power (on/off) */ - void turnOnOff(); + void toggle_power(); /** * Start the task. @@ -317,14 +317,13 @@ CameraTrigger::control(bool on) } void -CameraTrigger::keepAlive(bool on) +CameraTrigger::enable_keep_alive(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); @@ -337,25 +336,23 @@ CameraTrigger::keepAlive(bool on) } void -CameraTrigger::turnOnOff() +CameraTrigger::toggle_power() { - // schedule trigger on and off calls + // schedule power toggle 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() +CameraTrigger::shoot_once() { // 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); } @@ -368,13 +365,13 @@ CameraTrigger::start() control(true); } - // Prevent camera from sleeping, if triggering is enabled - if (_mode > 0 && _mode < 4) { - turnOnOff(); - keepAlive(true); + // Prevent camera from sleeping, if triggering is enabled and the interface supports it + if (_mode > 0 && _mode < 4 && _camera_interface->has_power_control()) { + toggle_power(); + enable_keep_alive(true); } else { - keepAlive(false); + enable_keep_alive(false); } // start to monitor at high rate for trigger enable command @@ -486,17 +483,23 @@ 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->turnOnOff(); - trig->keepAlive(true); - // Give the camera time to turn on, before starting to send trigger signals - poll_interval_usec = 5000000; - turning_on = true; + + if (trig->_camera_interface->has_power_control()) { + trig->toggle_power(); + trig->enable_keep_alive(true); + // Give the camera time to turn on, before starting to send trigger signals + poll_interval_usec = 5000000; + turning_on = true; + } } else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) { hrt_cancel(&(trig->_engagecall)); hrt_cancel(&(trig->_disengagecall)); - trig->keepAlive(false); - trig->turnOnOff(); + + if (trig->_camera_interface->has_power_control()) { + trig->enable_keep_alive(false); + trig->toggle_power(); + } } trig->_trigger_enabled = cmd.param1 > 0.0f; @@ -513,12 +516,12 @@ CameraTrigger::cycle_trampoline(void *arg) // First time valid position, take first shot trig->_last_shoot_position = current_position; trig->_valid_position = pos.xy_valid; - trig->shootOnce(); + trig->shoot_once(); } // 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) { - trig->shootOnce(); + trig->shoot_once(); trig->_last_shoot_position = current_position; } } @@ -563,7 +566,7 @@ CameraTrigger::engange_turn_on_off(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - trig->_camera_interface->turn_on_off(true); + trig->_camera_interface->send_toggle_power(true); } void @@ -571,7 +574,7 @@ CameraTrigger::disengage_turn_on_off(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - trig->_camera_interface->turn_on_off(false); + trig->_camera_interface->send_toggle_power(false); } void @@ -579,7 +582,7 @@ CameraTrigger::keep_alive_up(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - trig->_camera_interface->keep_alive(true); + trig->_camera_interface->send_keep_alive(true); } void @@ -587,7 +590,7 @@ CameraTrigger::keep_alive_down(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - trig->_camera_interface->keep_alive(false); + trig->_camera_interface->send_keep_alive(false); } void @@ -603,7 +606,7 @@ CameraTrigger::status() static int usage() { - PX4_INFO("usage: camera_trigger {start|stop|enable|disable|status|test}\n"); + PX4_INFO("usage: camera_trigger {start|stop|enable|disable|status|test|test_power}\n"); return 1; } @@ -650,6 +653,9 @@ int camera_trigger_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "test")) { camera_trigger::g_camera_trigger->test(); + } else if (!strcmp(argv[1], "test_power")) { + camera_trigger::g_camera_trigger->toggle_power(); + } else { return usage(); } diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h index 957a343812..dbb9def60f 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h @@ -25,21 +25,21 @@ public: /** * trigger the camera - * @param trigger: + * @param enable */ virtual void trigger(bool enable) {}; /** - * turn on/off the camera - * @param enable: + * send command to turn the camera on/off + * @param enable */ - virtual void turn_on_off(bool enable) {}; + virtual void send_toggle_power(bool enable) {}; /** - * prevent the camera from sleeping - * @param keep alive signal: + * send command to prevent the camera from sleeping + * @param enable */ - virtual void keep_alive(bool signal_on) {}; + virtual void send_keep_alive(bool enable) {}; /** * Display info. @@ -47,16 +47,11 @@ public: virtual void info() {}; /** - * Power on the camera - * @return 0 on success, <0 on error + * Checks if the interface has support for + * camera power control + * @return true if power control is supported */ - virtual int powerOn() { return -1; } - - /** - * Power off the camera - * @return 0 on success, <0 on error - */ - virtual int powerOff() { return -1; } + virtual bool has_power_control() { return false; } protected: diff --git a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp index 35c03deb6e..849b92bac0 100644 --- a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp +++ b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp @@ -9,11 +9,11 @@ // PWM levels of the interface to Seagull MAP 2 converter to // Multiport (http://www.seagulluav.com/manuals/Seagull_MAP2-Manual.pdf) #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_1_CAMERA_ON 1100 +#define PWM_1_CAMERA_AUTOFOCUS_SHOOT 1300 +#define PWM_1_CAMERA_INSTANT_SHOOT 1700 +#define PWM_1_CAMERA_OFF 1900 #define PWM_2_CAMERA_KEEP_ALIVE 1700 #define PWM_2_CAMERA_ON_OFF 1900 @@ -35,17 +35,21 @@ void CameraInterfaceSeagull::setup() { for (unsigned i = 0; i < arraySize(_pins); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { + + // Initialize the interface uint8_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]); up_pwm_trigger_init(pin_bitmask); - up_pwm_trigger_set(_pins[i + 1], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000)); - up_pwm_trigger_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000)); + + // Set both interface pins to disarmed + up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_DISARMED); + up_pwm_trigger_set(_pins[i], PWM_CAMERA_DISARMED); // We only support 2 consecutive pins while using the Seagull MAP2 return; } } - PX4_ERROR("Bad pin configuration - Seagull MAP2 requires 2 consecutive pins for control."); + PX4_ERR("Bad pin configuration - Seagull MAP2 requires 2 consecutive pins for control."); } void CameraInterfaceSeagull::trigger(bool enable) @@ -57,15 +61,15 @@ void CameraInterfaceSeagull::trigger(bool enable) for (unsigned i = 0; i < arraySize(_pins); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { - // Set all valid pins to shoot or neutral levels - up_pwm_trigger_set(_pins[i + 1], math::constrain(enable ? PWM_CAMERA_INSTANT_SHOOT : PWM_CAMERA_NEUTRAL, 1000, 2000)); + // Set channel 1 to shoot or neutral levels + up_pwm_trigger_set(_pins[i + 1], math::constrain(enable ? PWM_1_CAMERA_INSTANT_SHOOT : PWM_CAMERA_NEUTRAL, 1000, 2000)); } } } -void CameraInterfaceSeagull::keep_alive(bool signal_on) +void CameraInterfaceSeagull::send_keep_alive(bool enable) { - // This should alternate between signal_on and !signal_on to keep the camera alive + // This should alternate between enable and !enable to keep the camera alive if (!_camera_is_on) { return; @@ -74,18 +78,21 @@ void CameraInterfaceSeagull::keep_alive(bool signal_on) for (unsigned i = 0; i < arraySize(_pins); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { // Set channel 2 pin to keep_alive or netural signal - up_pwm_trigger_set(_pins[i], math::constrain(signal_on ? PWM_2_CAMERA_KEEP_ALIVE : PWM_CAMERA_NEUTRAL, 1000, 2000)); + up_pwm_trigger_set(_pins[i], math::constrain(enable ? PWM_2_CAMERA_KEEP_ALIVE : PWM_CAMERA_NEUTRAL, 1000, 2000)); } } } -void CameraInterfaceSeagull::turn_on_off(bool enable) +void CameraInterfaceSeagull::send_toggle_power(bool enable) { + // This should alternate between enable and !enable to toggle camera power + for (unsigned i = 0; i < arraySize(_pins); i = i + 2) { if (_pins[i] >= 0 && _pins[i + 1] >= 0) { - // For now, set channel one to neutral upon startup. + // Set channel 1 to neutral up_pwm_trigger_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); + // Set channel 2 to on_off or neutral signal up_pwm_trigger_set(_pins[i], math::constrain(enable ? PWM_2_CAMERA_ON_OFF : PWM_CAMERA_NEUTRAL, 1000, 2000)); } } diff --git a/src/drivers/camera_trigger/interfaces/src/seagull_map2.h b/src/drivers/camera_trigger/interfaces/src/seagull_map2.h index 49b809aa18..3b97f7d617 100644 --- a/src/drivers/camera_trigger/interfaces/src/seagull_map2.h +++ b/src/drivers/camera_trigger/interfaces/src/seagull_map2.h @@ -19,12 +19,15 @@ public: virtual ~CameraInterfaceSeagull(); void trigger(bool enable); - void keep_alive(bool signal_on); - void turn_on_off(bool enable); + void send_keep_alive(bool enable); + + void send_toggle_power(bool enable); void info(); + bool has_power_control() { return true; } + private: void setup();