diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 4325371c27..15c5dc6e7f 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -689,8 +689,6 @@ void Plane::update_flight_stage(void) // This prevents TECS from being stuck in the wrong stage if you switch from // AUTO to, say, FBWB during a landing, an aborted landing or takeoff. set_flight_stage(AP_FixedWing::FlightStage::NORMAL); - } else if (control_mode == &mode_autoland) { - set_flight_stage(AP_FixedWing::FlightStage::LAND); } return; } diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index 2150f690e4..a3931fb878 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/ArduPlane/mode_autoland.cpp @@ -43,7 +43,7 @@ ModeAutoLand::ModeAutoLand() : } bool ModeAutoLand::_enter() -{ +{ //must be flying to enter if (!plane.is_flying()) { gcs().send_text(MAV_SEVERITY_WARNING, "Must already be flying!"); @@ -52,8 +52,8 @@ bool ModeAutoLand::_enter() //takeoff direction must be set and must not be a quadplane, otherwise since flying switch to RTL so this can be used as FS action #if HAL_QUADPLANE_ENABLED - if (quadplane.available()) { - gcs().send_text(MAV_SEVERITY_WARNING, "AutoLand is fixed wing only mode"); + if (quadplane.available() && !quadplane.option_is_set(QuadPlane::OPTION::ALLOW_FW_LAND)) { + gcs().send_text(MAV_SEVERITY_WARNING, "Option not set to allow fixed wing autoland"); return false; } #endif @@ -105,6 +105,7 @@ void ModeAutoLand::navigate() plane.prev_WP_loc = plane.current_loc; plane.next_WP_loc = home_loc; plane.start_command(cmd); + plane.set_flight_stage(AP_FixedWing::FlightStage::LAND); } return; //otherwise keep flying the current command diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index e4489fd0d1..b68c2c27c7 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -62,6 +62,7 @@ public: friend class ModeQAutotune; friend class ModeQAcro; friend class ModeLoiterAltQLand; + friend class ModeAutoLand; QuadPlane(AP_AHRS &_ahrs);