dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

rtl_direct_mission_land: fix abs/rel usage of item.altitude 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2024-03-08 17:51:36 +01:00
parent e9fda548fa
commit 86c074378f
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/navigator/rtl_direct_mission_land.cpp
View File

@ -320,7 +320,8 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate()
_rtl_time_estimator.addDistance(hor_dist, direction, 0.f); _rtl_time_estimator.addDistance(hor_dist, direction, 0.f);
// Add the vertical loiter // 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; break;
} }
@ -341,13 +342,14 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate()
_rtl_time_estimator.addDistance(hor_dist, direction, _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 { } else {
// For VTOL, Rotary, go there horizontally first, then land // For VTOL, Rotary, go there horizontally first, then land
_rtl_time_estimator.addDistance(hor_dist, direction, 0.f); _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; 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); hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon);
_rtl_time_estimator.addDistance(hor_dist, direction, _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; break;
} }
} }
@ -371,7 +373,7 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate()
start_item_index = next_mission_item_index + 1; start_item_index = next_mission_item_index + 1;
hor_position_at_calculation_point(0) = next_position_mission_item.lat; hor_position_at_calculation_point(0) = next_position_mission_item.lat;
hor_position_at_calculation_point(1) = next_position_mission_item.lon; 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 { } else {