diff --git a/msg/camera_trigger.msg b/msg/camera_trigger.msg index 20eacded55..b50d1a5a5e 100644 --- a/msg/camera_trigger.msg +++ b/msg/camera_trigger.msg @@ -1,4 +1,3 @@ +uint64 timestamp_utc # UTC timestamp + uint32 seq # Image sequence number - -uint32 ORB_QUEUE_LENGTH = 3 - diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 08850f4d95..9a81f0992c 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -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| diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 4a449652bc..ac453cf502 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -57,10 +57,12 @@ #include #include +#include #include #include #include #include +#include #include @@ -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(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(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(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(); diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index a4fefe7731..9e4a60501e 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -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); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a7e97ebed4..3a9e51a47b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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: diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index faf2785109..e3ba947e60 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -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; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 488cc07400..97ac161e97 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -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; } diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index fe0a6375f3..d309df02e3 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -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), diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 21ccf1026f..480a9ce654 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -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,