diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp index da8cf3af9bef74..31076733e79542 100644 --- a/ArduPlane/mode_LoiterAltQLand.cpp +++ b/ArduPlane/mode_LoiterAltQLand.cpp @@ -19,10 +19,10 @@ bool ModeLoiterAltQLand::_enter() // at this point plane.next_WP_loc == current_loc const Location loiter_wp = plane.previous_mode->is_guided_mode() ? previous_wp : plane.next_WP_loc; - AP_Mission::Mission_Command cmd {}; - cmd.content.location = loiter_wp; + // AP_Mission::Mission_Command cmd {}; + // cmd.content.location = loiter_wp; - plane.do_landing_vtol_approach(cmd); + // plane.do_landing_vtol_approach(cmd); handle_guided_request(loiter_wp); @@ -35,25 +35,34 @@ bool ModeLoiterAltQLand::_enter() void ModeLoiterAltQLand::navigate() { - AP_Mission::Mission_Command cmd {}; - cmd.content.location = plane.next_WP_loc; - plane.verify_landing_vtol_approach(cmd); + // AP_Mission::Mission_Command cmd {}; + // cmd.content.location = plane.next_WP_loc; + // plane.verify_landing_vtol_approach(cmd); - // switch_qland(); + //switch_qland(); ModeLoiter::navigate(); + + + ftype dist; + if ((!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) && plane.nav_controller->reached_loiter_target()) { + const Location loiter_wp = plane.next_WP_loc; + plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND); + plane.set_next_WP(loiter_wp); + poscontrol.set_state(QuadPlane::QPOS_AIRBRAKE); + } } void ModeLoiterAltQLand::switch_qland() { - // if (!has_reached_transition_altitude) { - // ftype dist; - // if ((!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) && plane.nav_controller->reached_loiter_target()) { - // // we only need to satisfy the altitude requirement once - // has_reached_transition_altitude = true; - // gcs().send_text(MAV_SEVERITY_INFO, "ALT reached %.2f", plane.current_loc.alt*0.01); - // } - // } + if (!has_reached_transition_altitude) { + ftype dist; + if ((!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) && plane.nav_controller->reached_loiter_target()) { + // we only need to satisfy the altitude requirement once + has_reached_transition_altitude = true; + gcs().send_text(MAV_SEVERITY_INFO, "ALT reached %.2f", plane.current_loc.alt*0.01); + } + } // if (has_reached_transition_altitude) {