void AP_Landing_Deepstall::build_approach_path(bool use_current_heading)
{
    float loiter_radius = landing.nav_controller->loiter_radius(landing.aparm.loiter_radius);

    Vector3f wind = landing.ahrs.wind_estimate();
    // TODO: Support a user defined approach heading
    target_heading_deg = use_current_heading ? landing.ahrs.yaw_sensor * 1e-2 : (degrees(atan2f(-wind.y, -wind.x)));

    memcpy(&extended_approach, &landing_point, sizeof(Location));
    memcpy(&arc_exit, &landing_point, sizeof(Location));

    //extend the approach point to 1km away so that there is always a navigational target
    location_update(extended_approach, target_heading_deg, 1000.0);

    float expected_travel_distance = predict_travel_distance(wind, is_zero(approach_alt_offset) ?  landing_point.alt * 0.01f : approach_alt_offset,
                                                             false);
    float approach_extension_m = expected_travel_distance + approach_extension;
    float loiter_radius_m_abs = fabsf(loiter_radius);
    // an approach extensions must be at least half the loiter radius, or the aircraft has a
    // decent chance to be misaligned on final approach
    approach_extension_m = MAX(approach_extension_m, loiter_radius_m_abs * 0.5f);

    location_update(arc_exit, target_heading_deg + 180, approach_extension_m);
    memcpy(&arc, &arc_exit, sizeof(Location));
    memcpy(&arc_entry, &arc_exit, sizeof(Location));

    float arc_heading_deg = target_heading_deg + (landing_point.flags.loiter_ccw ? -90.0f : 90.0f);
    location_update(arc, arc_heading_deg, loiter_radius_m_abs);
    location_update(arc_entry, arc_heading_deg, loiter_radius_m_abs * 2);

#ifdef DEBUG_PRINTS
    // TODO: Send this information via a MAVLink packet
    gcs().send_text(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f",
                                     (double)(arc.lat / 1e7),(double)( arc.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f",
                                     (double)(arc_entry.lat / 1e7), (double)(arc_entry.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f",
                                     (double)(arc_exit.lat / 1e7), (double)(arc_exit.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f",
                                     (double)(extended_approach.lat / 1e7), (double)(extended_approach.lng / 1e7));
    gcs().send_text(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m,
                                     (double)expected_travel_distance);
    gcs().send_text(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg);
#endif // DEBUG_PRINTS

}
Exemplo n.º 2
0
void AP_Landing_Deepstall::build_approach_path(void)
{
    float loiter_radius = landing.nav_controller->loiter_radius(landing.aparm.loiter_radius);

    Vector3f wind = landing.ahrs.wind_estimate();
    // TODO: Support a user defined approach heading
    target_heading_deg = (degrees(atan2f(-wind.y, -wind.x)));

    memcpy(&extended_approach, &landing_point, sizeof(Location));
    memcpy(&arc_exit, &landing_point, sizeof(Location));

    //extend the approach point to 1km away so that there is always a navigational target
    location_update(extended_approach, target_heading_deg, 1000.0);

    float expected_travel_distance = predict_travel_distance(wind, landing_point.alt * 0.01f, false);
    float approach_extension_m = expected_travel_distance + approach_extension;
    // an approach extensions must be at least half the loiter radius, or the aircraft has a
    // decent chance to be misaligned on final approach
    approach_extension_m = MAX(approach_extension_m, loiter_radius * 0.5f);

    location_update(arc_exit, target_heading_deg + 180, approach_extension_m);
    memcpy(&arc, &arc_exit, sizeof(Location));
    memcpy(&arc_entry, &arc_exit, sizeof(Location));

    // TODO: Support loitering on either side of the approach path
    location_update(arc, target_heading_deg + 90.0, loiter_radius);
    location_update(arc_entry, target_heading_deg + 90.0, loiter_radius * 2);

#ifdef DEBUG_PRINTS
    // TODO: Send this information via a MAVLink packet
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f",
                                     (double)(arc.lat / 1e7),(double)( arc.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f",
                                     (double)(arc_entry.lat / 1e7), (double)(arc_entry.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f",
                                     (double)(arc_exit.lat / 1e7), (double)(arc_exit.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f",
                                     (double)(extended_approach.lat / 1e7), (double)(extended_approach.lng / 1e7));
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m,
                                     (double)expected_travel_distance);
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg);
#endif // DEBUG_PRINTS

}
/*
  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) > fabsf(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
        FALLTHROUGH;
    case DEEPSTALL_STAGE_ESTIMATE_WIND:
        {
        landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, landing_point.flags.loiter_ccw ? -1 : 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);
        delta *= (landing_point.flags.loiter_ccw ? -1 : 1);
        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
            return false;
        }
        stage = DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT;
        loiter_sum_cd = 0; // reset the loiter counter
        FALLTHROUGH;
        }
    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(false);
        }
        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, landing_point.flags.loiter_ccw ? -1 : 1);
            return false;
        }
        stage = DEEPSTALL_STAGE_FLY_TO_ARC;
        memcpy(&breakout_location, &current_loc, sizeof(Location));
        FALLTHROUGH;
    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;
        FALLTHROUGH;
    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, landing_point.flags.loiter_ccw ? -1 : 1);
            return false;
        }
        stage = DEEPSTALL_STAGE_APPROACH;
        }
        FALLTHROUGH;
    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();
        }
        }
        FALLTHROUGH;
    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);
        landing.disarm_if_autoland_complete_fn();
        return false;
    default:
        return true;
    }
}