diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 0bf353eba3..8697fb897e 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -268,6 +268,10 @@ bool AP_Mission::verify_command(const Mission_Command& cmd) case MAV_CMD_DO_SET_RELAY: case MAV_CMD_DO_REPEAT_SERVO: case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CONTROL_VIDEO: + case MAV_CMD_DO_DIGICAM_CONFIGURE: + case MAV_CMD_DO_DIGICAM_CONTROL: + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: return true; default: return _cmd_verify_fn(cmd); @@ -284,6 +288,11 @@ bool AP_Mission::start_command(const Mission_Command& cmd) case MAV_CMD_DO_REPEAT_SERVO: case MAV_CMD_DO_REPEAT_RELAY: return start_command_do_servorelayevents(cmd); + case MAV_CMD_DO_CONTROL_VIDEO: + case MAV_CMD_DO_DIGICAM_CONFIGURE: + case MAV_CMD_DO_DIGICAM_CONTROL: + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + return start_command_camera(cmd); default: return _cmd_start_fn(cmd); } diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 30a8a86458..696d2ee458 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -582,6 +582,7 @@ private: // mission items common to all vehicles: bool start_command_do_gripper(const AP_Mission::Mission_Command& cmd); bool start_command_do_servorelayevents(const AP_Mission::Mission_Command& cmd); + bool start_command_camera(const AP_Mission::Mission_Command& cmd); }; namespace AP { diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index ae99727dac..ea680dbf82 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -66,3 +66,47 @@ bool AP_Mission::start_command_do_servorelayevents(const AP_Mission::Mission_Com return true; } + +bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd) +{ + AP_Camera *camera = AP::camera(); + if (camera == nullptr) { + return true; + } + + switch (cmd.id) { + case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| + break; + + case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| + camera->configure( + cmd.content.digicam_configure.shooting_mode, + cmd.content.digicam_configure.shutter_speed, + cmd.content.digicam_configure.aperture, + cmd.content.digicam_configure.ISO, + cmd.content.digicam_configure.exposure_type, + cmd.content.digicam_configure.cmd_id, + cmd.content.digicam_configure.engine_cutoff_time); + break; + + case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| + camera->control( + cmd.content.digicam_control.session, + cmd.content.digicam_control.zoom_pos, + cmd.content.digicam_control.zoom_step, + cmd.content.digicam_control.focus_lock, + cmd.content.digicam_control.shooting_cmd, + cmd.content.digicam_control.cmd_id); + break; + + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + camera->set_trigger_distance(cmd.content.cam_trigg_dist.meters); + break; + + default: + return true; + } + + return true; +} +