Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_MISSION: Add missing do nothing commands mavsdk compatability #24702

Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 21 additions & 0 deletions libraries/AP_Mission/AP_Mission.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -376,8 +376,10 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
case MAV_CMD_DO_AUX_FUNCTION:
case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
case MAV_CMD_JUMP_TAG:
case MAV_CMD_IMAGE_START_CAPTURE:
case MAV_CMD_IMAGE_STOP_CAPTURE:
case MAV_CMD_SET_CAMERA_ZOOM:
case MAV_CMD_SET_CAMERA_FOCUS:
case MAV_CMD_VIDEO_START_CAPTURE:
Expand Down Expand Up @@ -422,6 +424,7 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_IMAGE_START_CAPTURE:
case MAV_CMD_IMAGE_STOP_CAPTURE:
case MAV_CMD_SET_CAMERA_ZOOM:
case MAV_CMD_SET_CAMERA_FOCUS:
case MAV_CMD_VIDEO_START_CAPTURE:
Expand All @@ -438,6 +441,8 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
return command_do_set_repeat_dist(cmd);
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
return start_command_do_gimbal_manager_pitchyaw(cmd);
case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: // Does nothing as current ArduPilot codebase does not check for who has gimbal control
return true;
case MAV_CMD_JUMP_TAG:
_jump_tag.tag = cmd.content.jump.target;
_jump_tag.age = 1;
Expand Down Expand Up @@ -1317,6 +1322,9 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
cmd.content.image_start_capture.start_seq_number = packet.param4;
break;

case MAV_CMD_IMAGE_STOP_CAPTURE: // Does nothing as current ArduPilot codebase has not implemented this command
break;

case MAV_CMD_SET_CAMERA_ZOOM:
cmd.content.set_camera_zoom.zoom_type = packet.param1;
cmd.content.set_camera_zoom.zoom_value = packet.param2;
Expand All @@ -1335,6 +1343,9 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
cmd.content.video_stop_capture.video_stream_id = packet.param1;
break;

case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: // MAV ID: 1001
break; // Does nothing as current ArduPilot codebase does not check for who has gimbal control

default:
// unrecognised command
return MAV_MISSION_UNSUPPORTED;
Expand Down Expand Up @@ -1830,6 +1841,9 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
packet.param4 = cmd.content.image_start_capture.start_seq_number;
break;

case MAV_CMD_IMAGE_STOP_CAPTURE: // Does nothing as current ArduPilot codebase has not implemented this command
break;

case MAV_CMD_SET_CAMERA_ZOOM:
packet.param1 = cmd.content.set_camera_zoom.zoom_type;
packet.param2 = cmd.content.set_camera_zoom.zoom_value;
Expand All @@ -1848,6 +1862,9 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
packet.param1 = cmd.content.video_stop_capture.video_stream_id;
break;

case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: // MAV ID: 1001
break; // Does nothing as current ArduPilot codebase does not check for who has gimbal control

default:
// unrecognised command
return false;
Expand Down Expand Up @@ -2651,8 +2668,12 @@ const char *AP_Mission::Mission_Command::type() const
return "PauseContinue";
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
return "GimbalPitchYaw";
case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
return "GimbalConfigure";
case MAV_CMD_IMAGE_START_CAPTURE:
return "ImageStartCapture";
case MAV_CMD_IMAGE_STOP_CAPTURE:
return "ImageStopCapture";
case MAV_CMD_SET_CAMERA_ZOOM:
return "SetCameraZoom";
case MAV_CMD_SET_CAMERA_FOCUS:
Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_Mission/AP_Mission_Commands.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -166,6 +166,9 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
camera->take_picture();
return true;

case MAV_CMD_IMAGE_STOP_CAPTURE: // Does nothing as current ArduPilot codebase has not implemented this command
return true;

case MAV_CMD_VIDEO_START_CAPTURE:
case MAV_CMD_VIDEO_STOP_CAPTURE:
{
Expand Down
Loading