diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index a81ed5a46e1a1b..a096d6eccac8fe 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -960,6 +960,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_comm #if MODE_AUTO_ENABLED == ENABLED MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet) { + if (!is_zero(packet.param1) || !is_zero(packet.param2)) { + // first-item/last item not supported + return MAV_RESULT_DENIED; + } if (copter.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) { copter.set_auto_armed(true); if (copter.mode_auto.mission.state() != AP_Mission::MISSION_RUNNING) { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 799a3db5e48618..887599d6b985db 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1080,6 +1080,10 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in return MAV_RESULT_FAILED; case MAV_CMD_MISSION_START: + if (!is_zero(packet.param1) || !is_zero(packet.param2)) { + // first-item/last item not supported + return MAV_RESULT_DENIED; + } plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); return MAV_RESULT_ACCEPTED; diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 576de91217b18f..63fb05f821e477 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -542,6 +542,10 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_ return handle_command_int_do_reposition(packet); case MAV_CMD_MISSION_START: + if (!is_zero(packet.param1) || !is_zero(packet.param2)) { + // first-item/last item not supported + return MAV_RESULT_DENIED; + } return handle_MAV_CMD_MISSION_START(packet); case MAV_CMD_NAV_LOITER_UNLIM: diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index ff34f556018709..c62039167151a2 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -701,6 +701,10 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_in packet.param4); case MAV_CMD_MISSION_START: + if (!is_zero(packet.param1) || !is_zero(packet.param2)) { + // first-item/last item not supported + return MAV_RESULT_DENIED; + } if (rover.set_mode(rover.mode_auto, ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; }