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

Add parameter and logic to set RTL heading mode.

Browse Source
This commit is contained in:
mcsauder
2021-10-22 09:28:30 +02:00
committed by Mathieu Bresciani
parent 6938d24ec7
commit 9301288d1f
3 changed files with 106 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+72 -26
View File
@@ -346,6 +346,8 @@ void RTL::set_rtl_item()
const float descend_altitude_target = min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt);
const float loiter_altitude = min(descend_altitude_target, _rtl_alt);
const RTLHeadingMode rtl_heading_mode = static_cast<RTLHeadingMode>(_param_rtl_hdg_md.get());
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
@@ -362,7 +364,14 @@ void RTL::set_rtl_item()
_mission_item.lon = gpos.lon;
_mission_item.altitude = _rtl_alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = _navigator->get_local_position()->heading;
if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
} else {
_mission_item.yaw = _destination.yaw;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
@@ -384,14 +393,17 @@ void RTL::set_rtl_item()
_mission_item.altitude = _rtl_alt;
_mission_item.altitude_is_relative = false;
// Use destination yaw if close to _destination.
// Check if we are pretty close to the destination already.
if (destination_dist < _param_rtl_min_dist.get()) {
if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING &&
destination_dist > _param_rtl_min_dist.get()) {
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING ||
destination_dist < _param_rtl_min_dist.get()) {
// Use destination yaw if close to _destination.
_mission_item.yaw = _destination.yaw;
} else {
// Use current heading to _destination.
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
@@ -410,7 +422,6 @@ void RTL::set_rtl_item()
case RTL_STATE_DESCEND: {
_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
@@ -422,6 +433,9 @@ void RTL::set_rtl_item()
if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) {
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
} else {
_mission_item.yaw = _destination.yaw;
}
@@ -449,20 +463,6 @@ void RTL::set_rtl_item()
case RTL_STATE_LOITER: {
const bool autoland = (_param_rtl_land_delay.get() > FLT_EPSILON);
// Don't change altitude.
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = _destination.yaw;
_mission_item.loiter_radius = _navigator->get_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 = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
if (autoland) {
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t",
@@ -475,17 +475,46 @@ void RTL::set_rtl_item()
events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering");
}
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude; // Don't change altitude.
_mission_item.altitude_is_relative = false;
if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
} else {
_mission_item.yaw = _destination.yaw;
}
_mission_item.loiter_radius = _navigator->get_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 = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
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);
if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) {
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) {
_mission_item.yaw = _destination.yaw;
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
@@ -507,7 +536,17 @@ void RTL::set_rtl_item()
_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);
if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) {
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) {
_mission_item.yaw = _destination.yaw;
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.origin = ORIGIN_ONBOARD;
break;
@@ -518,9 +557,16 @@ void RTL::set_rtl_item()
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.yaw = _destination.yaw;
_mission_item.altitude = _destination.alt;
_mission_item.altitude_is_relative = false;
if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
} else {
_mission_item.yaw = _destination.yaw;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;