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

camera_trigger : completely refactor state handling

This commit is contained in:
Mohammed Kabir 2017-05-01 19:05:50 +02:00 committed by Lorenz Meier
parent 7af7c86384
commit 2e92a3946d
9 changed files with 341 additions and 218 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
msg/camera_trigger.msg
View File

@ -1,4 +1,3 @@
uint64 timestamp_utc # UTC timestamp
uint32 seq # Image sequence number
uint32 ORB_QUEUE_LENGTH = 3

3
msg/vehicle_command.msg
View File

@ -42,10 +42,11 @@ uint32 VEHICLE_CMD_DO_SET_ROI = 201 # Sets the region of interest (ROI) for a
uint32 VEHICLE_CMD_DO_DIGICAM_CONTROL=203
uint32 VEHICLE_CMD_DO_MOUNT_CONFIGURE=204 # Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
uint32 VEHICLE_CMD_DO_MOUNT_CONTROL=205 # Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value|
uint32 VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206 # Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|
uint32 VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206 # Mission command to set TRIG_DIST for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty|
uint32 VEHICLE_CMD_DO_FENCE_ENABLE=207 # Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty|
uint32 VEHICLE_CMD_DO_PARACHUTE=208 # Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty|
uint32 VEHICLE_CMD_DO_INVERTED_FLIGHT=210 # Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty|
uint32 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214 # Mission command to set TRIG_INTERVAL for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty|
uint32 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220 # Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty|
uint32 VEHICLE_CMD_DO_GUIDED_MASTER=221 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty|
uint32 VEHICLE_CMD_DO_GUIDED_LIMITS=222 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|

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

@ -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();

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

@ -98,10 +98,10 @@ PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 40.0f);
* Camera trigger mode
*
* @value 0 Disable
* @value 1 On individual commands
* @value 1 Time based, on command
* @value 2 Time based, always on
* @value 3 Distance based, always on
* @value 4 Distance, mission controlled
* @value 4 Distance based, on command (Survey mode)
* @min 0
* @max 4
* @reboot_required true
@ -136,3 +136,16 @@ PARAM_DEFINE_INT32(TRIG_PINS, 56);
* @group Camera trigger
*/
PARAM_DEFINE_FLOAT(TRIG_DISTANCE, 25.0f);
/**
* Camera feedback mode
*
* Sets the camera feedback mode. This parameter will be moved once we
* support better camera feedback.
*
* @value 0 No camera feedback available
* @min 0
* @max 1
* @group Camera Control
*/
PARAM_DEFINE_INT32(CAM_FEEDBACK, 0);

1
src/modules/commander/commander.cpp
View File

@ -1226,6 +1226,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
case vehicle_command_s::VEHICLE_CMD_DO_LAND_START:
case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND:

4
src/modules/mavlink/mavlink_mission.cpp
View File

@ -1024,6 +1024,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_LAND_START:
case MAV_CMD_DO_TRIGGER_CONTROL:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_MOUNT_CONTROL:
@ -1034,6 +1035,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_ROI:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case MAV_CMD_DO_VTOL_TRANSITION:
case MAV_CMD_NAV_DELAY:
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
@ -1099,6 +1101,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
case NAV_CMD_DO_CHANGE_SPEED:
case NAV_CMD_DO_SET_SERVO:
case NAV_CMD_DO_LAND_START:
case NAV_CMD_DO_TRIGGER_CONTROL:
case NAV_CMD_DO_DIGICAM_CONTROL:
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
@ -1109,6 +1112,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_ROI:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case NAV_CMD_DO_VTOL_TRANSITION:
break;

4
src/modules/navigator/mission_block.cpp
View File

@ -73,6 +73,7 @@ bool
MissionBlock::is_mission_item_reached()
{
/* handle non-navigation or indefinite waypoints */
switch (_mission_item.nav_cmd) {
case NAV_CMD_DO_SET_SERVO:
return true;
@ -86,6 +87,7 @@ MissionBlock::is_mission_item_reached()
return false;
case NAV_CMD_DO_LAND_START:
case NAV_CMD_DO_TRIGGER_CONTROL:
case NAV_CMD_DO_DIGICAM_CONTROL:
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
@ -96,6 +98,7 @@ MissionBlock::is_mission_item_reached()
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_ROI:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
return true;
case NAV_CMD_DO_VTOL_TRANSITION:
@ -445,6 +448,7 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item,
void
MissionBlock::issue_command(const struct mission_item_s *item)
{
if (item_contains_position(item)) {
return;
}

2
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -280,6 +280,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t
missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
missionitem.nav_cmd != NAV_CMD_DO_LAND_START &&
missionitem.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
@ -290,6 +291,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
missionitem.nav_cmd != NAV_CMD_ROI &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1),

2
src/modules/navigator/navigation.h
View File

@ -78,9 +78,11 @@ enum NAV_CMD {
NAV_CMD_DO_DIGICAM_CONTROL = 203,
NAV_CMD_DO_MOUNT_CONFIGURE = 204,
NAV_CMD_DO_MOUNT_CONTROL = 205,
NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214,
NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206,
NAV_CMD_IMAGE_START_CAPTURE = 2000,
NAV_CMD_IMAGE_STOP_CAPTURE = 2001,
NAV_CMD_DO_TRIGGER_CONTROL = 2003,
NAV_CMD_VIDEO_START_CAPTURE = 2500,
NAV_CMD_VIDEO_STOP_CAPTURE = 2501,
NAV_CMD_DO_VTOL_TRANSITION = 3000,