diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 8ae5614669..799a3db5e4 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -874,12 +874,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com return MAV_RESULT_FAILED; } +#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED // these are GUIDED mode commands that are RATE or slew enabled, so you can have more powerful control than default controls. MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet) { switch(packet.command) { - -#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED case MAV_CMD_GUIDED_CHANGE_SPEED: { // command is only valid in guided mode if (plane.control_mode != &plane.mode_guided) { @@ -1008,14 +1007,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl plane.guided_state.target_heading_time_ms = AP_HAL::millis(); return MAV_RESULT_ACCEPTED; } -#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED - - } // anything else ... return MAV_RESULT_UNSUPPORTED; - } +#endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { @@ -1027,11 +1023,13 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in case MAV_CMD_DO_REPOSITION: return handle_command_int_do_reposition(packet); +#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED // special 'slew-enabled' guided commands here... for speed,alt, and direction commands case MAV_CMD_GUIDED_CHANGE_SPEED: case MAV_CMD_GUIDED_CHANGE_ALTITUDE: case MAV_CMD_GUIDED_CHANGE_HEADING: return handle_command_int_guided_slew_commands(packet); +#endif #if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED case MAV_CMD_DO_FOLLOW: