Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 17:35:22 +08:00
Code Issues Actions 28 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-16 11:25:06 +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
set MIXER_AUX none
set AUX_MODE none
camera_trigger start
camera_trigger start --pwm
fi
fi
@@ -606,7 +606,7 @@ then
# Try to get an USB console
nshterm /dev/ttyACM0 &
else
mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
mavlink start -r 80000 -d /dev/ttyACM0 -m config -x
fi
#
src/drivers/camera_trigger/interfaces/src/pwm.cpp
+40 -84
View File
@@ -4,9 +4,17 @@
#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():
CameraInterface(),
_vehicle_status_sub(-1),
_camera_is_on(false)
{
_p_pin = param_find("TRIG_PINS");
@@ -42,59 +50,37 @@ CameraInterfacePWM::~CameraInterfacePWM()
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++) {
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
// 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)
{
// Check if armed, otherwise don't send high PWM values
if (_vehicle_status_sub < 0) {
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
// This only starts working upon prearming
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 (_vehicle_status.arming_state != _vehicle_status.ARMING_STATE_ARMED
|| !_vehicle_status.no_pwm) {
if (enable) {
// 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_DISARMED, 1000, 2000));
up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
}
}
} else {
if (!_camera_is_on) {
// Turn camera on and give time to start up
powerOn();
return;
}
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));
}
// Set all valid pins back to neutral 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_NEUTRAL, 1000, 2000));
}
}
}
@@ -102,62 +88,32 @@ void CameraInterfacePWM::trigger(bool enable)
int CameraInterfacePWM::powerOn()
{
// Check if armed, otherwise don't send high PWM values
if (_vehicle_status_sub < 0) {
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
// 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));
}
}
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 (_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;
}
_camera_is_on = true;
return 0;
}
int CameraInterfacePWM::powerOff()
{
// Check if armed, otherwise don't send high PWM values
if (_vehicle_status_sub < 0) {
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
// 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));
}
}
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 (_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;
}
_camera_is_on = false;
return 0;
}
src/drivers/camera_trigger/interfaces/src/pwm.h
-11
View File
@@ -40,18 +40,7 @@ public:
private:
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;
const char *_pwm_dev;
int _pwm_fd;
bool _camera_is_on;
};