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

rtl: head towards home location after descend

Browse Source
This commit is contained in:
David Jablonski
2021-02-01 23:57:47 +01:00
committed by Silvan Fuhrer
parent aeccb621c5
commit ed7a5314d8
2 changed files with 27 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+26 -1
View File
@@ -258,6 +258,7 @@ void RTL::on_activation()
const vehicle_global_position_s &global_position = *_navigator->get_global_position();
_rtl_loiter_rad = _param_rtl_loiter_rad.get();
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
@@ -458,6 +459,24 @@ void RTL::set_rtl_item()
break;
}
case RTL_STATE_HEAD_TO_CENTER: {
_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.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
// Disable previous setpoint to prevent drift.
pos_sp_triplet->previous.valid = false;
break;
}
case RTL_STATE_TRANSITION_TO_MC: {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
break;
@@ -559,7 +578,7 @@ void RTL::advance_rtl()
_rtl_state = RTL_STATE_LOITER;
} else if (vtol_in_fw_mode) {
_rtl_state = RTL_STATE_TRANSITION_TO_MC;
_rtl_state = RTL_STATE_HEAD_TO_CENTER;
} else {
_rtl_state = RTL_STATE_LAND;
@@ -578,6 +597,12 @@ void RTL::advance_rtl()
_rtl_state = RTL_STATE_LAND;
break;
case RTL_STATE_HEAD_TO_CENTER:
_rtl_state = RTL_STATE_TRANSITION_TO_MC;
break;
case RTL_STATE_TRANSITION_TO_MC:
_rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL;
src/modules/navigator/rtl.h
+1
View File
@@ -121,6 +121,7 @@ private:
RTL_MOVE_TO_LAND_HOVER_VTOL,
RTL_STATE_LAND,
RTL_STATE_LANDED,
RTL_STATE_HEAD_TO_CENTER,
} _rtl_state{RTL_STATE_NONE};
struct RTLPosition {