From adffb859627779712b62da118f0a402e6c2f16e7 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Fri, 10 Jun 2016 14:54:12 +0200 Subject: [PATCH] adding arming check before setting PWM --- .../camera_trigger/interfaces/src/pwm.cpp | 67 +++++++++++++------ .../camera_trigger/interfaces/src/pwm.h | 24 ++++--- 2 files changed, 62 insertions(+), 29 deletions(-) diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp index 0aae31a2e8..c5df0e489e 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp +++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp @@ -1,7 +1,12 @@ +#include +#include +#include + #include "pwm.h" CameraInterfacePWM::CameraInterfacePWM(): CameraInterface(), + _vehicle_status_sub(-1), _camera_is_on(false) { _p_pin = param_find("TRIG_PINS"); @@ -21,7 +26,7 @@ CameraInterfacePWM::CameraInterfacePWM(): _pins[i] = single_pin - 1; - if (_pins[i] < 0 || _pins[i] >= static_cast(sizeof(_gpios) / sizeof(_gpios[0]))) { + if (_pins[i] < 0) { _pins[i] = -1; } @@ -44,8 +49,9 @@ void CameraInterfacePWM::setup() } for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - if (_pin[i] >= 0) { - int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000)); + if (_pins[i] >= 0) { + px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, + 2000)); // TODO(birchera): use return value to make sure everything is fine } } } @@ -53,10 +59,17 @@ void CameraInterfacePWM::setup() void CameraInterfacePWM::trigger(bool enable) { // Check if armed, otherwise don't send high PWM values - if (_disarmed) { + if (_vehicle_status_sub < 0) { + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + } + + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + + if (_vehicle_status.arming_state != _vehicle_status.ARMING_STATE_ARMED + || !_vehicle_status.no_pwm) { for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - if (_pin[i] >= 0) { - int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000)); + if (_pins[i] >= 0) { + px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000)); } } @@ -70,16 +83,16 @@ void CameraInterfacePWM::trigger(bool enable) if (trig) { // Set all valid pins to shoot level for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - if (_pin[i] >= 0) { - int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000)); + if (_pins[i] >= 0) { + px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), 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 (_pin[i] >= 0) { - int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); + if (_pins[i] >= 0) { + px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000)); } } } @@ -89,18 +102,25 @@ void CameraInterfacePWM::trigger(bool enable) int CameraInterfacePWM::powerOn() { // Check if armed, otherwise don't send high PWM values - if (_disarmed) { + if (_vehicle_status_sub < 0) { + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + } + + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + + if (_vehicle_status.arming_state != _vehicle_status.ARMING_STATE_ARMED + || !_vehicle_status.no_pwm) { for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - if (_pin[i] >= 0) { - int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000)); + if (_pins[i] >= 0) { + px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000)); } } } else { // Set all valid pins to turn on level for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - if (_pin[i] >= 0) { - int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_ON, 1000, 2000)); + if (_pins[i] >= 0) { + px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_ON, 1000, 2000)); } } @@ -113,18 +133,25 @@ int CameraInterfacePWM::powerOn() int CameraInterfacePWM::powerOff() { // Check if armed, otherwise don't send high PWM values - if (_disarmed) { + if (_vehicle_status_sub < 0) { + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + } + + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + + if (_vehicle_status.arming_state != _vehicle_status.ARMING_STATE_ARMED + || !_vehicle_status.no_pwm) { for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - if (_pin[i] >= 0) { - int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000)); + if (_pins[i] >= 0) { + px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000)); } } } else { // Set all valid pins to turn off level for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - if (_pin[i] >= 0) { - int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_OFF, 1000, 2000)); + if (_pins[i] >= 0) { + px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_OFF, 1000, 2000)); } } diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h index 4cd21c29b9..5dceba0a35 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.h +++ b/src/drivers/camera_trigger/interfaces/src/pwm.h @@ -6,20 +6,23 @@ */ #pragma once +#include + +#include #include "camera_interface.h" // TODO(birchera): check if this is the right device and addresses -#define PWM_DEVICE_PATH /dev/pwm_output0 -#define PWM_CAMERA_BASE 0x2a00 -#define PWM_CAMERA_SET(_pin) _PX4_IOC(_PWM_CAMERA_BASE, 0x30 + _pin) +#define PWM_DEVICE_PATH "/dev/pwm_output0" +#define PWM_CAMERA_BASE 0x2a00 +#define PWM_CAMERA_SET(_pin) _PX4_IOC(PWM_CAMERA_BASE, 0x30 + _pin) // 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_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_CAMERA_DISARMED 90 // TODO(birchera): check here value +#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 class CameraInterfacePWM : public CameraInterface @@ -38,6 +41,9 @@ public: int _pins[6]; private: + int _vehicle_status_sub; + struct vehicle_status_s _vehicle_status; + param_t _p_pin; const char *_pwm_dev; int _pwm_fd;