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

RTL: implement proper RTL sequence for VTOL 

- descend to RTL descend altitude
- transition
- move to land waypoint
- loiter and then land

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2020-04-24 15:17:47 +03:00 committed by Lorenz Meier
parent 197a21a22e
commit 6ff95ae645
2 changed files with 39 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -341,13 +341,18 @@ void RTL::set_rtl_item()
break; break;
} }
case RTL_STATE_DESCEND: { case RTL_MOVE_TO_LAND_HOVER_VTOL: {
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; _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);
break;
}
} else { case RTL_STATE_DESCEND: {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
}
_mission_item.lat = _destination.lat; _mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon; _mission_item.lon = _destination.lon;
@ -394,7 +399,7 @@ void RTL::set_rtl_item()
_navigator->set_can_loiter_at_sp(true); _navigator->set_can_loiter_at_sp(true);
if (autoland && (get_time_inside(_mission_item) > FLT_EPSILON)) { if (autoland) {
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
(double)get_time_inside(_mission_item)); (double)get_time_inside(_mission_item));
@ -461,20 +466,40 @@ void RTL::set_rtl_item()
void RTL::advance_rtl() void RTL::advance_rtl()
{ {
// determines if the vehicle should loiter above land
const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA;
// vehicle is a vtol and currently in fixed wing mode
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) { switch (_rtl_state) {
case RTL_STATE_CLIMB: case RTL_STATE_CLIMB:
_rtl_state = RTL_STATE_RETURN; _rtl_state = RTL_STATE_RETURN;
break; break;
case RTL_STATE_RETURN: case RTL_STATE_RETURN:
_rtl_state = RTL_STATE_DESCEND;
if (vtol_in_fw_mode || descend_and_loiter) {
_rtl_state = RTL_STATE_DESCEND;
} else {
_rtl_state = RTL_STATE_LAND;
}
break; break;
case RTL_STATE_TRANSITION_TO_MC: case RTL_STATE_TRANSITION_TO_MC:
// Only go to land if autoland is enabled. _rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL;
if (_param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA) {
break;
case RTL_MOVE_TO_LAND_HOVER_VTOL:
if (descend_and_loiter) {
_rtl_state = RTL_STATE_LOITER; _rtl_state = RTL_STATE_LOITER;
} else { } else {
@ -486,12 +511,11 @@ void RTL::advance_rtl()
case RTL_STATE_DESCEND: case RTL_STATE_DESCEND:
// If the vehicle is a vtol in fixed wing mode, then first transition to hover // If the vehicle is a vtol in fixed wing mode, then first transition to hover
if (_navigator->get_vstatus()->is_vtol if (vtol_in_fw_mode) {
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
_rtl_state = RTL_STATE_TRANSITION_TO_MC; _rtl_state = RTL_STATE_TRANSITION_TO_MC;
} else if (_param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA) { } else if (descend_and_loiter) {
_rtl_state = RTL_STATE_DESCEND; _rtl_state = RTL_STATE_LOITER;
} else { } else {
_rtl_state = RTL_STATE_LAND; _rtl_state = RTL_STATE_LAND;

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

@ -104,6 +104,7 @@ private:
RTL_STATE_TRANSITION_TO_MC, RTL_STATE_TRANSITION_TO_MC,
RTL_STATE_DESCEND, RTL_STATE_DESCEND,
RTL_STATE_LOITER, RTL_STATE_LOITER,
RTL_MOVE_TO_LAND_HOVER_VTOL,
RTL_STATE_LAND, RTL_STATE_LAND,
RTL_STATE_LANDED, RTL_STATE_LANDED,
} _rtl_state{RTL_STATE_NONE}; } _rtl_state{RTL_STATE_NONE};