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

Camera trigger update (#4998)

Browse Source 
* updating the camera driver, correct init and keepAlive function

* removing debug output
This commit is contained in:
Andreas Bircher
2016-07-07 11:40:11 +02:00
committed by Lorenz Meier
parent 15880f8d13
commit 9974b6f747
5 changed files with 143 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/camera_trigger/camera_trigger.cpp
+71 -1
View File
@@ -103,6 +103,11 @@ public:
*/
void shootOnce();
/**
* Toggle keep camera alive functionality
*/
void keepAlive(bool on);
/**
* Start the task.
*/
@@ -127,6 +132,9 @@ private:
struct hrt_call _engagecall;
struct hrt_call _disengagecall;
struct hrt_call _keepalivecall_up;
struct hrt_call _keepalivecall_down;
static struct work_s _work;
int _gpio_fd;
@@ -167,6 +175,14 @@ private:
* Resets trigger
*/
static void disengage(void *arg);
/**
* Fires trigger
*/
static void keep_alive_up(void *arg);
/**
* Resets trigger
*/
static void keep_alive_down(void *arg);
};
@@ -232,6 +248,7 @@ CameraTrigger::CameraTrigger() :
}
struct camera_trigger_s report = {};
_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &report);
}
@@ -269,6 +286,26 @@ CameraTrigger::control(bool on)
_trigger_enabled = on;
}
void
CameraTrigger::keepAlive(bool on)
{
if (on) {
// schedule keep-alive up and down calls
hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000),
(hrt_callout)&CameraTrigger::keep_alive_up, this);
// schedule keep-alive up and down calls
hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000),
(hrt_callout)&CameraTrigger::keep_alive_down, this);
} else {
// cancel all calls
hrt_cancel(&_keepalivecall_up);
hrt_cancel(&_keepalivecall_down);
}
}
void
CameraTrigger::shootOnce()
{
@@ -289,6 +326,14 @@ CameraTrigger::start()
control(true);
}
// Prevent camera from sleeping, if triggering is enabled
if (_mode > 0) {
keepAlive(true);
} else {
keepAlive(false);
}
// start to monitor at high rate for trigger enable command
work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1));
@@ -300,6 +345,8 @@ CameraTrigger::stop()
work_cancel(LPWORK, &_work);
hrt_cancel(&_engagecall);
hrt_cancel(&_disengagecall);
hrt_cancel(&_keepalivecall_up);
hrt_cancel(&_keepalivecall_down);
if (camera_trigger::g_camera_trigger != nullptr) {
delete(camera_trigger::g_camera_trigger);
@@ -390,7 +437,14 @@ CameraTrigger::cycle_trampoline(void *arg)
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
// Set trigger to false if the set distance is not positive
// Set trigger to disabled if the set distance is not positive
if (cmd.param1 > 0.0f && !trig->_trigger_enabled) {
trig->_camera_interface->powerOn();
} else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) {
trig->_camera_interface->powerOff();
}
trig->_trigger_enabled = cmd.param1 > 0.0f;
trig->_distance = cmd.param1;
}
@@ -450,6 +504,22 @@ CameraTrigger::disengage(void *arg)
trig->_camera_interface->trigger(false);
}
void
CameraTrigger::keep_alive_up(void *arg)
{
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
trig->_camera_interface->keep_alive(true);
}
void
CameraTrigger::keep_alive_down(void *arg)
{
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
trig->_camera_interface->keep_alive(false);
}
void
CameraTrigger::info()
{
src/drivers/camera_trigger/camera_trigger_params.c
+5 -16
View File
@@ -36,25 +36,12 @@
* Camera trigger parameters
*
* @author Mohammed Kabir <mhkabir98@gmail.com>
* @author Andreas Bircher <andreas@wingtra.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* Camera trigger Interface
*
* Selects the trigger interface
*
* @value 1 GPIO
* @value 2 Seagull MAP2 (PWM)
*
* @reboot_required true
*
* @group Camera trigger
*/
PARAM_DEFINE_INT32(TRIG_INTERFACE, 2);
/**
* Camera trigger Interface
*
@@ -102,7 +89,7 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0);
*
* @unit ms
* @min 0.1
* @max 3
* @max 3000
* @decimal 1
* @group Camera trigger
*/
@@ -126,7 +113,9 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0);
/**
* Camera trigger pin
*
* Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6)
* Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6 on px4fmu-v2 and the rail
* pins on px4fmu-v4). The PWM interface takes two pins per camera, while relay
* triggers on every pin individually. Example: Value 34 would trigger on pins 3 and 4.
*
* @min 1
* @max 123456
src/drivers/camera_trigger/interfaces/src/camera_interface.h
+5
View File
@@ -24,6 +24,11 @@ public:
*/
virtual void trigger(bool enable) {};
/**
* prevent the camera from sleeping
* @param keep alive signal:
*/
virtual void keep_alive(bool signal_on) {};
/**
* Display info.
src/drivers/camera_trigger/interfaces/src/pwm.cpp
+61 -16
View File
@@ -7,12 +7,14 @@
// 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_DISARMED 900
#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_2_CAMERA_KEEP_ALIVE 1700
#define PWM_2_CAMERA_ON_OFF 1900
CameraInterfacePWM::CameraInterfacePWM():
CameraInterface(),
@@ -48,13 +50,18 @@ CameraInterfacePWM::CameraInterfacePWM():
CameraInterfacePWM::~CameraInterfacePWM()
{
// Deinitialise pwm channels
// Can currently not be used, because up_pwm_servo_deinit() will deinitialise all pwm channels
// up_pwm_servo_deinit();
}
void CameraInterfacePWM::setup()
{
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
if (_pins[i] >= 0) {
// TODO(birchera): use return value to make sure everything is fine
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
uint8_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]);
up_pwm_servo_init(pin_bitmask);
up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
}
}
@@ -65,23 +72,52 @@ void CameraInterfacePWM::trigger(bool enable)
// This only starts working upon prearming
if (!_camera_is_on) {
// (TODO: powerOn does not work yet)
// Turn camera on and give time to start up
powerOn();
// powerOn();
return;
}
if (enable) {
// Set all valid pins to shoot 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_INSTANT_SHOOT, 1000, 2000));
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
up_pwm_servo_set(_pins[i + 1], 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) {
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
}
}
}
}
void CameraInterfacePWM::keep_alive(bool signal_on)
{
// This should alternate between signal_on and !signal_on to keep the camera alive
if (!_camera_is_on) {
// (TODO: powerOn does not work yet)
// Turn camera on and give time to start up
powerOn();
}
if (signal_on) {
// Set channel 2 pin to keep_alive signal
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_KEEP_ALIVE, 1000, 2000));
}
}
} else {
// Set channel 2 pin to neutral signal
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
}
}
@@ -93,9 +129,17 @@ int CameraInterfacePWM::powerOn()
// 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));
// for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
// if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
// up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_ON, 1000, 2000));
// up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000));
// }
// }
// For now, set channel one on neutral upon startup.
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
}
}
@@ -109,9 +153,10 @@ int CameraInterfacePWM::powerOff()
// 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));
for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_OFF, 1000, 2000));
up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000));
}
}
src/drivers/camera_trigger/interfaces/src/pwm.h
+1
View File
@@ -18,6 +18,7 @@ public:
virtual ~CameraInterfacePWM();
void trigger(bool enable);
void keep_alive(bool signal_on);
int powerOn();
int powerOff();