From 86c074378f533335c59af9d83c521bcc1acb7172 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 8 Mar 2024 17:51:36 +0100 Subject: [PATCH] rtl_direct_mission_land: fix abs/rel usage of item.altitude Signed-off-by: Silvan Fuhrer --- src/modules/navigator/rtl_direct_mission_land.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 99e96e3141..6935bc35e8 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -320,7 +320,8 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); // Add the vertical loiter - _rtl_time_estimator.addVertDistance(next_position_mission_item.altitude - altitude_at_calculation_point); + _rtl_time_estimator.addVertDistance(get_absolute_altitude_for_item(next_position_mission_item) - + altitude_at_calculation_point); break; } @@ -341,13 +342,14 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() _rtl_time_estimator.addDistance(hor_dist, direction, - next_position_mission_item.altitude - altitude_at_calculation_point); + get_absolute_altitude_for_item(next_position_mission_item) - altitude_at_calculation_point); } else { // For VTOL, Rotary, go there horizontally first, then land _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); - _rtl_time_estimator.addDescendMCLand(next_position_mission_item.altitude - altitude_at_calculation_point); + _rtl_time_estimator.addDescendMCLand(get_absolute_altitude_for_item(next_position_mission_item) - + altitude_at_calculation_point); } break; @@ -363,7 +365,7 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); _rtl_time_estimator.addDistance(hor_dist, direction, - next_position_mission_item.altitude - altitude_at_calculation_point); + get_absolute_altitude_for_item(next_position_mission_item) - altitude_at_calculation_point); break; } } @@ -371,7 +373,7 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() start_item_index = next_mission_item_index + 1; hor_position_at_calculation_point(0) = next_position_mission_item.lat; hor_position_at_calculation_point(1) = next_position_mission_item.lon; - altitude_at_calculation_point = next_position_mission_item.altitude; + altitude_at_calculation_point = get_absolute_altitude_for_item(next_position_mission_item); } else {