fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

adding the initial pwm trigger logic

Browse Source
This commit is contained in:
Andreas Bircher 2016-06-10 12:11:56 +02:00 committed by Lorenz Meier
parent 3671bfb743
commit f83c53c274
2 changed files with 125 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
src/drivers/camera_trigger/interfaces/src/pwm.cpp
View File

@ -1,8 +1,33 @@
#include "pwm.h"
CameraInterfacePWM::CameraInterfacePWM():
CameraInterface()
CameraInterface(),
_camera_is_on(false)
{
_p_pin = param_find("TRIG_PINS");
int pin_list;
param_get(_p_pin, &pin_list);
// Set all pins as invalid
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
_pins[i] = -1;
}
// Convert number to individual channels
unsigned i = 0;
int single_pin;
while ((single_pin = pin_list % 10)) {
_pins[i] = single_pin - 1;
if (_pins[i] < 0 || _pins[i] >= static_cast<int>(sizeof(_gpios) / sizeof(_gpios[0]))) {
_pins[i] = -1;
}
pin_list /= 10;
i++;
}
}
CameraInterfacePWM::~CameraInterfacePWM()
@ -11,20 +36,100 @@ 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 (_pin[i] >= 0) {
int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
}
}
}
void CameraInterfacePWM::trigger(bool enable)
{
// Check if armed, otherwise don't send high PWM values
if (_disarmed) {
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));
}
}
} 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 (_pin[i] >= 0) {
int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[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));
}
}
}
}
}
int CameraInterfacePWM::powerOn()
{
// Check if armed, otherwise don't send high PWM values
if (_disarmed) {
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));
}
}
} 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));
}
}
_camera_is_on = true;
}
return 0;
}
int CameraInterfacePWM::powerOff()
{
// Check if armed, otherwise don't send high PWM values
if (_disarmed) {
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));
}
}
} 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));
}
}
_camera_is_on = false;
}
return 0;
}

19
src/drivers/camera_trigger/interfaces/src/pwm.h
View File

@ -8,6 +8,19 @@
#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)
// 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
class CameraInterfacePWM : public CameraInterface
{
@ -22,6 +35,12 @@ public:
int powerOn();
int powerOff();
int _pins[6];
private:
param_t _p_pin;
const char *_pwm_dev;
int _pwm_fd;
bool _camera_is_on;
};