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

If already in landing, go straight to state RTL_LAND. Also cleaned up the comments

Browse Source
This commit is contained in:
Jake Dahl 2018-07-25 14:43:20 -06:00 committed by Lorenz Meier
parent 130cdf25bf
commit 21db15ff4f
1 changed files with 30 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -59,7 +59,7 @@ RTL::RTL(Navigator *navigator) :
void
RTL::on_inactive()
{
// reset RTL state
// Reset RTL state.
_rtl_state = RTL_STATE_NONE;
}
@ -72,23 +72,26 @@ RTL::rtl_type() const
void
RTL::on_activation()
{
if (_navigator->get_land_detected()->landed
|| (_navigator->get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {
// for safety reasons don't go into RTL if landed or currently in landing
if (_navigator->get_land_detected()->landed) {
// For safety reasons don't go into RTL if landed or currently in landing.
_rtl_state = RTL_STATE_LANDED;
} else if (_navigator->get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
// Do not go into full RTL if already performing a land.
_rtl_state = RTL_STATE_LAND;
} else if ((rtl_type() == RTL_LAND) && _navigator->on_mission_landing()) {
// RTL straight to RETURN state, but mission will takeover for landing
// RTL straight to RETURN state, but mission will takeover for landing.
} else if ((_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get())
|| _rtl_alt_min) {
// if lower than return altitude, climb up first
// if rtl_alt_min is true then forcing altitude change even if above
// If lower than return altitude, climb up first.
// If rtl_alt_min is true then forcing altitude change even if above.
_rtl_state = RTL_STATE_CLIMB;
} else {
// otherwise go straight to return
// Otherwise go straight to return
_rtl_state = RTL_STATE_RETURN;
}
@ -113,9 +116,9 @@ RTL::set_return_alt_min(bool min)
void
RTL::set_rtl_item()
{
// RTL_TYPE: mission landing
// landing using planned mission landing, fly to DO_LAND_START instead of returning HOME
// do nothing, let navigator takeover with mission landing
// RTL_TYPE: mission landing.
// Landing using planned mission landing, fly to DO_LAND_START instead of returning HOME.
// Do nothing, let navigator takeover with mission landing.
if (rtl_type() == RTL_LAND) {
if (_rtl_state > RTL_STATE_CLIMB) {
if (_navigator->start_mission_landing()) {
@ -123,7 +126,7 @@ RTL::set_rtl_item()
return;
} else {
// otherwise use regular RTL
// Otherwise use regular RTL.
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unable to use mission landing");
}
}
@ -136,18 +139,18 @@ RTL::set_rtl_item()
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// check if we are pretty close to home already
// Check if we are pretty close to home already.
const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
// compute the return altitude
// Compute the return altitude.
float return_alt = max(home.alt + _param_return_alt.get(), gpos.alt);
// we are close to home, limit climb to min
// We are close to home, limit climb to min.
if (home_dist < _param_rtl_min_dist.get()) {
return_alt = home.alt + _param_descend_alt.get();
}
// compute the loiter altitude
// Compute the loiter altitude.
const float loiter_altitude = min(home.alt + _param_descend_alt.get(), gpos.alt);
switch (_rtl_state) {
@ -171,20 +174,20 @@ RTL::set_rtl_item()
case RTL_STATE_RETURN: {
// don't change altitude
// Don't change altitude.
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.altitude = return_alt;
_mission_item.altitude_is_relative = false;
// use home yaw if close to home
/* check if we are pretty close to home already */
// Use home yaw if close to home.
// Check if we are pretty close to home already.
if (home_dist < _param_rtl_min_dist.get()) {
_mission_item.yaw = home.yaw;
} else {
// use current heading to home
// Use current heading to home.
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, home.lat, home.lon);
}
@ -211,7 +214,7 @@ RTL::set_rtl_item()
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
// except for vtol which might be still off here and should point towards this location
// Except for vtol which might be still off here and should point towards this location.
const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) {
@ -226,7 +229,7 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
/* disable previous setpoint to prevent drift */
// Disable previous setpoint to prevent drift.
pos_sp_triplet->previous.valid = false;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
@ -237,7 +240,7 @@ RTL::set_rtl_item()
case RTL_STATE_LOITER: {
const bool autoland = (_param_land_delay.get() > FLT_EPSILON);
// don't change altitude
// Don't change altitude.
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.altitude = loiter_altitude;
@ -265,7 +268,7 @@ RTL::set_rtl_item()
}
case RTL_STATE_LAND: {
// land at home position
// Land at home position.
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
@ -293,12 +296,12 @@ RTL::set_rtl_item()
reset_mission_item_reached();
/* execute command if set. This is required for commands like VTOL transition */
// Execute command if set. This is required for commands like VTOL transition.
if (!item_contains_position(_mission_item)) {
issue_command(_mission_item);
}
/* convert mission item to current position setpoint and make it valid */
// Convert mission item to current position setpoint and make it valid.
mission_apply_limitation(_mission_item);
if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
@ -329,7 +332,7 @@ RTL::advance_rtl()
case RTL_STATE_DESCEND:
/* only go to land if autoland is enabled */
// Only go to land if autoland is enabled.
if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
_rtl_state = RTL_STATE_LOITER;