diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 86ce3b9b7b..58c6472c3b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -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;