Copter: RTL: ensure rally point is in absolute alt frame as RTL_ALT_TYPE takes precedence

This commit is contained in:
Iampete1 2023-10-27 00:17:23 +01:00 committed by Andrew Tridgell
parent 1bfe83bb63
commit 935ea02a71

View File

@ -408,9 +408,10 @@ void ModeRTL::build_path()
// return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set) // return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
void ModeRTL::compute_return_target() void ModeRTL::compute_return_target()
{ {
// set return target to nearest rally point or home position (Note: alt is absolute) // set return target to nearest rally point or home position
#if HAL_RALLY_ENABLED #if HAL_RALLY_ENABLED
rtl_path.return_target = copter.rally.calc_best_rally_or_home_location(copter.current_loc, ahrs.get_home().alt); rtl_path.return_target = copter.rally.calc_best_rally_or_home_location(copter.current_loc, ahrs.get_home().alt);
rtl_path.return_target.change_alt_frame(Location::AltFrame::ABSOLUTE);
#else #else
rtl_path.return_target = ahrs.get_home(); rtl_path.return_target = ahrs.get_home();
#endif #endif