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

AP_Rally: tidy calculation of nearest rally/home

This commit is contained in:
Peter Barker 2022-06-30 13:11:45 +10:00 committed by Andrew Tridgell
parent 4d4a1ddd42
commit a2a63e14b1
1 changed files with 4 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Rally/AP_Rally.cpp
View File

@ -169,15 +169,11 @@ bool AP_Rally::find_nearest_rally_point(const Location &current_loc, RallyLocati
// 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 no valid rally point, return home position:
Location return_loc { AP::ahrs().get_home() };
return_loc.set_alt_cm(rtl_home_alt, Location::AltFrame::ABSOLUTE);
RallyLocation ral_loc;
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