fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 08:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtl_direct_mission: rtl_estimate for fixedwing land is diagonal

Browse Source
This commit is contained in:
Konrad
2024-03-08 17:01:53 +01:00
committed by Silvan Fuhrer
parent 89844625b4
commit 91d1342f20
1 changed files with 28 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl_direct_mission_land.cpp
+28 -2
View File
@@ -308,8 +308,6 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate()
break;
}
case NAV_CMD_LAND: // Fallthrough
case NAV_CMD_VTOL_LAND: // Fallthrough
case NAV_CMD_LOITER_TO_ALT: {
// Go to point horizontally
matrix::Vector2f direction{};
@@ -327,6 +325,34 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate()
break;
}
case NAV_CMD_LAND: // Fallthrough
case NAV_CMD_VTOL_LAND: {
matrix::Vector2f direction{};
get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1),
next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1));
float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0),
hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon);
// For fixed wing, add diagonal line
if ((_vehicle_status_sub.get().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING)
&& (!_vehicle_status_sub.get().is_vtol)) {
_rtl_time_estimator.addDistance(hor_dist, direction,
next_position_mission_item.altitude - 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);
}
break;
}
default: {
// Default assume can go to the location directly
matrix::Vector2f direction{};