diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index bf1ac6993e..929eae1bf9 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -428,10 +428,6 @@ private: bool verify_within_distance(); void do_change_speed(const AP_Mission::Mission_Command& cmd); void do_set_home(const AP_Mission::Mission_Command& cmd); -#if CAMERA == ENABLED - void do_digicam_configure(const AP_Mission::Mission_Command& cmd); - void do_digicam_control(const AP_Mission::Mission_Command& cmd); -#endif void do_set_reverse(const AP_Mission::Mission_Command& cmd); // commands.cpp diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index f6d7f245d1..c44d72aed8 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -59,23 +59,6 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) do_set_home(cmd); break; -#if CAMERA == ENABLED - 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)| - do_digicam_configure(cmd); - 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| - do_digicam_control(cmd); - break; - - case MAV_CMD_DO_SET_CAM_TRIGG_DIST: - camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters); - break; -#endif - #if MOUNT == ENABLED // Sets the region of interest (ROI) for a sensor set or the // vehicle itself. This can then be used by the vehicles control @@ -176,8 +159,6 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_SET_HOME: 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: case MAV_CMD_DO_SET_ROI: case MAV_CMD_DO_SET_REVERSE: @@ -367,33 +348,6 @@ void Rover::do_set_home(const AP_Mission::Mission_Command& cmd) } } -#if CAMERA == ENABLED - -// do_digicam_configure Send Digicam Configure message with the camera library -void Rover::do_digicam_configure(const AP_Mission::Mission_Command& cmd) -{ - 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); -} - -// do_digicam_control Send Digicam Control message with the camera library -void Rover::do_digicam_control(const AP_Mission::Mission_Command& cmd) -{ - 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); -} - -#endif - void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd) { control_mode->set_reversed(cmd.p1 == 1);