fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

RTL: VTOL: do the land delay loitering (RTL_LAND_DELAY) in fixed-wing flight

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-01-18 17:03:23 +01:00 committed by Roman Bapst
parent cc29754163
commit acbada73dd
2 changed files with 43 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
src/modules/navigator/rtl.cpp
View File

@ -392,23 +392,6 @@ void RTL::set_rtl_item()
break;
}
case RTL_STATE_TRANSITION_TO_MC: {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
break;
}
case RTL_MOVE_TO_LAND_HOVER_VTOL: {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.origin = ORIGIN_ONBOARD;
break;
}
case RTL_STATE_DESCEND: {
_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
@ -470,6 +453,23 @@ void RTL::set_rtl_item()
break;
}
case RTL_STATE_TRANSITION_TO_MC: {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
break;
}
case RTL_MOVE_TO_LAND_HOVER_VTOL: {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.origin = ORIGIN_ONBOARD;
break;
}
case RTL_STATE_LAND: {
// Land at destination.
_mission_item.nav_cmd = NAV_CMD_LAND;
@ -531,7 +531,6 @@ void RTL::advance_rtl()
const bool vtol_in_fw_mode = _navigator->get_vstatus()->is_vtol
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
switch (_rtl_state) {
case RTL_STATE_CLIMB:
_rtl_state = RTL_STATE_RETURN;
@ -549,6 +548,30 @@ void RTL::advance_rtl()
break;
case RTL_STATE_DESCEND:
if (descend_and_loiter) {
_rtl_state = RTL_STATE_LOITER;
} else if (vtol_in_fw_mode) {
_rtl_state = RTL_STATE_TRANSITION_TO_MC;
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_LOITER:
if (vtol_in_fw_mode) {
_rtl_state = RTL_STATE_TRANSITION_TO_MC;
} else {
_rtl_state = RTL_STATE_LAND;
}
_rtl_state = RTL_STATE_LAND;
break;
case RTL_STATE_TRANSITION_TO_MC:
@ -558,32 +581,8 @@ void RTL::advance_rtl()
case RTL_MOVE_TO_LAND_HOVER_VTOL:
if (descend_and_loiter) {
_rtl_state = RTL_STATE_LOITER;
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_DESCEND:
// If the vehicle is a vtol in fixed wing mode, then first transition to hover
if (vtol_in_fw_mode) {
_rtl_state = RTL_STATE_TRANSITION_TO_MC;
} else if (descend_and_loiter) {
_rtl_state = RTL_STATE_LOITER;
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_LOITER:
_rtl_state = RTL_STATE_LAND;
break;
case RTL_STATE_LAND:

2
src/modules/navigator/rtl.h
View File

@ -115,9 +115,9 @@ private:
RTL_STATE_NONE = 0,
RTL_STATE_CLIMB,
RTL_STATE_RETURN,
RTL_STATE_TRANSITION_TO_MC,
RTL_STATE_DESCEND,
RTL_STATE_LOITER,
RTL_STATE_TRANSITION_TO_MC,
RTL_MOVE_TO_LAND_HOVER_VTOL,
RTL_STATE_LAND,
RTL_STATE_LANDED,