Exemplo n.º 1
0
/*
  a special glide slope calculation for the landing approach

  During the land approach use a linear glide slope to a point
  projected through the landing point. We don't use the landing point
  itself as that leads to discontinuities close to the landing point,
  which can lead to erratic pitch control
 */
void Plane::setup_landing_glide_slope(void)
{
        Location loc = next_WP_loc;

        // project a point 500 meters past the landing point, passing
        // through the landing point
        const float land_projection = 500;        
        int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
        float total_distance = get_distance(prev_WP_loc, next_WP_loc);

        // If someone mistakenly puts all 0's in their LAND command then total_distance
        // will be calculated as 0 and cause a divide by 0 error below.  Lets avoid that.
        if (total_distance < 1) {
            total_distance = 1;
        }

        // height we need to sink for this WP
        float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f;

        // current ground speed
        float groundspeed = ahrs.groundspeed();
        if (groundspeed < 0.5f) {
            groundspeed = 0.5f;
        }

        // calculate time to lose the needed altitude
        float sink_time = total_distance / groundspeed;
        if (sink_time < 0.5f) {
            sink_time = 0.5f;
        }

        // find the sink rate needed for the target location
        float sink_rate = sink_height / sink_time;

        // the height we aim for is the one to give us the right flare point
        float aim_height = aparm.land_flare_sec * sink_rate;
        if (aim_height <= 0) {
            aim_height = g.land_flare_alt;
        } 
            
        // don't allow the aim height to be too far above LAND_FLARE_ALT
        if (g.land_flare_alt > 0 && aim_height > g.land_flare_alt*2) {
            aim_height = g.land_flare_alt*2;
        }

        // time before landing that we will flare
        float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate();

        // distance to flare is based on ground speed, adjusted as we
        // get closer. This takes into account the wind
        float flare_distance = groundspeed * flare_time;
        
        // don't allow the flare before half way along the final leg
        if (flare_distance > total_distance/2) {
            flare_distance = total_distance/2;
        }

        // now calculate our aim point, which is before the landing
        // point and above it
        location_update(loc, land_bearing_cd*0.01f, -flare_distance);
        loc.alt += aim_height*100;

        // calculate slope to landing point
        float land_slope = (sink_height - aim_height) / total_distance;

        // calculate point along that slope 500m ahead
        location_update(loc, land_bearing_cd*0.01f, land_projection);
        loc.alt -= land_slope * land_projection * 100;

        // setup the offset_cm for set_target_altitude_proportion()
        target_altitude.offset_cm = loc.alt - prev_WP_loc.alt;

        // calculate the proportion we are to the target
        float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc);

        // now setup the glide slope for landing
        set_target_altitude_proportion(loc, 1.0f - land_proportion);

        // stay within the range of the start and end locations in altitude
        constrain_target_altitude_location(loc, prev_WP_loc);
}
Exemplo n.º 2
0
/*
  update navigation for landing. Called when on landing approach or
  final flare
 */
bool Plane::verify_land()
{
    // we don't 'verify' landing in the sense that it never completes,
    // so we don't verify command completion. Instead we use this to
    // adjust final landing parameters

    // If a go around has been commanded, we are done landing.  This will send
    // the mission to the next mission item, which presumably is a mission
    // segment with operations to perform when a landing is called off.
    // If there are no commands after the land waypoint mission item then
    // the plane will proceed to loiter about its home point.
    if (auto_state.commanded_go_around) {
        return true;
    }

    float height = height_above_target();

    // use rangefinder to correct if possible
    height -= rangefinder_correction();

    /* Set land_complete (which starts the flare) under 3 conditions:
       1) we are within LAND_FLARE_ALT meters of the landing altitude
       2) we are within LAND_FLARE_SEC of the landing point vertically
          by the calculated sink rate (if LAND_FLARE_SEC != 0)
       3) we have gone past the landing point and don't have
          rangefinder data (to prevent us keeping throttle on 
          after landing if we've had positive baro drift)
    */
#if RANGEFINDER_ENABLED == ENABLED
    bool rangefinder_in_range = rangefinder_state.in_range;
#else
    bool rangefinder_in_range = false;
#endif
    if (height <= g.land_flare_alt ||
        (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec) ||
        (!rangefinder_in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) ||
        (fabsf(auto_state.sink_rate) < 0.2f && !is_flying())) {

        if (!auto_state.land_complete) {
            auto_state.post_landing_stats = true;
            if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
                gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), (double)gps.ground_speed());
            } else {
                gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"), 
                        (double)height, (double)auto_state.sink_rate, (double)gps.ground_speed());
            }
        }
        auto_state.land_complete = true;

        if (gps.ground_speed() < 3) {
            // reload any airspeed or groundspeed parameters that may have
            // been set for landing. We don't do this till ground
            // speed drops below 3.0 m/s as otherwise we will change
            // target speeds too early.
            g.airspeed_cruise_cm.load();
            g.min_gndspeed_cm.load();
            aparm.throttle_cruise.load();
        }
    }

    /*
      when landing we keep the L1 navigation waypoint 200m ahead. This
      prevents sudden turns if we overshoot the landing point
     */
    struct Location land_WP_loc = next_WP_loc;
	int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
    location_update(land_WP_loc,
                    land_bearing_cd*0.01f, 
                    get_distance(prev_WP_loc, current_loc) + 200);
    nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);

    // once landed and stationary, post some statistics
    // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
    if (auto_state.post_landing_stats && !arming.is_armed()) {
        auto_state.post_landing_stats = false;
        gcs_send_text_fmt(PSTR("Distance from LAND point=%.2fm"), (double)get_distance(current_loc, next_WP_loc));
    }

    // check if we should auto-disarm after a confirmed landing
    disarm_if_autoland_complete();

    /*
      we return false as a landing mission item never completes

      we stay on this waypoint unless the GCS commands us to change
      mission item or reset the mission, or a go-around is commanded
     */
    return false;
}
Exemplo n.º 3
0
/*
  update navigation for landing. Called when on landing approach or
  final flare
 */
bool Plane::verify_land()
{
    // we don't 'verify' landing in the sense that it never completes,
    // so we don't verify command completion. Instead we use this to
    // adjust final landing parameters

    // when aborting a landing, mimic the verify_takeoff with steering hold. Once
    // the altitude has been reached, restart the landing sequence
    if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {

        throttle_suppressed = false;
        auto_state.land_complete = false;
        auto_state.land_pre_flare = false;
        nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));

        // see if we have reached abort altitude
        if (adjusted_relative_altitude_cm() > auto_state.takeoff_altitude_rel_cm) {
            next_WP_loc = current_loc;
            mission.stop();
            bool success = restart_landing_sequence();
            mission.resume();
            if (!success) {
                // on a restart failure lets RTL or else the plane may fly away with nowhere to go!
                set_mode(RTL, MODE_REASON_MISSION_END);
            }
            // make sure to return false so it leaves the mission index alone
        }
        return false;
    }

    float height = height_above_target();

    // use rangefinder to correct if possible
    height -= rangefinder_correction();

    /* Set land_complete (which starts the flare) under 3 conditions:
       1) we are within LAND_FLARE_ALT meters of the landing altitude
       2) we are within LAND_FLARE_SEC of the landing point vertically
          by the calculated sink rate (if LAND_FLARE_SEC != 0)
       3) we have gone past the landing point and don't have
          rangefinder data (to prevent us keeping throttle on 
          after landing if we've had positive baro drift)
    */
#if RANGEFINDER_ENABLED == ENABLED
    bool rangefinder_in_range = rangefinder_state.in_range;
#else
    bool rangefinder_in_range = false;
#endif

    // flare check:
    // 1) below flare alt/sec requires approach stage check because if sec/alt are set too
    //    large, and we're on a hard turn to line up for approach, we'll prematurely flare by
    //    skipping approach phase and the extreme roll limits will make it hard to line up with runway
    // 2) passed land point and don't have an accurate AGL
    // 3) probably crashed (ensures motor gets turned off)

    bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
                              flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE);
    bool below_flare_alt = (height <= g.land_flare_alt);
    bool below_flare_sec = (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec);
    bool probably_crashed = (g.crash_detection_enable && fabsf(auto_state.sink_rate) < 0.2f && !is_flying());

    if ((on_approach_stage && below_flare_alt) ||
        (on_approach_stage && below_flare_sec && (auto_state.wp_proportion > 0.5)) ||
        (!rangefinder_in_range && auto_state.wp_proportion >= 1) ||
        probably_crashed) {

        if (!auto_state.land_complete) {
            auto_state.post_landing_stats = true;
            if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
                gcs_send_text_fmt(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed());
            } else {
                gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f",
                                  (double)height, (double)auto_state.sink_rate,
                                  (double)gps.ground_speed(),
                                  (double)get_distance(current_loc, next_WP_loc));
            }
            auto_state.land_complete = true;
            update_flight_stage();
        }


        if (gps.ground_speed() < 3) {
            // reload any airspeed or groundspeed parameters that may have
            // been set for landing. We don't do this till ground
            // speed drops below 3.0 m/s as otherwise we will change
            // target speeds too early.
            g.airspeed_cruise_cm.load();
            g.min_gndspeed_cm.load();
            aparm.throttle_cruise.load();
        }
    } else if (!auto_state.land_complete && !auto_state.land_pre_flare && aparm.land_pre_flare_airspeed > 0) {
        bool reached_pre_flare_alt = g.land_pre_flare_alt > 0 && (height <= g.land_pre_flare_alt);
        bool reached_pre_flare_sec = g.land_pre_flare_sec > 0 && (height <= auto_state.sink_rate * g.land_pre_flare_sec);
        if (reached_pre_flare_alt || reached_pre_flare_sec) {
            auto_state.land_pre_flare = true;
            update_flight_stage();
        }
    }

    /*
      when landing we keep the L1 navigation waypoint 200m ahead. This
      prevents sudden turns if we overshoot the landing point
     */
    struct Location land_WP_loc = next_WP_loc;
	int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
    location_update(land_WP_loc,
                    land_bearing_cd*0.01f, 
                    get_distance(prev_WP_loc, current_loc) + 200);
    nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);

    // once landed and stationary, post some statistics
    // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
    if (auto_state.post_landing_stats && !arming.is_armed()) {
        auto_state.post_landing_stats = false;
        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc));
    }

    // check if we should auto-disarm after a confirmed landing
    disarm_if_autoland_complete();

    /*
      we return false as a landing mission item never completes

      we stay on this waypoint unless the GCS commands us to change
      mission item, reset the mission, command a go-around or finish
      a land_abort procedure.
     */
    return false;
}
Exemplo n.º 4
0
/*
  check that we have fetched all mission terrain data
 */
void AP_Terrain::update_mission_data(void)
{
    if (last_mission_change_ms != mission.last_change_time_ms() ||
        last_mission_spacing != grid_spacing) {
        // the mission has changed - start again
        next_mission_index = 1;
        next_mission_pos = 0;
        last_mission_change_ms = mission.last_change_time_ms();
        last_mission_spacing = grid_spacing;
    }
    if (next_mission_index == 0) {
        // nothing to do
        return;
    }

    uint16_t pending, loaded;
    get_statistics(pending, loaded);
    if (pending && ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
        // wait till we have fully filled the current set of grids
        return;
    }

    // don't do more than 20 waypoints at a time, to prevent too much
    // CPU usage
    for (uint8_t i=0; i<20; i++) {
        // get next mission command
        AP_Mission::Mission_Command cmd;
        if (!mission.read_cmd_from_storage(next_mission_index, cmd)) {
            // nothing more to do
            next_mission_index = 0;
            return;
        }

        // we only want nav waypoint commands. That should be enough to
        // prefill the terrain data and makes many things much simpler
        while ((cmd.id != MAV_CMD_NAV_WAYPOINT &&
                cmd.id != MAV_CMD_NAV_SPLINE_WAYPOINT) ||
               (cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) {
            next_mission_index++;
            if (!mission.read_cmd_from_storage(next_mission_index, cmd)) {
                // nothing more to do
                next_mission_index = 0;
                next_mission_pos = 0;
                return;
            }
        }

        // we will fetch 5 points around the waypoint. Four at 10 grid
        // spacings away at 45, 135, 225 and 315 degrees, and the
        // point itself
        if (next_mission_pos != 4) {
            location_update(cmd.content.location, 45+90*next_mission_pos, grid_spacing.get() * 10);
        }

        // we have a mission command to check
        float height;
        if (!height_amsl(cmd.content.location, height)) {
            // if we can't get data for a mission item then return and
            // check again next time
            return;
        }

        next_mission_pos++;
        if (next_mission_pos == 5) {
#if TERRAIN_DEBUG
            hal.console->printf("checked waypoint %u\n", (unsigned)next_mission_index);
#endif

            // move to next waypoint
            next_mission_index++;
            next_mission_pos = 0;
        }
    }
}
Exemplo n.º 5
0
/*
  update navigation for landing. Called when on landing approach or
  final flare
 */
bool AP_Landing::type_slope_verify_land(const AP_SpdHgtControl::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
        const int32_t auto_state_takeoff_altitude_rel_cm, const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range, bool &throttle_suppressed)
{
    // we don't 'verify' landing in the sense that it never completes,
    // so we don't verify command completion. Instead we use this to
    // adjust final landing parameters

    // when aborting a landing, mimic the verify_takeoff with steering hold. Once
    // the altitude has been reached, restart the landing sequence
    if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {

        throttle_suppressed = false;


        complete = false;
        pre_flare = false;
        nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));

        // see if we have reached abort altitude
        if (adjusted_relative_altitude_cm_fn() > auto_state_takeoff_altitude_rel_cm) {
            next_WP_loc = current_loc;
            mission.stop();
            if (restart_landing_sequence()) {
                mission.resume();
            }
            // else we're in AUTO with a stopped mission and handle_auto_mode() will set RTL
        }
        // make sure to return false so it leaves the mission index alone
        return false;
    }


    /* Set land_complete (which starts the flare) under 3 conditions:
       1) we are within LAND_FLARE_ALT meters of the landing altitude
       2) we are within LAND_FLARE_SEC of the landing point vertically
          by the calculated sink rate (if LAND_FLARE_SEC != 0)
       3) we have gone past the landing point and don't have
          rangefinder data (to prevent us keeping throttle on
          after landing if we've had positive baro drift)
    */

    // flare check:
    // 1) below flare alt/sec requires approach stage check because if sec/alt are set too
    //    large, and we're on a hard turn to line up for approach, we'll prematurely flare by
    //    skipping approach phase and the extreme roll limits will make it hard to line up with runway
    // 2) passed land point and don't have an accurate AGL
    // 3) probably crashed (ensures motor gets turned off)

    bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
                              flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE);
    bool below_flare_alt = (height <= flare_alt);
    bool below_flare_sec = (flare_sec > 0 && height <= sink_rate * flare_sec);
    bool probably_crashed = (aparm.crash_detection_enable && fabsf(sink_rate) < 0.2f && !is_flying);

    if ((on_approach_stage && below_flare_alt) ||
        (on_approach_stage && below_flare_sec && (wp_proportion > 0.5)) ||
        (!rangefinder_state_in_range && wp_proportion >= 1) ||
        probably_crashed) {

        if (!complete) {
            post_stats = true;
            if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)ahrs.get_gps().ground_speed());
            } else {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f",
                                  (double)height, (double)sink_rate,
                                  (double)ahrs.get_gps().ground_speed(),
                                  (double)get_distance(current_loc, next_WP_loc));
            }
            complete = true;
            update_flight_stage_fn();
        }


        if (ahrs.get_gps().ground_speed() < 3) {
            // reload any airspeed or groundspeed parameters that may have
            // been set for landing. We don't do this till ground
            // speed drops below 3.0 m/s as otherwise we will change
            // target speeds too early.
            aparm.airspeed_cruise_cm.load();
            aparm.min_gndspeed_cm.load();
            aparm.throttle_cruise.load();
        }
    } else if (!complete && !pre_flare && pre_flare_airspeed > 0) {
        bool reached_pre_flare_alt = pre_flare_alt > 0 && (height <= pre_flare_alt);
        bool reached_pre_flare_sec = pre_flare_sec > 0 && (height <= sink_rate * pre_flare_sec);
        if (reached_pre_flare_alt || reached_pre_flare_sec) {
            pre_flare = true;
            update_flight_stage_fn();
        }
    }

    /*
      when landing we keep the L1 navigation waypoint 200m ahead. This
      prevents sudden turns if we overshoot the landing point
     */
    struct Location land_WP_loc = next_WP_loc;
    int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
    location_update(land_WP_loc,
                    land_bearing_cd*0.01f,
                    get_distance(prev_WP_loc, current_loc) + 200);
    nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);

    // once landed and stationary, post some statistics
    // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
    if (post_stats && !is_armed) {
        post_stats = false;
        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc));
    }

    // check if we should auto-disarm after a confirmed landing
    disarm_if_autoland_complete_fn();

    /*
      we return false as a landing mission item never completes

      we stay on this waypoint unless the GCS commands us to change
      mission item, reset the mission, command a go-around or finish
      a land_abort procedure.
     */
    return false;
}
Exemplo n.º 6
0
/*
  update navigation for landing
 */
bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
        const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms,
        const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range)
{
    switch (stage) {
    case DEEPSTALL_STAGE_FLY_TO_LANDING:
        if (get_distance(current_loc, landing_point) > 2 * landing.aparm.loiter_radius) {
            landing.nav_controller->update_waypoint(current_loc, landing_point);
            return false;
        }
        stage = DEEPSTALL_STAGE_ESTIMATE_WIND;
        loiter_sum_cd = 0; // reset the loiter counter
        // no break
    case DEEPSTALL_STAGE_ESTIMATE_WIND:
        {
        landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, 1);
        if (!landing.nav_controller->reached_loiter_target() || (fabsf(height) > DEEPSTALL_LOITER_ALT_TOLERANCE)) {
            // wait until the altitude is correct before considering a breakout
            return false;
        }
        // only count loiter progress when within the target altitude
        int32_t target_bearing = landing.nav_controller->target_bearing_cd();
        int32_t delta = wrap_180_cd(target_bearing - last_target_bearing);
        if (delta > 0) { // only accumulate turns in the correct direction
            loiter_sum_cd += delta;
        }
        last_target_bearing = target_bearing;
        if (loiter_sum_cd < 36000) {
            // wait until we've done at least one complete loiter at the correct altitude
            landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, 1);
            return false;
        }
        stage = DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT;
        loiter_sum_cd = 0; // reset the loiter counter
        // no break
        }
    case DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT:
        // rebuild the approach path if we have done less then a full circle to allow it to be
        // more into the wind as the estimator continues to refine itself, and allow better
        // compensation on windy days. This is limited to a single full circle though, as on
        // a no wind day you could be in this loop forever otherwise.
        if (loiter_sum_cd < 36000) {
            build_approach_path();
        }
        if (!verify_breakout(current_loc, arc_entry, height)) {
            int32_t target_bearing = landing.nav_controller->target_bearing_cd();
            int32_t delta = wrap_180_cd(target_bearing - last_target_bearing);
            if (delta > 0) { // only accumulate turns in the correct direction
                loiter_sum_cd += delta;
            }
            last_target_bearing = target_bearing;
            landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, 1);
            return false;
        }
        stage = DEEPSTALL_STAGE_FLY_TO_ARC;
        memcpy(&breakout_location, &current_loc, sizeof(Location));
        // no break
    case DEEPSTALL_STAGE_FLY_TO_ARC:
        if (get_distance(current_loc, arc_entry) > 2 * landing.aparm.loiter_radius) {
            landing.nav_controller->update_waypoint(breakout_location, arc_entry);
            return false;
        }
        stage = DEEPSTALL_STAGE_ARC;
        // no break
    case DEEPSTALL_STAGE_ARC:
        {
        Vector2f groundspeed = landing.ahrs.groundspeed_vector();
        if (!landing.nav_controller->reached_loiter_target() ||
            (fabsf(wrap_180(target_heading_deg -
                            degrees(atan2f(-groundspeed.y, -groundspeed.x) + M_PI))) >= 10.0f)) {
            landing.nav_controller->update_loiter(arc, landing.aparm.loiter_radius, 1);
            return false;
        }
        stage = DEEPSTALL_STAGE_APPROACH;
        }
        // no break
    case DEEPSTALL_STAGE_APPROACH:
        {
        Location entry_point;
        landing.nav_controller->update_waypoint(arc_exit, extended_approach);

        float relative_alt_D;
        landing.ahrs.get_relative_position_D_home(relative_alt_D);

        const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, false);

        memcpy(&entry_point, &landing_point, sizeof(Location));
        location_update(entry_point, target_heading_deg + 180.0, travel_distance);

        if (!location_passed_point(current_loc, arc_exit, entry_point)) {
            if (location_passed_point(current_loc, arc_exit, extended_approach)) {
                // this should never happen, but prevent against an indefinite fly away
                stage = DEEPSTALL_STAGE_FLY_TO_LANDING;
            }
            return false;
        }
        predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, true);
        stage = DEEPSTALL_STAGE_LAND;
        stall_entry_time = AP_HAL::millis();

        const SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator);
        if (elevator != nullptr) {
            // take the last used elevator angle as the starting deflection
            // don't worry about bailing here if the elevator channel can't be found
            // that will be handled within override_servos
            initial_elevator_pwm = elevator->get_output_pwm();
        }
        L1_xtrack_i = 0; // reset the integrators
        }
        // no break
    case DEEPSTALL_STAGE_LAND:
        // while in deepstall the only thing verify needs to keep the extended approach point sufficently far away
        landing.nav_controller->update_waypoint(current_loc, extended_approach);
        return false;
    default:
        return true;
    }
}