Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Rally: tidy calculation of nearest rally/home

Browse Source
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 // 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 Location AP_Rally::calc_best_rally_or_home_location(const Location &current_loc, float rtl_home_alt) const
{ {
RallyLocation ral_loc = {}; // if no valid rally point, return home position:
Location return_loc = {}; Location return_loc { AP::ahrs().get_home() };
const struct Location &home_loc = AP::ahrs().get_home(); return_loc.set_alt_cm(rtl_home_alt, Location::AltFrame::ABSOLUTE);
// 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
RallyLocation ral_loc;
if (find_nearest_rally_point(current_loc, ral_loc)) { if (find_nearest_rally_point(current_loc, ral_loc)) {
Location loc = rally_location_to_location(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 // use the rally point if it's closer then home, or we aren't generally considering home as acceptable