diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 2a05d21784..d39aa417b4 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,6 +41,7 @@ #include "rtl.h" #include "navigator.h" +#include static constexpr float DELAY_SIGMA = 0.01f; @@ -68,6 +69,79 @@ void RTL::on_activation() { + // find the RTL destination: go through the safe points & home position + const home_position_s &home_position = *_navigator->get_home_position(); + const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + + mission_stats_entry_s stats; + int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + int num_safe_points = 0; + + if (ret == sizeof(mission_stats_entry_s)) { + num_safe_points = stats.num_items; + } + + int closest_index = 0; + mission_safe_point_s closest_safe_point; + + // take home position into account + double dlat = home_position.lat - global_position.lat; + double dlon = home_position.lon - global_position.lon; + double min_dist_squared = dlat * dlat + dlon * dlon; + + // find the closest point + for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) { + mission_safe_point_s mission_safe_point; + + if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) != + sizeof(mission_safe_point_s)) { + PX4_ERR("dm_read failed"); + continue; + } + + // TODO: take altitude into account for distance measurement + + dlat = mission_safe_point.lat - global_position.lat; + dlon = mission_safe_point.lon - global_position.lon; + double dist_squared = dlat * dlat + dlon * dlon; + + if (dist_squared < min_dist_squared) { + closest_index = current_seq; + min_dist_squared = dist_squared; + closest_safe_point = mission_safe_point; + } + } + + if (closest_index == 0) { + _destination.set(home_position); + + } else { + + // TODO: handle all possible mission_safe_point.frame cases + switch (closest_safe_point.frame) { + case 0: // MAV_FRAME_GLOBAL + _destination.safe_point_index = closest_index; + _destination.lat = closest_safe_point.lat; + _destination.lon = closest_safe_point.lon; + _destination.alt = closest_safe_point.alt; + _destination.yaw = home_position.yaw; + break; + + case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT + _destination.safe_point_index = closest_index; + _destination.lat = closest_safe_point.lat; + _destination.lon = closest_safe_point.lon; + _destination.alt = closest_safe_point.alt + home_position.alt; // alt of safe point is rel to home + _destination.yaw = home_position.yaw; + break; + + default: + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME. Landing at HOME."); + _destination.set(home_position); + break; + } + } + _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get()); if (_navigator->get_land_detected()->landed) { @@ -77,8 +151,7 @@ RTL::on_activation() } else if ((rtl_type() == RTL_LAND) && _navigator->on_mission_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_rtl_return_alt.get()) - || _rtl_alt_min) { + } else if ((global_position.alt < _destination.alt + _param_rtl_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. @@ -111,7 +184,7 @@ void RTL::set_rtl_item() { // RTL_TYPE: mission landing. - // Landing using planned mission landing, fly to DO_LAND_START instead of returning HOME. + // Landing using planned mission landing, fly to DO_LAND_START instead of returning _destination. // Do nothing, let navigator takeover with mission landing. if (rtl_type() == RTL_LAND) { if (_rtl_state > RTL_STATE_CLIMB) { @@ -128,16 +201,15 @@ RTL::set_rtl_item() _navigator->set_can_loiter_at_sp(false); - const home_position_s &home = *_navigator->get_home_position(); const vehicle_global_position_s &gpos = *_navigator->get_global_position(); position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - // 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); + // Check if we are pretty close to the destination already. + const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); // Compute the loiter altitude. - const float loiter_altitude = math::min(home.alt + _param_rtl_descend_alt.get(), gpos.alt); + const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -153,8 +225,8 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)", - (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _navigator->get_home_position()->alt)); + mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)", + (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); break; } @@ -162,19 +234,19 @@ RTL::set_rtl_item() // Don't change altitude. _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.lat = home.lat; - _mission_item.lon = home.lon; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; _mission_item.altitude = _rtl_alt; _mission_item.altitude_is_relative = false; - // 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; + // 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()) { + _mission_item.yaw = _destination.yaw; } else { - // Use current heading to home. - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, home.lat, home.lon); + // Use current heading to _destination. + _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); } _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); @@ -182,8 +254,8 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - home.alt)); + mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); break; } @@ -195,8 +267,8 @@ RTL::set_rtl_item() case RTL_STATE_DESCEND: { _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.lat = home.lat; - _mission_item.lon = home.lon; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; _mission_item.altitude = loiter_altitude; _mission_item.altitude_is_relative = false; @@ -207,7 +279,7 @@ RTL::set_rtl_item() _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); } else { - _mission_item.yaw = home.yaw; + _mission_item.yaw = _destination.yaw; } _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); @@ -218,8 +290,8 @@ RTL::set_rtl_item() // 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)", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - home.alt)); + mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); break; } @@ -227,11 +299,11 @@ RTL::set_rtl_item() const bool autoland = (_param_rtl_land_delay.get() > FLT_EPSILON); // Don't change altitude. - _mission_item.lat = home.lat; - _mission_item.lon = home.lon; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; _mission_item.altitude = loiter_altitude; _mission_item.altitude_is_relative = false; - _mission_item.yaw = home.yaw; + _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 = math::max(_param_rtl_land_delay.get(), 0.0f); @@ -254,19 +326,19 @@ RTL::set_rtl_item() } case RTL_STATE_LAND: { - // Land at home position. + // Land at destination. _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.lat = home.lat; - _mission_item.lon = home.lon; - _mission_item.yaw = home.yaw; - _mission_item.altitude = home.alt; + _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; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home"); + mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination"); break; } @@ -352,34 +424,33 @@ RTL::advance_rtl() float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) { - const home_position_s &home = *_navigator->get_home_position(); const vehicle_global_position_s &gpos = *_navigator->get_global_position(); - // horizontal distance to home position - const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon); + // horizontal distance to destination + const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); float rtl_altitude; - if (home_dist <= _param_rtl_min_dist.get()) { - rtl_altitude = home.alt + _param_rtl_descend_alt.get(); + if (destination_dist <= _param_rtl_min_dist.get()) { + rtl_altitude = _destination.alt + _param_rtl_descend_alt.get(); - } else if (gpos.alt > home.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) { + } else if (gpos.alt > _destination.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) { rtl_altitude = gpos.alt; } else if (cone_half_angle_deg <= 0) { - rtl_altitude = home.alt + _param_rtl_return_alt.get(); + rtl_altitude = _destination.alt + _param_rtl_return_alt.get(); } else { // constrain cone half angle to meaningful values. All other cases are already handled above. const float cone_half_angle_rad = math::radians(math::constrain(cone_half_angle_deg, 1.0f, 89.0f)); - // minimum height above home position required - float height_above_home_min = home_dist / tanf(cone_half_angle_rad); + // minimum height above destination required + float height_above_destination_min = destination_dist / tanf(cone_half_angle_rad); // minimum altitude we need in order to be within the user defined cone - const float altitude_min = math::constrain(height_above_home_min + home.alt, home.alt, - home.alt + _param_rtl_return_alt.get()); + const float altitude_min = math::constrain(height_above_destination_min + _destination.alt, _destination.alt, + _destination.alt + _param_rtl_return_alt.get()); if (gpos.alt < altitude_min) { rtl_altitude = altitude_min; @@ -390,7 +461,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) } // always demand altitude which is higher or equal the RTL descend altitude - rtl_altitude = math::max(rtl_altitude, home.alt + _param_rtl_descend_alt.get()); + rtl_altitude = math::max(rtl_altitude, _destination.alt + _param_rtl_descend_alt.get()); return rtl_altitude; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index ea6cf59dd1..7138828ae1 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -46,6 +46,8 @@ #include "navigator_mode.h" #include "mission_block.h" +#include + class Navigator; class RTL : public MissionBlock, public ModuleParams @@ -94,6 +96,25 @@ private: RTL_STATE_LANDED, } _rtl_state{RTL_STATE_NONE}; + struct RTLPosition { + double lat; + double lon; + float alt; + float yaw; + uint8_t safe_point_index; ///< 0 = home position + + void set(const home_position_s &home_position) + { + lat = home_position.lat; + lon = home_position.lon; + alt = home_position.alt; + yaw = home_position.yaw; + safe_point_index = 0; + } + }; + + RTLPosition _destination; ///< the RTL position to fly to (typically the home position or a safe point) + float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position bool _rtl_alt_min{false}; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 2ec7afc972..bb57aab9d3 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -97,7 +97,7 @@ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); * * @unit m * @min 0.5 - * @max 20 + * @max 100 * @decimal 1 * @increment 0.5 * @group Return Mode