fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 05:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

adding arming check before setting PWM

Browse Source
This commit is contained in:
Andreas Bircher
2016-06-17 22:39:02 +02:00
committed by Lorenz Meier
parent 4c5f32ab16
commit adffb85962
2 changed files with 62 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/camera_trigger/interfaces/src/pwm.cpp
+47 -20
View File
@@ -1,7 +1,12 @@
#include <px4.h>
#include <sys/ioctl.h>
#include <lib/helpfuncs/helpfuncs.h>
#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<int>(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));
}
}
src/drivers/camera_trigger/interfaces/src/pwm.h
+15 -9
View File
@@ -6,20 +6,23 @@
*/
#pragma once
#include <systemlib/param/param.h>
#include <uORB/topics/vehicle_status.h>
#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;