diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index b9adc7e663..aa705b4229 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -934,13 +934,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_DIGICAM_CONTROL: - rover.camera.control(packet.param1, - packet.param2, - packet.param3, - packet.param4, - packet.param5, - packet.param6); - + if (rover.camera.control(packet.param1, + packet.param2, + packet.param3, + packet.param4, + packet.param5, + packet.param6)) { + rover.log_picture(); + } result = MAV_RESULT_ACCEPTED; break; #endif // CAMERA == ENABLED diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index a20f6f7258..1efbb452ff 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -348,13 +348,14 @@ void Rover::do_digicam_configure(const AP_Mission::Mission_Command& cmd) void Rover::do_digicam_control(const AP_Mission::Mission_Command& cmd) { #if CAMERA == ENABLED - 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); - log_picture(); + if (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)) { + log_picture(); + } #endif }