Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Rally: Move handling of home out of find_nearest_rally_point

This commit is contained in:
Michael du Breuil 2018-12-28 20:55:25 -07:00 committed by Andrew Tridgell
parent c3de3cc923
commit 36cc66f0f3
1 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Rally/AP_Rally.cpp
View File

@ -118,7 +118,6 @@ Location AP_Rally::rally_location_to_location(const RallyLocation &rally_loc) co
bool AP_Rally::find_nearest_rally_point(const Location &current_loc, RallyLocation &return_loc) const
{
float min_dis = -1;
const struct Location &home_loc = _ahrs.get_home();
for (uint8_t i = 0; i < (uint8_t) _rally_point_total_count; i++) {
RallyLocation next_rally;
@ -134,13 +133,8 @@ bool AP_Rally::find_nearest_rally_point(const Location &current_loc, RallyLocati
}
}
// if home is included, return false (meaning use home) if it is closer than all rally points
if (_rally_incl_home && (get_distance(current_loc, home_loc) < min_dis)) {
return false;
}
// if a limit is defined and all rally points are beyond that limit, use home if it is closer
if ((_rally_limit_km > 0) && (min_dis > _rally_limit_km*1000.0f) && (get_distance(current_loc, home_loc) < min_dis)) {
if ((_rally_limit_km > 0) && (min_dis > _rally_limit_km*1000.0f)) {
return false; // use home position
}
@ -155,14 +149,17 @@ Location AP_Rally::calc_best_rally_or_home_location(const Location &current_loc,
Location return_loc = {};
const struct Location &home_loc = _ahrs.get_home();
// 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
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
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 || (get_distance(current_loc, loc) < get_distance(current_loc, return_loc))) {
return_loc = rally_location_to_location(ral_loc);
}
}
return return_loc;