fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 13:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

RTL: use loiter radius from mission for loiter before enganging mission in RTL

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-09-15 13:17:36 -04:00
committed by Daniel Agar
parent 0ce76679b7
commit 7a760c8c6e
5 changed files with 16 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+10 -6
View File
@@ -265,8 +265,6 @@ 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_land_detected()->landed) {
// For safety reasons don't go into RTL if landed.
_rtl_state = RTL_STATE_LANDED;
@@ -328,6 +326,10 @@ void RTL::set_rtl_item()
const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon);
const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
// if we will switch to mission for landing, already set the loiter radius (incl. direction) from mission
const float landing_loiter_radius = _destination.type == RTL_DESTINATION_MISSION_LANDING ?
_navigator->get_mission_landing_loiter_radius() : _param_rtl_loiter_rad.get();
const RTLHeadingMode rtl_heading_mode = static_cast<RTLHeadingMode>(_param_rtl_hdg_md.get());
switch (_rtl_state) {
@@ -423,9 +425,8 @@ void RTL::set_rtl_item()
_mission_item.yaw = _destination.yaw;
}
if (_navigator->get_vstatus()->is_vtol
|| _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
_mission_item.loiter_radius = _rtl_loiter_rad;
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
_mission_item.loiter_radius = landing_loiter_radius;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
@@ -471,7 +472,10 @@ void RTL::set_rtl_item()
_mission_item.yaw = _destination.yaw;
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
_mission_item.loiter_radius = landing_loiter_radius;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f);
_mission_item.autocontinue = autocontinue;