mirror of https://github.com/ArduPilot/ardupilot
AP_Rally: Move handling of home out of find_nearest_rally_point
This commit is contained in:
parent
c3de3cc923
commit
36cc66f0f3
|
@ -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 ¤t_loc, RallyLocation &return_loc) const
|
bool AP_Rally::find_nearest_rally_point(const Location ¤t_loc, RallyLocation &return_loc) const
|
||||||
{
|
{
|
||||||
float min_dis = -1;
|
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++) {
|
for (uint8_t i = 0; i < (uint8_t) _rally_point_total_count; i++) {
|
||||||
RallyLocation next_rally;
|
RallyLocation next_rally;
|
||||||
|
@ -134,13 +133,8 @@ bool AP_Rally::find_nearest_rally_point(const Location ¤t_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 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
|
return false; // use home position
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -155,14 +149,17 @@ Location AP_Rally::calc_best_rally_or_home_location(const Location ¤t_loc,
|
||||||
Location return_loc = {};
|
Location return_loc = {};
|
||||||
const struct Location &home_loc = _ahrs.get_home();
|
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)) {
|
if (find_nearest_rally_point(current_loc, ral_loc)) {
|
||||||
// valid rally point found
|
Location loc = rally_location_to_location(ral_loc);
|
||||||
return_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
|
||||||
} else {
|
if (!_rally_incl_home || (get_distance(current_loc, loc) < get_distance(current_loc, return_loc))) {
|
||||||
// no valid rally point, return home position
|
return_loc = rally_location_to_location(ral_loc);
|
||||||
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;
|
return return_loc;
|
||||||
|
|
Loading…
Reference in New Issue