dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

initial version camera turn on / off

This commit is contained in:
Andreas Bircher 2016-08-02 13:59:19 +02:00 committed by Lorenz Meier
parent a97c5ec4e1
commit 70cd06bc84
5 changed files with 145 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
src/drivers/camera_trigger/camera_trigger.cpp
View File

@ -108,6 +108,11 @@ public:
*/
void keepAlive(bool on);
/**
* Toggle camera on/off functionality
*/
void turnOnOff();
/**
* Start the task.
*/
@ -132,6 +137,8 @@ private:
struct hrt_call _engagecall;
struct hrt_call _disengagecall;
struct hrt_call _engage_turn_on_off_call;
struct hrt_call _disengage_turn_on_off_call;
struct hrt_call _keepalivecall_up;
struct hrt_call _keepalivecall_down;
@ -175,6 +182,14 @@ private:
* Resets trigger
*/
static void disengage(void *arg);
/**
* Fires on/off
*/
static void engange_turn_on_off(void *arg);
/**
* Resets on/off
*/
static void disengage_turn_on_off(void *arg);
/**
* Fires trigger
*/
@ -197,6 +212,10 @@ CameraTrigger *g_camera_trigger;
CameraTrigger::CameraTrigger() :
_engagecall {},
_disengagecall {},
_engage_turn_on_off_call {},
_disengage_turn_on_off_call {},
_keepalivecall_up {},
_keepalivecall_down {},
_gpio_fd(-1),
_mode(0),
_activation_time(0.5f /* ms */),
@ -306,6 +325,18 @@ CameraTrigger::keepAlive(bool on)
}
void
CameraTrigger::turnOnOff()
{
// schedule trigger on and off calls
hrt_call_after(&_engage_turn_on_off_call, 0,
(hrt_callout)&CameraTrigger::engange_turn_on_off, this);
// schedule trigger on and off calls
hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000),
(hrt_callout)&CameraTrigger::disengage_turn_on_off, this);
}
void
CameraTrigger::shootOnce()
{
@ -327,7 +358,8 @@ CameraTrigger::start()
}
// Prevent camera from sleeping, if triggering is enabled
if (_mode > 0) {
if (_mode > 0 && _mode < 4) {
turnOnOff();
keepAlive(true);
} else {
@ -345,6 +377,8 @@ CameraTrigger::stop()
work_cancel(LPWORK, &_work);
hrt_cancel(&_engagecall);
hrt_cancel(&_disengagecall);
hrt_cancel(&_engage_turn_on_off_call);
hrt_cancel(&_disengage_turn_on_off_call);
hrt_cancel(&_keepalivecall_up);
hrt_cancel(&_keepalivecall_down);
@ -358,6 +392,7 @@ CameraTrigger::test()
{
struct vehicle_command_s cmd = {};
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
cmd.param5 = 1.0f;
orb_advert_t pub;
@ -429,6 +464,8 @@ CameraTrigger::cycle_trampoline(void *arg)
if (pos.xy_valid) {
bool turning_on = false;
if (updated && trig->_mode == 4) {
// Check update from command
@ -439,10 +476,16 @@ CameraTrigger::cycle_trampoline(void *arg)
// Set trigger to disabled if the set distance is not positive
if (cmd.param1 > 0.0f && !trig->_trigger_enabled) {
trig->_camera_interface->powerOn();
trig->turnOnOff();
trig->keepAlive(true);
poll_interval_usec = 2000000;
turning_on = true;
} else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) {
trig->_camera_interface->powerOff();
hrt_cancel(&(trig->_engagecall));
hrt_cancel(&(trig->_disengagecall));
trig->keepAlive(false);
trig->turnOnOff();
}
trig->_trigger_enabled = cmd.param1 > 0.0f;
@ -450,7 +493,7 @@ CameraTrigger::cycle_trampoline(void *arg)
}
}
if (trig->_trigger_enabled || trig->_mode < 4) {
if ((trig->_trigger_enabled || trig->_mode < 4) && !turning_on) {
// Initialize position if not done yet
math::Vector<2> current_position(pos.x, pos.y);
@ -504,6 +547,23 @@ CameraTrigger::disengage(void *arg)
trig->_camera_interface->trigger(false);
}
void
CameraTrigger::engange_turn_on_off(void *arg)
{
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
trig->_camera_interface->turn_on_off(true);
}
void
CameraTrigger::disengage_turn_on_off(void *arg)
{
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
trig->_camera_interface->turn_on_off(false);
}
void
CameraTrigger::keep_alive_up(void *arg)
{

6
src/drivers/camera_trigger/camera_trigger_params.c
View File

@ -67,7 +67,7 @@ PARAM_DEFINE_INT32(TRIG_INTERFACE, 2);
* @decimal 1
* @group Camera trigger
*/
PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 40.0f);
PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 3000.0f);
/**
* Camera trigger polarity
@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0);
* @decimal 1
* @group Camera trigger
*/
PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 0.5f);
PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 500.0f);
/**
* Camera trigger mode
@ -123,7 +123,7 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0);
* @reboot_required true
* @group Camera trigger
*/
PARAM_DEFINE_INT32(TRIG_PINS, 6);
PARAM_DEFINE_INT32(TRIG_PINS, 56);
/**
* Camera trigger distance

6
src/drivers/camera_trigger/interfaces/src/camera_interface.h
View File

@ -24,6 +24,12 @@ public:
*/
virtual void trigger(bool enable) {};
/**
* turn on/off the camera
* @param enable:
*/
virtual void turn_on_off(bool enable) {};
/**
* prevent the camera from sleeping
* @param keep alive signal:

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

@ -2,6 +2,7 @@
#include <sys/ioctl.h>
#include <lib/mathlib/mathlib.h>
#include <uORB/topics/trigger_pwm.h>
#include "drivers/drv_pwm_output.h"
#include "pwm.h"
@ -18,6 +19,7 @@
CameraInterfacePWM::CameraInterfacePWM():
CameraInterface(),
_pwm_pub {nullptr, nullptr},
_camera_is_on(false)
{
_p_pin = param_find("TRIG_PINS");
@ -34,17 +36,24 @@ CameraInterfacePWM::CameraInterfacePWM():
int single_pin;
while ((single_pin = pin_list % 10)) {
_pins[i] = single_pin - 1;
if (_pins[i] < 0) {
_pins[i] = -1;
}
// Wingtra safety check, pins can only be on 5 or 6!!!
if (_pins[i] != 4 && _pins[i] != 5) {
_pins[i] = -1;
}
pin_list /= 10;
i++;
}
_orb_id[0] = 0;
_orb_id[1] = 1;
setup();
}
@ -63,6 +72,13 @@ void CameraInterfacePWM::setup()
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));
trigger_pwm_s pwm;
pwm.timestamp = hrt_absolute_time();
pwm.pwm = PWM_CAMERA_DISARMED;
orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT);
pwm.pwm = PWM_CAMERA_DISARMED;
orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT);
}
}
}
@ -72,17 +88,21 @@ 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();
return;
}
trigger_pwm_s pwm;
pwm.timestamp = hrt_absolute_time();
if (enable) {
// Set all valid pins to shoot level
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));
pwm.pwm = PWM_CAMERA_INSTANT_SHOOT;
orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT);
}
}
@ -91,6 +111,9 @@ void CameraInterfacePWM::trigger(bool enable)
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));
pwm.pwm = PWM_CAMERA_NEUTRAL;
orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT);
}
}
}
@ -101,16 +124,20 @@ 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();
return;
}
trigger_pwm_s pwm;
pwm.timestamp = hrt_absolute_time();
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));
pwm.pwm = PWM_2_CAMERA_KEEP_ALIVE;
orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT);
}
}
@ -119,50 +146,50 @@ void CameraInterfacePWM::keep_alive(bool signal_on)
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));
pwm.pwm = PWM_CAMERA_NEUTRAL;
orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT);
}
}
}
}
int CameraInterfacePWM::powerOn()
void CameraInterfacePWM::turn_on_off(bool enable)
{
// 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 = 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));
// }
// }
trigger_pwm_s pwm;
pwm.timestamp = hrt_absolute_time();
// 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));
if (enable) {
// 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));
up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000));
pwm.pwm = PWM_CAMERA_NEUTRAL;
orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT);
pwm.pwm = PWM_2_CAMERA_ON_OFF;
orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT);
}
}
}
_camera_is_on = true;
} else {
// 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));
up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
return 0;
}
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 = 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));
pwm.pwm = PWM_CAMERA_NEUTRAL;
orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[1]), &pwm, &(_orb_id[1]), ORB_PRIO_DEFAULT);
pwm.pwm = PWM_CAMERA_NEUTRAL;
orb_publish_auto(ORB_ID(trigger_pwm), &(_pwm_pub[0]), &pwm, &(_orb_id[0]), ORB_PRIO_DEFAULT);
}
}
_camera_is_on = !_camera_is_on;
}
_camera_is_on = false;
return 0;
}
void CameraInterfacePWM::info()

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

@ -6,6 +6,7 @@
*/
#pragma once
#include <drivers/drv_hrt.h>
#include <systemlib/param/param.h>
#include <uORB/topics/vehicle_status.h>
@ -20,8 +21,7 @@ public:
void trigger(bool enable);
void keep_alive(bool signal_on);
int powerOn();
int powerOff();
void turn_on_off(bool enable);
void info();
@ -29,7 +29,10 @@ public:
private:
void setup();
param_t _p_pin;
bool _camera_is_on;
param_t _p_pin;
orb_advert_t _pwm_pub[2];
int _orb_id[2];
bool _camera_is_on;
};