C++ (Cpp) find_nearest_rally_point Examples

Example #1

File: AP_Rally.cpp Project: Benbenbeni/ardupilot-1

// return best RTL location from current position
Location AP_Rally::calc_best_rally_or_home_location(const Location &current_loc, float rtl_home_alt) const
{
    RallyLocation ral_loc = {};
    Location return_loc = {};
    const struct Location &home_loc = _ahrs.get_home();
    
    if (find_nearest_rally_point(current_loc, ral_loc)) {
        // valid rally point found
        return_loc = rally_location_to_location(ral_loc);
    } else {
        // no valid rally point, return home position
        return_loc = home_loc;
        return_loc.alt = rtl_home_alt;
        return_loc.flags.relative_alt = false; // read_alt_to_hold returns an absolute altitude
    }

    return return_loc;
}

Example #2

File: AP_Rally.cpp Project: KIrill-ka/ardupilot

// return best RTL location from current position
Location AP_Rally::calc_best_rally_or_home_location(const Location &current_loc, float rtl_home_alt) const
{
    RallyLocation ral_loc = {};
    Location return_loc = {};
    const struct Location &home_loc = AP::ahrs().get_home();
    
    // no valid rally point, return home position
    return_loc = home_loc;
    return_loc.alt = rtl_home_alt;
    return_loc.relative_alt = false; // read_alt_to_hold returns an absolute altitude

    if (find_nearest_rally_point(current_loc, ral_loc)) {
        Location loc = rally_location_to_location(ral_loc);
        // use the rally point if it's closer then home, or we aren't generally considering home as acceptable
        if (!_rally_incl_home  || (current_loc.get_distance(loc) < current_loc.get_distance(return_loc))) {
            return_loc = rally_location_to_location(ral_loc);
        }
    }

    return return_loc;
}