mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-27 22:00:35 +08:00
camera_trigger : completely refactor state handling
This commit is contained in:
Mohammed Kabir
committed by
Lorenz Meier
Lorenz Meier
parent
7af7c86384
commit
2e92a3946d
@@ -57,10 +57,12 @@
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/camera_trigger.h>
|
||||
#include <uORB/topics/camera_capture.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
@@ -69,11 +71,9 @@
|
||||
#include "interfaces/src/pwm.h"
|
||||
#include "interfaces/src/seagull_map2.h"
|
||||
|
||||
#define TRIGGER_PIN_DEFAULT 1
|
||||
|
||||
extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]);
|
||||
|
||||
typedef enum : int32_t {
|
||||
typedef enum : uint8_t {
|
||||
CAMERA_INTERFACE_MODE_NONE = 0,
|
||||
CAMERA_INTERFACE_MODE_GPIO,
|
||||
CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM,
|
||||
@@ -81,6 +81,16 @@ typedef enum : int32_t {
|
||||
CAMERA_INTERFACE_MODE_GENERIC_PWM
|
||||
} camera_interface_mode_t;
|
||||
|
||||
typedef enum : uint8_t {
|
||||
TRIGGER_MODE_NONE = 0,
|
||||
TRIGGER_MODE_INTERVAL_ON_CMD,
|
||||
TRIGGER_MODE_INTERVAL_ALWAYS_ON,
|
||||
TRIGGER_MODE_DISTANCE_ALWAYS_ON,
|
||||
TRIGGER_MODE_DISTANCE_ON_CMD
|
||||
} trigger_mode_t;
|
||||
|
||||
#define commandParamToInt(n) static_cast<int>(n >= 0 ? n + 0.5f : n - 0.5f)
|
||||
|
||||
class CameraTrigger
|
||||
{
|
||||
public:
|
||||
@@ -95,9 +105,14 @@ public:
|
||||
~CameraTrigger();
|
||||
|
||||
/**
|
||||
* Set the trigger on / off
|
||||
* Run intervalometer update
|
||||
*/
|
||||
void control(bool on);
|
||||
void update_intervalometer();
|
||||
|
||||
/**
|
||||
* Run distance-based trigger update
|
||||
*/
|
||||
void update_distance();
|
||||
|
||||
/**
|
||||
* Trigger the camera just once
|
||||
@@ -145,18 +160,20 @@ private:
|
||||
|
||||
static struct work_s _work;
|
||||
|
||||
int _mode;
|
||||
float _activation_time;
|
||||
float _interval;
|
||||
float _distance;
|
||||
uint32_t _trigger_seq;
|
||||
bool _trigger_enabled;
|
||||
bool _trigger_paused;
|
||||
bool _one_shot;
|
||||
bool _test_shot;
|
||||
bool _turning_on;
|
||||
math::Vector<2> _last_shoot_position;
|
||||
bool _valid_position;
|
||||
|
||||
int _vcommand_sub;
|
||||
int _vlposition_sub;
|
||||
int _command_sub;
|
||||
int _lpos_sub;
|
||||
|
||||
orb_advert_t _trigger_pub;
|
||||
orb_advert_t _cmd_ack_pub;
|
||||
@@ -166,6 +183,9 @@ private:
|
||||
param_t _p_interval;
|
||||
param_t _p_distance;
|
||||
param_t _p_interface;
|
||||
param_t _p_feedback;
|
||||
|
||||
trigger_mode_t _trigger_mode;
|
||||
|
||||
camera_interface_mode_t _camera_interface_mode;
|
||||
CameraInterface *_camera_interface; ///< instance of camera interface
|
||||
@@ -216,23 +236,26 @@ CameraTrigger::CameraTrigger() :
|
||||
_disengage_turn_on_off_call {},
|
||||
_keepalivecall_up {},
|
||||
_keepalivecall_down {},
|
||||
_mode(0),
|
||||
_activation_time(0.5f /* ms */),
|
||||
_interval(100.0f /* ms */),
|
||||
_distance(25.0f /* m */),
|
||||
_trigger_seq(0),
|
||||
_trigger_enabled(false),
|
||||
_trigger_paused(false),
|
||||
_one_shot(false),
|
||||
_test_shot(false),
|
||||
_turning_on(false),
|
||||
_last_shoot_position(0.0f, 0.0f),
|
||||
_valid_position(false),
|
||||
_vcommand_sub(-1),
|
||||
_vlposition_sub(-1),
|
||||
_command_sub(-1),
|
||||
_lpos_sub(-1),
|
||||
_trigger_pub(nullptr),
|
||||
_cmd_ack_pub(nullptr),
|
||||
_trigger_mode(TRIGGER_MODE_NONE),
|
||||
_camera_interface_mode(CAMERA_INTERFACE_MODE_GPIO),
|
||||
_camera_interface(nullptr)
|
||||
{
|
||||
//Initiate Camera interface basedon camera_interface_mode
|
||||
// Initiate camera interface based on camera_interface_mode
|
||||
if (_camera_interface != nullptr) {
|
||||
delete (_camera_interface);
|
||||
// set to zero to ensure parser is not used while not instantiated
|
||||
@@ -251,7 +274,7 @@ CameraTrigger::CameraTrigger() :
|
||||
param_get(_p_activation_time, &_activation_time);
|
||||
param_get(_p_interval, &_interval);
|
||||
param_get(_p_distance, &_distance);
|
||||
param_get(_p_mode, &_mode);
|
||||
param_get(_p_mode, &_trigger_mode);
|
||||
param_get(_p_interface, &_camera_interface_mode);
|
||||
|
||||
switch (_camera_interface_mode) {
|
||||
@@ -291,6 +314,10 @@ CameraTrigger::CameraTrigger() :
|
||||
param_set(_p_activation_time, &(_activation_time));
|
||||
}
|
||||
|
||||
// Advertise critical publishers here, because we cannot advertise in interrupt context
|
||||
struct camera_trigger_s trigger = {};
|
||||
_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
|
||||
|
||||
}
|
||||
|
||||
CameraTrigger::~CameraTrigger()
|
||||
@@ -301,12 +328,13 @@ CameraTrigger::~CameraTrigger()
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::control(bool on)
|
||||
CameraTrigger::update_intervalometer()
|
||||
{
|
||||
// always execute, even if already on
|
||||
// to reset timings if necessary
|
||||
|
||||
if (on) {
|
||||
// the actual intervalometer runs in interrupt context, so we only need to call
|
||||
// control_intervalometer once on enabling/disabling trigger to schedule the calls.
|
||||
|
||||
if (_trigger_enabled && !_trigger_paused) {
|
||||
// schedule trigger on and off calls
|
||||
hrt_call_every(&_engagecall, 0, (_interval * 1000),
|
||||
(hrt_callout)&CameraTrigger::engage, this);
|
||||
@@ -315,16 +343,47 @@ CameraTrigger::control(bool on)
|
||||
hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000),
|
||||
(hrt_callout)&CameraTrigger::disengage, this);
|
||||
|
||||
} else {
|
||||
// cancel all calls
|
||||
hrt_cancel(&_engagecall);
|
||||
hrt_cancel(&_disengagecall);
|
||||
// ensure that the pin is off
|
||||
hrt_call_after(&_disengagecall, 0,
|
||||
(hrt_callout)&CameraTrigger::disengage, this);
|
||||
}
|
||||
|
||||
_trigger_enabled = on;
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::update_distance()
|
||||
{
|
||||
|
||||
if (_lpos_sub < 0) {
|
||||
_lpos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||
}
|
||||
|
||||
if (_turning_on) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (_trigger_enabled) {
|
||||
|
||||
struct vehicle_local_position_s local = {};
|
||||
orb_copy(ORB_ID(vehicle_local_position), _lpos_sub, &local);
|
||||
|
||||
if (local.xy_valid) {
|
||||
|
||||
// Initialize position if not done yet
|
||||
math::Vector<2> current_position(local.x, local.y);
|
||||
|
||||
if (!_valid_position) {
|
||||
// First time valid position, take first shot
|
||||
_last_shoot_position = current_position;
|
||||
_valid_position = local.xy_valid;
|
||||
shoot_once();
|
||||
}
|
||||
|
||||
// Check that distance threshold is exceeded
|
||||
if ((_last_shoot_position - current_position).length() >= _distance) {
|
||||
shoot_once();
|
||||
_last_shoot_position = current_position;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
@@ -361,24 +420,28 @@ CameraTrigger::toggle_power()
|
||||
void
|
||||
CameraTrigger::shoot_once()
|
||||
{
|
||||
// schedule trigger on and off calls
|
||||
hrt_call_after(&_engagecall, 0,
|
||||
(hrt_callout)&CameraTrigger::engage, this);
|
||||
if (!_trigger_paused) {
|
||||
|
||||
// schedule trigger on and off calls
|
||||
hrt_call_after(&_engagecall, 0,
|
||||
(hrt_callout)&CameraTrigger::engage, this);
|
||||
|
||||
hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000),
|
||||
(hrt_callout)&CameraTrigger::disengage, this);
|
||||
}
|
||||
|
||||
hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000),
|
||||
(hrt_callout)&CameraTrigger::disengage, this);
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::start()
|
||||
{
|
||||
// enable immediate if configured that way
|
||||
if (_mode == 2) {
|
||||
control(true);
|
||||
}
|
||||
|
||||
// If not in mission mode and the interface supports it, enable power to the camera
|
||||
if ((_mode > 0 && _mode < 4) && _camera_interface->has_power_control()) {
|
||||
if ((_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON ||
|
||||
_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) &&
|
||||
_camera_interface->has_power_control() &&
|
||||
!_camera_interface->is_powered_on()) {
|
||||
|
||||
// If in always-on mode and the interface supports it, enable power to the camera
|
||||
toggle_power();
|
||||
enable_keep_alive(true);
|
||||
|
||||
@@ -386,6 +449,17 @@ CameraTrigger::start()
|
||||
enable_keep_alive(false);
|
||||
}
|
||||
|
||||
// enable immediately if configured that way
|
||||
if (_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON) {
|
||||
// enable and start triggering
|
||||
_trigger_enabled = true;
|
||||
update_intervalometer();
|
||||
|
||||
} else if (_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
|
||||
// just enable, but do not fire. actual trigger is based on distance covered
|
||||
_trigger_enabled = true;
|
||||
}
|
||||
|
||||
// start to monitor at high rate for trigger enable command
|
||||
work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1));
|
||||
|
||||
@@ -425,188 +499,199 @@ CameraTrigger::cycle_trampoline(void *arg)
|
||||
|
||||
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
|
||||
|
||||
if (trig->_vcommand_sub < 0) {
|
||||
trig->_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
// default loop polling interval
|
||||
int poll_interval_usec = 5000;
|
||||
|
||||
if (trig->_command_sub < 0) {
|
||||
trig->_command_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
}
|
||||
|
||||
bool updated = false;
|
||||
orb_check(trig->_vcommand_sub, &updated);
|
||||
orb_check(trig->_command_sub, &updated);
|
||||
|
||||
struct vehicle_command_s cmd = {};
|
||||
unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
bool need_ack = false;
|
||||
|
||||
// this flag is set when the polling loop is slowed down to allow the camera to power on
|
||||
bool turning_on = false;
|
||||
trig->_turning_on = false;
|
||||
|
||||
// while the trigger is inactive it has to be ready to become active instantaneously
|
||||
int poll_interval_usec = 5000;
|
||||
// these flags are used to detect state changes in the command loop
|
||||
bool main_state = trig->_trigger_enabled;
|
||||
bool pause_state = trig->_trigger_paused;
|
||||
|
||||
/**
|
||||
* Test mode handling
|
||||
*/
|
||||
|
||||
// check for command update
|
||||
// Command handling
|
||||
if (updated) {
|
||||
|
||||
orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);
|
||||
orb_copy(ORB_ID(vehicle_command), trig->_command_sub, &cmd);
|
||||
|
||||
// We always check for this command as it is used by the GCS to fire test shots
|
||||
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
|
||||
|
||||
need_ack = true;
|
||||
|
||||
if (cmd.param5 > 0) {
|
||||
|
||||
// If camera isn't powered on, we enable power to it on
|
||||
// getting the test command.
|
||||
|
||||
if (trig->_camera_interface->has_power_control() &&
|
||||
!trig->_camera_interface->is_powered_on()) {
|
||||
|
||||
trig->toggle_power();
|
||||
trig->enable_keep_alive(true);
|
||||
|
||||
// Give the camera time to turn on before starting to send trigger signals
|
||||
poll_interval_usec = 3000000;
|
||||
turning_on = true;
|
||||
}
|
||||
|
||||
// Schedule test shot
|
||||
if (commandParamToInt(cmd.param7) == 1) {
|
||||
// test shots are not logged or forwarded to GCS for geotagging
|
||||
trig->_test_shot = true;
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
if (commandParamToInt((float)cmd.param5) == 1) {
|
||||
// Schedule shot
|
||||
trig->_one_shot = true;
|
||||
|
||||
}
|
||||
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
|
||||
|
||||
need_ack = true;
|
||||
PX4_INFO("got trigger control");
|
||||
|
||||
if (commandParamToInt(cmd.param3) == 1) {
|
||||
// pause triggger
|
||||
trig->_trigger_paused = true;
|
||||
PX4_INFO("paused by command");
|
||||
|
||||
} else if (commandParamToInt(cmd.param3) == 0) {
|
||||
trig->_trigger_paused = false;
|
||||
PX4_INFO("unpaused by command");
|
||||
|
||||
}
|
||||
|
||||
if (commandParamToInt(cmd.param2) == 1) {
|
||||
// reset trigger sequence
|
||||
trig->_trigger_seq = 0;
|
||||
|
||||
}
|
||||
|
||||
if (commandParamToInt(cmd.param1) == 1) {
|
||||
trig->_trigger_enabled = true;
|
||||
PX4_INFO("enabled by command");
|
||||
|
||||
} else if (commandParamToInt(cmd.param1) == 0) {
|
||||
trig->_trigger_enabled = false;
|
||||
PX4_INFO("disabled by command");
|
||||
|
||||
}
|
||||
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
|
||||
|
||||
need_ack = true;
|
||||
|
||||
if (cmd.param1 > 0.0f) {
|
||||
trig->_distance = cmd.param1;
|
||||
param_set(trig->_p_distance, &(trig->_distance));
|
||||
}
|
||||
|
||||
// We can only control the shutter integration time of the camera in GPIO mode
|
||||
if (cmd.param2 > 0.0f) {
|
||||
if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
|
||||
trig->_activation_time = cmd.param2;
|
||||
param_set(trig->_p_activation_time, &(trig->_activation_time));
|
||||
}
|
||||
}
|
||||
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
|
||||
|
||||
need_ack = true;
|
||||
|
||||
if (cmd.param1 > 0.0f) {
|
||||
trig->_interval = cmd.param1;
|
||||
param_set(trig->_p_interval, &(trig->_interval));
|
||||
}
|
||||
|
||||
// We can only control the shutter integration time of the camera in GPIO mode
|
||||
if (cmd.param2 > 0.0f) {
|
||||
if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
|
||||
trig->_activation_time = cmd.param2;
|
||||
param_set(trig->_p_activation_time, &(trig->_activation_time));
|
||||
}
|
||||
}
|
||||
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} // TODO : live geotag retransmission interface
|
||||
|
||||
}
|
||||
|
||||
if (trig->_test_shot && !turning_on) {
|
||||
// State change handling
|
||||
if ((main_state != trig->_trigger_enabled) ||
|
||||
(pause_state != trig->_trigger_paused) ||
|
||||
trig->_one_shot) {
|
||||
|
||||
if (trig->_trigger_enabled || trig->_one_shot) { // Just got enabled via a command
|
||||
|
||||
// If camera isn't already powered on, we enable power to it
|
||||
if (!trig->_camera_interface->is_powered_on() &&
|
||||
trig->_camera_interface->has_power_control()) {
|
||||
|
||||
trig->toggle_power();
|
||||
trig->enable_keep_alive(true);
|
||||
|
||||
// Give the camera time to turn on before starting to send trigger signals
|
||||
poll_interval_usec = 3000000;
|
||||
trig->_turning_on = true;
|
||||
}
|
||||
|
||||
} else if (!trig->_trigger_enabled || trig->_trigger_paused) { // Just got disabled/paused via a command
|
||||
|
||||
// Power off the camera if we are disabled
|
||||
if (trig->_camera_interface->is_powered_on() &&
|
||||
trig->_camera_interface->has_power_control() &&
|
||||
!trig->_trigger_enabled) {
|
||||
|
||||
trig->enable_keep_alive(false);
|
||||
trig->toggle_power();
|
||||
}
|
||||
|
||||
// cancel all calls for both disabled and paused
|
||||
hrt_cancel(&trig->_engagecall);
|
||||
hrt_cancel(&trig->_disengagecall);
|
||||
|
||||
// ensure that the pin is off
|
||||
hrt_call_after(&trig->_disengagecall, 0,
|
||||
(hrt_callout)&CameraTrigger::disengage, trig);
|
||||
|
||||
}
|
||||
|
||||
// only run on state changes, not every loop iteration
|
||||
if (trig->_trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) {
|
||||
|
||||
// update intervalometer state and reset calls
|
||||
trig->update_intervalometer();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// run every loop iteration and trigger if needed
|
||||
if (trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
|
||||
trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
|
||||
|
||||
// update distance counter and trigger
|
||||
trig->update_distance();
|
||||
|
||||
}
|
||||
|
||||
// One shot command-based capture handling
|
||||
if (trig->_one_shot && !trig->_turning_on) {
|
||||
|
||||
// One-shot trigger
|
||||
trig->shoot_once();
|
||||
trig->_test_shot = false;
|
||||
trig->_one_shot = false;
|
||||
|
||||
if (trig->_test_shot) {
|
||||
trig->_test_shot = false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Main mode handling
|
||||
*/
|
||||
|
||||
if (trig->_mode == 1) { // 1 - Command controlled mode
|
||||
|
||||
// Check for command update
|
||||
if (updated) {
|
||||
|
||||
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
|
||||
|
||||
need_ack = true;
|
||||
|
||||
if (cmd.param3 > 0.0f) {
|
||||
// reset trigger sequence
|
||||
trig->_trigger_seq = 0;
|
||||
|
||||
}
|
||||
|
||||
if (cmd.param2 > 0.0f) {
|
||||
// set trigger rate from command
|
||||
trig->_interval = cmd.param2;
|
||||
param_set(trig->_p_interval, &(trig->_interval));
|
||||
}
|
||||
|
||||
if (cmd.param1 > 0.0f) {
|
||||
trig->control(true);
|
||||
// while the trigger is active there is no
|
||||
// need to poll at a very high rate
|
||||
poll_interval_usec = 100000;
|
||||
|
||||
} else {
|
||||
trig->control(false);
|
||||
|
||||
}
|
||||
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
} else if (trig->_mode == 3 || trig->_mode == 4) { // 3,4 - Distance controlled modes
|
||||
|
||||
// Set trigger based on covered distance
|
||||
if (trig->_vlposition_sub < 0) {
|
||||
trig->_vlposition_sub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||
}
|
||||
|
||||
struct vehicle_local_position_s pos = {};
|
||||
|
||||
orb_copy(ORB_ID(vehicle_local_position), trig->_vlposition_sub, &pos);
|
||||
|
||||
if (pos.xy_valid) {
|
||||
|
||||
// Check for command update
|
||||
if (updated) {
|
||||
|
||||
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
|
||||
|
||||
need_ack = true;
|
||||
|
||||
if (cmd.param1 > 0.0f && !trig->_trigger_enabled) {
|
||||
|
||||
if (trig->_camera_interface->has_power_control()) {
|
||||
trig->toggle_power();
|
||||
trig->enable_keep_alive(true);
|
||||
|
||||
// Give the camera time to turn on before sending trigger signals
|
||||
poll_interval_usec = 3000000;
|
||||
turning_on = true;
|
||||
}
|
||||
|
||||
} else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) {
|
||||
|
||||
// Disable trigger if the set distance is not positive
|
||||
hrt_cancel(&(trig->_engagecall));
|
||||
hrt_cancel(&(trig->_disengagecall));
|
||||
|
||||
if (trig->_camera_interface->has_power_control()) {
|
||||
trig->enable_keep_alive(false);
|
||||
trig->toggle_power();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
trig->_trigger_enabled = cmd.param1 > 0.0f;
|
||||
trig->_distance = cmd.param1;
|
||||
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
if (trig->_trigger_enabled && !turning_on) {
|
||||
|
||||
// Initialize position if not done yet
|
||||
math::Vector<2> current_position(pos.x, pos.y);
|
||||
|
||||
if (!trig->_valid_position) {
|
||||
// First time valid position, take first shot
|
||||
trig->_last_shoot_position = current_position;
|
||||
trig->_valid_position = pos.xy_valid;
|
||||
trig->shoot_once();
|
||||
}
|
||||
|
||||
// Check that distance threshold is exceeded
|
||||
if ((trig->_last_shoot_position - current_position).length() >= trig->_distance) {
|
||||
trig->shoot_once();
|
||||
trig->_last_shoot_position = current_position;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
} else {
|
||||
poll_interval_usec = 100000;
|
||||
}
|
||||
}
|
||||
|
||||
// Send ACKs for trigger commands
|
||||
// Command ACK handling
|
||||
if (updated && need_ack) {
|
||||
vehicle_command_ack_s command_ack = {};
|
||||
|
||||
@@ -633,23 +718,28 @@ CameraTrigger::engage(void *arg)
|
||||
|
||||
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
|
||||
|
||||
struct camera_trigger_s report = {};
|
||||
|
||||
// Set timestamp the instant before the trigger goes off
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
// Trigger the camera
|
||||
trig->_camera_interface->trigger(true);
|
||||
|
||||
report.seq = trig->_trigger_seq++;
|
||||
// Send camera trigger message. This messages indicates that we sent
|
||||
// the camera trigger request. Does not guarantee capture.
|
||||
|
||||
if (trig->_trigger_pub == nullptr) {
|
||||
trig->_trigger_pub = orb_advertise_queue(ORB_ID(camera_trigger), &report,
|
||||
camera_trigger_s::ORB_QUEUE_LENGTH);
|
||||
struct camera_trigger_s trigger = {};
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &report);
|
||||
// Set timestamp the instant after the trigger goes off
|
||||
trigger.timestamp = hrt_absolute_time();
|
||||
|
||||
timespec tv = {};
|
||||
px4_clock_gettime(CLOCK_REALTIME, &tv);
|
||||
trigger.timestamp_utc = (uint64_t) tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
|
||||
|
||||
trigger.seq = trig->_trigger_seq;
|
||||
|
||||
orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
|
||||
|
||||
// increment frame count
|
||||
trig->_trigger_seq++;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
@@ -695,17 +785,30 @@ CameraTrigger::keep_alive_down(void *arg)
|
||||
void
|
||||
CameraTrigger::status()
|
||||
{
|
||||
PX4_INFO("state : %s", _trigger_enabled ? "enabled" : "disabled");
|
||||
PX4_INFO("mode : %i", _mode);
|
||||
PX4_INFO("interval : %.2f [ms]", (double)_interval);
|
||||
PX4_INFO("distance : %.2f [m]", (double)_distance);
|
||||
PX4_INFO("main state : %s", _trigger_enabled ? "enabled" : "disabled");
|
||||
PX4_INFO("pause state : %s", _trigger_paused ? "paused" : "active");
|
||||
PX4_INFO("mode : %i", _trigger_mode);
|
||||
|
||||
if (_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON ||
|
||||
_trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) {
|
||||
PX4_INFO("interval : %.2f [ms]", (double)_interval);
|
||||
|
||||
} else if (_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON ||
|
||||
_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD) {
|
||||
PX4_INFO("distance : %.2f [m]", (double)_distance);
|
||||
}
|
||||
|
||||
if (_camera_interface->has_power_control()) {
|
||||
PX4_INFO("camera power : %s", _camera_interface->is_powered_on() ? "ON" : "OFF");
|
||||
}
|
||||
|
||||
PX4_INFO("activation time : %.2f [ms]", (double)_activation_time);
|
||||
_camera_interface->info();
|
||||
}
|
||||
|
||||
static int usage()
|
||||
{
|
||||
PX4_INFO("usage: camera_trigger {start|stop|enable|disable|status|test|test_power}\n");
|
||||
PX4_INFO("usage: camera_trigger {start|stop|status|test|test_power}\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
@@ -743,12 +846,6 @@ int camera_trigger_main(int argc, char *argv[])
|
||||
} else if (!strcmp(argv[1], "status")) {
|
||||
camera_trigger::g_camera_trigger->status();
|
||||
|
||||
} else if (!strcmp(argv[1], "enable")) {
|
||||
camera_trigger::g_camera_trigger->control(true);
|
||||
|
||||
} else if (!strcmp(argv[1], "disable")) {
|
||||
camera_trigger::g_camera_trigger->control(false);
|
||||
|
||||
} else if (!strcmp(argv[1], "test")) {
|
||||
camera_trigger::g_camera_trigger->test();
|
||||
|
||||
|
||||
Reference in New Issue
Block a user