fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 01:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixing the driver interface

Browse Source 
Conflicts:
	PX4/src/drivers/camera_trigger/interfaces/src/pwm.cpp
	PX4/src/drivers/camera_trigger/interfaces/src/pwm.h
This commit is contained in:
Andreas Bircher
2016-06-17 22:39:02 +02:00
committed by Lorenz Meier
parent f038b16734
commit e951a356fe
3 changed files with 42 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rcS
+2 -2
View File
@@ -537,7 +537,7 @@ then
# Get FMU driver out of the way # Get FMU driver out of the way
set MIXER_AUX none set MIXER_AUX none
set AUX_MODE none set AUX_MODE none
camera_trigger start camera_trigger start --pwm
fi fi
fi fi
@@ -606,7 +606,7 @@ then
# Try to get an USB console # Try to get an USB console
nshterm /dev/ttyACM0 & nshterm /dev/ttyACM0 &
else else
mavlink start -r 800000 -d /dev/ttyACM0 -m config -x mavlink start -r 80000 -d /dev/ttyACM0 -m config -x
fi fi
# #
src/drivers/camera_trigger/interfaces/src/pwm.cpp
+40 -84
View File
@@ -4,9 +4,17 @@
#include "pwm.h" #include "pwm.h"
// 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
CameraInterfacePWM::CameraInterfacePWM(): CameraInterfacePWM::CameraInterfacePWM():
CameraInterface(), CameraInterface(),
_vehicle_status_sub(-1),
_camera_is_on(false) _camera_is_on(false)
{ {
_p_pin = param_find("TRIG_PINS"); _p_pin = param_find("TRIG_PINS");
@@ -42,59 +50,37 @@ CameraInterfacePWM::~CameraInterfacePWM()
void CameraInterfacePWM::setup() void CameraInterfacePWM::setup()
{ {
_pwm_dev = PWM_DEVICE_PATH; // Used for direct pwm output without mixer
_pwm_fd = open(_pwm_dev, 0); // open pwm device
if (_pwm_fd < 0) {
err(1, "can't open %s", _pwm_dev);
}
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
if (_pins[i] >= 0) { if (_pins[i] >= 0) {
px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, // TODO(birchera): use return value to make sure everything is fine
2000)); // TODO(birchera): use return value to make sure everything is fine up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
} }
} }
} }
void CameraInterfacePWM::trigger(bool enable) void CameraInterfacePWM::trigger(bool enable)
{ {
// Check if armed, otherwise don't send high PWM values // This only starts working upon prearming
if (_vehicle_status_sub < 0) {
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); if (!_camera_is_on) {
// Turn camera on and give time to start up
powerOn();
return;
} }
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); if (enable) {
// Set all valid pins to shoot level
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++) { for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
if (_pins[i] >= 0) { if (_pins[i] >= 0) {
px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000)); up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
} }
} }
} else { } else {
if (!_camera_is_on) { // Set all valid pins back to neutral level
// Turn camera on and give time to start up for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
powerOn(); if (_pins[i] >= 0) {
return; up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
}
if (trig) {
// Set all valid pins to shoot level
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
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 (_pins[i] >= 0) {
px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
}
} }
} }
} }
@@ -102,62 +88,32 @@ void CameraInterfacePWM::trigger(bool enable)
int CameraInterfacePWM::powerOn() int CameraInterfacePWM::powerOn()
{ {
// Check if armed, otherwise don't send high PWM values // This only starts working upon prearming
if (_vehicle_status_sub < 0) {
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); // 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));
}
} }
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); _camera_is_on = true;
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 (_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 (_pins[i] >= 0) {
px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_ON, 1000, 2000));
}
}
_camera_is_on = true;
}
return 0; return 0;
} }
int CameraInterfacePWM::powerOff() int CameraInterfacePWM::powerOff()
{ {
// Check if armed, otherwise don't send high PWM values // This only starts working upon prearming
if (_vehicle_status_sub < 0) {
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); // 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));
}
} }
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); _camera_is_on = false;
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 (_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 (_pins[i] >= 0) {
px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_OFF, 1000, 2000));
}
}
_camera_is_on = false;
}
return 0; return 0;
} }
src/drivers/camera_trigger/interfaces/src/pwm.h
-11
View File
@@ -40,18 +40,7 @@ public:
private: private:
void setup(); void setup();
int _vehicle_status_sub;
struct vehicle_status_s _vehicle_status;
int _vehicle_status_sub;
struct vehicle_status_s _vehicle_status;
int _vehicle_status_sub;
struct vehicle_status_s _vehicle_status;
param_t _p_pin; param_t _p_pin;
const char *_pwm_dev;
int _pwm_fd;
bool _camera_is_on; bool _camera_is_on;
}; };