diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 4a9a33464f..287b7d3b88 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -119,10 +119,10 @@ protected: /** * @brief Has Mission a Land Start or Land Item * - * @return true If mission has a land start of land item + * @return true If mission has a land start of land item and a land item * @return false otherwise */ - bool hasMissionLandStart() const { return _mission.land_start_index > 0;}; + bool hasMissionLandStart() const { return _mission.land_start_index > 0 && _mission.land_index > 0;}; /** * @brief Go to next Mission Item * Go to next non jump mission item diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 71fcb756a5..8c0654f8a8 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -961,3 +961,101 @@ MissionBlock::initialize() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; } + +void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius, + HeadingMode heading_mode) const +{ + item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + item.lat = dest.lat; + item.lon = dest.lon; + item.altitude = dest.alt; + item.altitude_is_relative = false; + + item. yaw = setYawFromHeadingMode(dest, heading_mode); + + item.acceptance_radius = _navigator->get_acceptance_radius(); + item.time_inside = 0.0f; + item.autocontinue = true; + item.origin = ORIGIN_ONBOARD; + item.loiter_radius = loiter_radius; +} + +void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, + float loiter_radius, HeadingMode heading_mode) const +{ + const bool autocontinue = (loiter_time > -FLT_EPSILON); + + if (autocontinue) { + item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + + } else { + item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + } + + item.lat = dest.lat; + item.lon = dest.lon; + item.altitude = dest.alt; + item.altitude_is_relative = false; + + item. yaw = setYawFromHeadingMode(dest, heading_mode); + + item.acceptance_radius = _navigator->get_acceptance_radius(); + item.time_inside = math::max(loiter_time, 0.0f); + item.autocontinue = autocontinue; + item.origin = ORIGIN_ONBOARD; + item.loiter_radius = loiter_radius; +} + +void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, + HeadingMode heading_mode) const +{ + item.nav_cmd = NAV_CMD_WAYPOINT; + item.lat = dest.lat; + item.lon = dest.lon; + item.altitude = dest.alt; + item.altitude_is_relative = false; + + item.autocontinue = true; + item.acceptance_radius = _navigator->get_acceptance_radius(); + item.time_inside = 0.f; + item.origin = ORIGIN_ONBOARD; + + item. yaw = setYawFromHeadingMode(dest, heading_mode); +} + +void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, + HeadingMode heading_mode) const +{ + item.nav_cmd = NAV_CMD_LAND; + item.lat = dest.lat; + item.lon = dest.lon; + item.altitude = dest.alt; + + if (heading_mode == HeadingMode::CURRENT_HEADING) { + item.yaw = _navigator->get_local_position()->heading; + + } else { + item.yaw = dest.yaw; + } + + item.acceptance_radius = _navigator->get_acceptance_radius(); + item.time_inside = 0.0f; + item.autocontinue = true; + item.origin = ORIGIN_ONBOARD; +} + +float MissionBlock::setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const +{ + float desired_yaw(_navigator->get_local_position()->heading); + + if (heading_mode == HeadingMode::NAVIGATION_HEADING) { + desired_yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, dest.lat, dest.lon); + + } else if (heading_mode == HeadingMode::DESTINATION_HEADING) { + desired_yaw = dest.yaw; + + } + + return desired_yaw; +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 1a669512af..9b79ea6790 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -146,6 +146,15 @@ public: const struct mission_item_s *const mission_item_next) const; protected: + /** + * @brief heading mode for setting navigation items + * + */ + enum class HeadingMode { + NAVIGATION_HEADING = 0, + DESTINATION_HEADING, + CURRENT_HEADING, + }; /** * Check if mission item has been reached (for Waypoint based mission items) or Completed (Action based mission items) * @@ -201,6 +210,19 @@ protected: */ void mission_apply_limitation(mission_item_s &item); + void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius, + HeadingMode heading_mode) const; + + void setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, + float loiter_radius, HeadingMode heading_mode) const; + + void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, + HeadingMode heading_mode) const; + + void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, HeadingMode heading_mode) const; + + float setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const; + /** * @brief Issue a command for mission items with a nav_cmd that specifies an action * diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 54296a99b1..aaccaa9fdd 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -227,6 +227,18 @@ struct mission_fence_point_s { uint8_t _padding0[5]; /**< padding struct size to alignment boundary */ }; +/** + * @brief Return to launch position. + * Defines the position and landing yaw for the return to launch destination. + * + */ +struct DestinationPosition { + double lat; /**< latitude in WGS84 [rad].*/ + double lon; /**< longitude in WGS84 [rad].*/ + float alt; /**< altitude in MSL [m].*/ + float yaw; /**< final yaw when landed [rad].*/ +}; + #if (__GNUC__ >= 5) || __clang__ #pragma GCC diagnostic pop #endif // GCC >= 5 || Clang diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fd0f076569..7fd03f3e5d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -720,6 +720,17 @@ void Navigator::run() } else { _pos_sp_triplet_published_invalid_once = false; + } + +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF + + // If we are in VTOL takeoff, do not switch until it is finished. + if (_navigation_mode == &_vtol_takeoff && !get_mission_result()->finished) { + navigation_mode_new = &_vtol_takeoff; + + } else +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF + { navigation_mode_new = &_rtl; } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c7ededb4eb..d55d7b815b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -42,13 +42,16 @@ #include "rtl.h" #include "navigator.h" +#include "mission_block.h" #include #include using namespace time_literals; using namespace math; +using matrix::wrap_pi; +static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES{10.0f}; // [m] We don't consider safe points valid if the distance from the current home to the safe point is smaller than this distance static constexpr float MIN_DIST_THRESHOLD = 2.f; RTL::RTL(Navigator *navigator) : @@ -76,7 +79,7 @@ void RTL::updateDatamanCache() case DatamanState::Read: _dataman_state = DatamanState::ReadWait; - success = _dataman_client_geofence.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&_stats), + success = _dataman_client_safepoint.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&_stats), sizeof(mission_stats_entry_s)); if (!success) { @@ -88,9 +91,9 @@ void RTL::updateDatamanCache() case DatamanState::ReadWait: - _dataman_client_geofence.update(); + _dataman_client_safepoint.update(); - if (_dataman_client_geofence.lastOperationCompleted(success)) { + if (_dataman_client_safepoint.lastOperationCompleted(success)) { if (!success) { _error_state = DatamanState::ReadWait; @@ -101,14 +104,14 @@ void RTL::updateDatamanCache() _update_counter = _stats.update_counter; _safe_points_updated = false; - _dataman_cache_geofence.invalidate(); + _dataman_cache_safepoint.invalidate(); - if (_dataman_cache_geofence.size() != _stats.num_items) { - _dataman_cache_geofence.resize(_stats.num_items); + if (_dataman_cache_safepoint.size() != _stats.num_items) { + _dataman_cache_safepoint.resize(_stats.num_items); } - for (int index = 1; index <= _dataman_cache_geofence.size(); ++index) { - _dataman_cache_geofence.load(DM_KEY_SAFE_POINTS, index); + for (int index = 1; index <= _dataman_cache_safepoint.size(); ++index) { + _dataman_cache_safepoint.load(DM_KEY_SAFE_POINTS, index); } _dataman_state = DatamanState::Load; @@ -122,9 +125,9 @@ void RTL::updateDatamanCache() case DatamanState::Load: - _dataman_cache_geofence.update(); + _dataman_cache_safepoint.update(); - if (!_dataman_cache_geofence.isLoading()) { + if (!_dataman_cache_safepoint.isLoading()) { _dataman_state = DatamanState::UpdateRequestWait; _safe_points_updated = true; } @@ -146,10 +149,6 @@ void RTL::updateDatamanCache() const dm_item_t dm_item = static_cast(_mission_sub.get().dataman_id); _dataman_cache_landItem.invalidate(); - if (_mission_sub.get().land_start_index > 0) { - _dataman_cache_landItem.load(dm_item, _mission_sub.get().land_start_index); - } - if (_mission_sub.get().land_index > 0) { _dataman_cache_landItem.load(dm_item, _mission_sub.get().land_index); } @@ -182,6 +181,7 @@ void RTL::on_inactive() _vehicle_status_sub.update(); _mission_sub.update(); _home_pos_sub.update(); + _wind_sub.update(); updateDatamanCache(); @@ -227,7 +227,8 @@ void RTL::on_inactive() estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate(); break; - default: break; + default: + break; } } @@ -252,7 +253,6 @@ void RTL::on_activation() _rtl_direct.on_activation(); break; - default: break; } @@ -264,6 +264,7 @@ void RTL::on_active() _vehicle_status_sub.update(); _mission_sub.update(); _home_pos_sub.update(); + _wind_sub.update(); updateDatamanCache(); @@ -290,31 +291,49 @@ void RTL::setRtlTypeAndDestination() if (_param_rtl_type.get() != 2) { // check the closest allowed destination. - bool isMissionLanding{false}; - RtlDirect::RtlPosition rtl_position; + DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME}; + DestinationPosition rtl_position; float rtl_alt; - findRtlDestination(isMissionLanding, rtl_position, rtl_alt); + findRtlDestination(destination_type, rtl_position, rtl_alt); - if (isMissionLanding) { + switch (destination_type) { + case DestinationType::DESTINATION_TYPE_MISSION_LAND: _rtl_type = RtlType::RTL_DIRECT_MISSION_LAND; _rtl_mission_type_handle->setRtlAlt(rtl_alt); + break; + + case DestinationType::DESTINATION_TYPE_SAFE_POINT: // Fallthrough + case DestinationType::DESTINATION_TYPE_HOME: // Fallthrough + default: + + loiter_point_s landing_loiter; + landing_loiter.lat = rtl_position.lat; + landing_loiter.lon = rtl_position.lon; + landing_loiter.height_m = NAN; + + land_approaches_s rtl_land_approaches{readVtolLandApproaches(rtl_position)}; + + if (rtl_land_approaches.isAnyApproachValid()) { + landing_loiter = chooseBestLandingApproach(rtl_land_approaches); + } - } else { _rtl_type = RtlType::RTL_DIRECT; _rtl_direct.setRtlAlt(rtl_alt); - _rtl_direct.setRtlPosition(rtl_position); + _rtl_direct.setRtlPosition(rtl_position, landing_loiter); + + break; } } } -void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt) +void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosition &rtl_position, float &rtl_alt) { // set destination to home per default, then check if other valid landing spot is closer rtl_position.alt = _home_pos_sub.get().alt; rtl_position.lat = _home_pos_sub.get().lat; rtl_position.lon = _home_pos_sub.get().lon; rtl_position.yaw = _home_pos_sub.get().yaw; - isMissionLanding = false; + destination_type = DestinationType::DESTINATION_TYPE_HOME; const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); @@ -323,8 +342,9 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl float home_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)}; float min_dist; - if ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) { - // Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode. + if (((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) || ((_param_rtl_approach_force.get() == 1) + && !hasVtolLandApproach(rtl_position))) { + // Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we forces approach landing and it is not defined for home. min_dist = FLT_MAX; } else { @@ -332,7 +352,8 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl } // consider the mission landing if available and allowed - if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == 3)) && hasMissionLandStart()) { + if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == 3) || (fabsf(FLT_MAX - min_dist) < FLT_EPSILON)) + && hasMissionLandStart()) { mission_item_s land_mission_item; const dm_item_t dm_item = static_cast(_mission_sub.get().dataman_id); bool success = _dataman_cache_landItem.loadWait(dm_item, _mission_sub.get().land_index, @@ -348,18 +369,25 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)}; if ((dist + MIN_DIST_THRESHOLD) < min_dist) { - min_dist = dist; + if (_param_rtl_type.get() != 0) { + min_dist = dist; + + } else { + // Mission landing is not allowed, but home has no approaches. Still use mission landing. + min_dist = FLT_MAX; + } + setLandPosAsDestination(rtl_position, land_mission_item); - isMissionLanding = true; + destination_type = DestinationType::DESTINATION_TYPE_MISSION_LAND; } } if (_safe_points_updated) { - for (int current_seq = 1; current_seq <= _dataman_cache_geofence.size(); ++current_seq) { + for (int current_seq = 1; current_seq <= _dataman_cache_safepoint.size(); ++current_seq) { mission_item_s mission_safe_point; - bool success = _dataman_cache_geofence.loadWait(DM_KEY_SAFE_POINTS, current_seq, + bool success = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS, current_seq, reinterpret_cast(&mission_safe_point), sizeof(mission_item_s), 500_ms); @@ -375,10 +403,15 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)}; - if ((dist + MIN_DIST_THRESHOLD) < min_dist) { - min_dist = dist; - setSafepointAsDestination(rtl_position, mission_safe_point); - isMissionLanding = false; + DestinationPosition safepoint_position; + setSafepointAsDestination(safepoint_position, mission_safe_point); + + if (((dist + MIN_DIST_THRESHOLD) < min_dist) && ((_param_rtl_approach_force.get() == 0) + || hasVtolLandApproach(safepoint_position))) { + min_dist = dist; + rtl_position = safepoint_position; + destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; + } } } } @@ -392,7 +425,7 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl } } -void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_item_s &land_mission_item) +void RTL::setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) const { rtl_position.alt = land_mission_item.altitude_is_relative ? land_mission_item.altitude + _home_pos_sub.get().alt : land_mission_item.altitude; @@ -401,8 +434,8 @@ void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_ rtl_position.yaw = _home_pos_sub.get().yaw; } -void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, - const mission_item_s &mission_safe_point) +void RTL::setSafepointAsDestination(DestinationPosition &rtl_position, + const mission_item_s &mission_safe_point) const { // There is a safe point closer than home/mission landing // TODO: handle all possible mission_safe_point.frame cases @@ -429,8 +462,8 @@ void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, } } -float RTL::calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, - float cone_half_angle_deg) +float RTL::calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position, + float cone_half_angle_deg) const { // horizontal distance to destination const float destination_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, @@ -504,7 +537,7 @@ void RTL::init_rtl_mission_type() case RtlType::RTL_MISSION_FAST_REVERSE: _rtl_mission_type_handle = new RtlMissionFastReverse(_navigator); _set_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE; - _rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;; + _rtl_type = RtlType::RTL_MISSION_FAST_REVERSE; break; default: @@ -528,7 +561,99 @@ void RTL::parameters_update() } } -bool RTL::hasMissionLandStart() +bool RTL::hasMissionLandStart() const { return _mission_sub.get().land_start_index > 0; } + +bool RTL::hasVtolLandApproach(const DestinationPosition &rtl_position) const +{ + return readVtolLandApproaches(rtl_position).isAnyApproachValid(); +} + +loiter_point_s RTL::chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches) +{ + const float wind_direction = atan2f(_wind_sub.get().windspeed_east, _wind_sub.get().windspeed_north); + int8_t min_index = -1; + float wind_angle_prev = INFINITY; + + for (int i = 0; i < vtol_land_approaches.num_approaches_max; i++) { + + if (vtol_land_approaches.approaches[i].isValid()) { + const float wind_angle = wrap_pi(get_bearing_to_next_waypoint(_home_pos_sub.get().lat, + _home_pos_sub.get().lon, vtol_land_approaches.approaches[i].lat, + vtol_land_approaches.approaches[i].lon) - wind_direction); + + if (fabsf(wind_angle) < wind_angle_prev) { + min_index = i; + wind_angle_prev = fabsf(wind_angle); + } + + } + } + + if (min_index >= 0) { + return vtol_land_approaches.approaches[min_index]; + + } else { + + return loiter_point_s(); + } +} + +land_approaches_s RTL::readVtolLandApproaches(DestinationPosition rtl_position) const +{ + + // go through all mission items in the rally point storage. If we find a mission item of type NAV_CMD_RALLY_POINT + // which is within MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES of our current home position then treat ALL following mission items of type NAV_CMD_LOITER_TO_ALT which come + // BEFORE the next mission item of type NAV_CMD_RALLY_POINT as land approaches for the home position + land_approaches_s vtol_land_approaches{}; + + if (!_safe_points_updated) { + return vtol_land_approaches; + } + + bool foundHomeLandApproaches = false; + uint8_t sector_counter = 0; + + for (int current_seq = 1; current_seq <= _stats.num_items; ++current_seq) { + mission_item_s mission_item{}; + + bool success_mission_item = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS, current_seq, + reinterpret_cast(&mission_item), + sizeof(mission_item_s)); + + if (!success_mission_item) { + PX4_ERR("dm_read failed"); + break; + } + + if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) { + + if (foundHomeLandApproaches) { + break; + } + + const float dist_to_safepoint = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, rtl_position.lat, + rtl_position.lon); + + if (dist_to_safepoint < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { + foundHomeLandApproaches = true; + vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(mission_item.lat, mission_item.lon); + } + + sector_counter = 0; + } + + if (foundHomeLandApproaches && mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { + vtol_land_approaches.approaches[sector_counter].lat = mission_item.lat; + vtol_land_approaches.approaches[sector_counter].lon = mission_item.lon; + vtol_land_approaches.approaches[sector_counter].height_m = MissionBlock::get_absolute_altitude_for_item(mission_item, + _home_pos_sub.get().alt); + vtol_land_approaches.approaches[sector_counter].loiter_radius_m = mission_item.loiter_radius; + sector_counter++; + } + } + + return vtol_land_approaches; +} diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 2a96cf04b0..53f6f2057c 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -44,6 +44,7 @@ #include #include "navigator_mode.h" +#include "navigation.h" #include #include "rtl_base.h" #include "rtl_direct.h" @@ -88,7 +89,14 @@ public: void updateSafePoints() { _initiate_safe_points_updated = true; } private: - bool hasMissionLandStart(); + enum class DestinationType { + DESTINATION_TYPE_HOME, + DESTINATION_TYPE_MISSION_LAND, + DESTINATION_TYPE_SAFE_POINT, + }; + +private: + bool hasMissionLandStart() const; /** * @brief function to call regularly to do background work @@ -101,20 +109,20 @@ private: * @brief Find RTL destination. * */ - void findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt); + void findRtlDestination(DestinationType &destination_type, DestinationPosition &rtl_position, float &rtl_alt); /** * @brief Set the position of the land start marker in the planned mission as destination. * */ - void setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_item_s &land_mission_item); + void setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) const; /** * @brief Set the safepoint as destination. * * @param mission_safe_point is the mission safe point/rally point to set as destination. */ - void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_item_s &mission_safe_point); + void setSafepointAsDestination(DestinationPosition &rtl_position, const mission_item_s &mission_safe_point) const; /** * @brief calculate return altitude from cone half angle @@ -123,7 +131,8 @@ private: * @param[in] cone_half_angle_deg half angle of the cone [deg] * @return return altitude */ - float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, float cone_half_angle_deg); + float calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position, + float cone_half_angle_deg) const; /** * @brief initialize RTL mission type @@ -137,6 +146,33 @@ private: */ void parameters_update(); + /** + * @brief read VTOL land approaches + * + * @param[in] rtl_position landing position of the rtl + * + */ + land_approaches_s readVtolLandApproaches(DestinationPosition rtl_position) const; + + /** + * @brief Has VTOL land approach + * + * @param[in] rtl_position landing position of the rtl + * + * @return true if home land approaches are defined for home position + * @return false otherwise + */ + bool hasVtolLandApproach(const DestinationPosition &rtl_position) const; + + /** + * @brief Choose best landing approach + * + * Choose best landing approach for home considering wind + * + * @return loiter_point_s best landing approach + */ + loiter_point_s chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches); + enum class DatamanState { UpdateRequestWait, Read, @@ -156,10 +192,10 @@ private: DatamanState _error_state{DatamanState::UpdateRequestWait}; uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, safe points data was updated bool _safe_points_updated{false}; ///< flag indicating if safe points are updated to dataman cache - DatamanCache _dataman_cache_geofence{"rtl_dm_cache_miss_geo", 4}; - DatamanClient &_dataman_client_geofence = _dataman_cache_geofence.client(); + mutable DatamanCache _dataman_cache_safepoint{"rtl_dm_cache_miss_geo", 4}; + DatamanClient &_dataman_client_safepoint = _dataman_cache_safepoint.client(); bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed - DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; + mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; int16_t _mission_counter = -1; mission_stats_entry_s _stats; @@ -173,7 +209,8 @@ private: (ParamInt) _param_rtl_cone_half_angle_deg, (ParamFloat) _param_rtl_return_alt, (ParamFloat) _param_rtl_min_dist, - (ParamFloat) _param_nav_acc_rad + (ParamFloat) _param_nav_acc_rad, + (ParamInt) _param_rtl_approach_force ) uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -182,6 +219,7 @@ private: uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::SubscriptionData _mission_sub{ORB_ID(mission)}; uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; + uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; uORB::Publication _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)}; }; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 22c79f7161..c29df4327a 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -40,23 +40,26 @@ * @author Julian Kent */ +#include + #include "rtl_direct.h" #include "navigator.h" -#include #include #include - -static constexpr float DELAY_SIGMA = 0.01f; - -using namespace time_literals; using namespace math; RtlDirect::RtlDirect(Navigator *navigator) : MissionBlock(navigator), ModuleParams(navigator) { + _destination.lat = static_cast(NAN); + _destination.lon = static_cast(NAN); + _land_approach.lat = static_cast(NAN); + _land_approach.lon = static_cast(NAN); + _land_approach.height_m = NAN; + _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); @@ -72,54 +75,45 @@ void RtlDirect::on_inactivation() if (_navigator->get_precland()->is_activated()) { _navigator->get_precland()->on_inactivation(); } + + _rtl_state = RTLState::IDLE; } void RtlDirect::on_activation() { _global_pos_sub.update(); - _local_pos_sub.update(); _land_detected_sub.update(); _vehicle_status_sub.update(); parameters_update(); - if (_land_detected_sub.get().landed) { - // For safety reasons don't go into RTL if landed. - _rtl_state = RTL_STATE_LANDED; - - } else if ((_global_pos_sub.get().alt < _destination.alt + _param_rtl_return_alt.get()) || _enforce_rtl_alt) { - - // 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 start with climb - _rtl_state = RTL_STATE_RETURN; - } + _rtl_state = getActivationLandState(); // reset cruising speed and throttle to default for RTL _navigator->reset_cruising_speed(); _navigator->set_cruising_throttle(); set_rtl_item(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: start return at %d m (%d m above destination)\t", + (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); + events::send(events::ID("vrtl_return_at"), events::Log::Info, + "RTL: start return at {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); } void RtlDirect::on_active() { _global_pos_sub.update(); - _local_pos_sub.update(); - _land_detected_sub.update(); _vehicle_status_sub.update(); parameters_update(); - if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) { - advance_rtl(); + if (_rtl_state != RTLState::IDLE && is_mission_item_reached_or_completed()) { set_rtl_item(); } - if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { + if (_rtl_state == RTLState::LAND && _param_rtl_pld_md.get() > 0) { // Need to update the position and type on the current setpoint triplet. _navigator->get_precland()->on_active(); @@ -128,249 +122,204 @@ void RtlDirect::on_active() } } +void RtlDirect::setRtlPosition(DestinationPosition rtl_position, loiter_point_s loiter_pos) +{ + _home_pos_sub.update(); + + parameters_update(); + + // Only allow to set a new approach if the mode is not activated yet. + if (!isActive()) { + _land_approach = loiter_pos; + _destination = rtl_position; + _force_heading = false; + + // Input sanitation + if (!PX4_ISFINITE(_destination.lat) || !PX4_ISFINITE(_destination.lon)) { + // We don't have a valid rtl position, use the home position instead. + _destination.lat = _home_pos_sub.get().lat; + _destination.lon = _home_pos_sub.get().lon; + _destination.alt = _home_pos_sub.get().alt; + _destination.yaw = _home_pos_sub.get().yaw; + } + + if (!PX4_ISFINITE(_destination.alt)) { + // Not a valid rtl land altitude. Assume same altitude as home position. + _destination.alt = _home_pos_sub.get().alt; + } + + if (!PX4_ISFINITE(_land_approach.lat) || !PX4_ISFINITE(_land_approach.lon)) { + _land_approach.lat = _destination.lat; + _land_approach.lon = _destination.lon; + + } else { + const float dist_to_destination{get_distance_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon)}; + + if (dist_to_destination > _navigator->get_acceptance_radius()) { + _force_heading = true; + } + } + + if (!PX4_ISFINITE(_land_approach.height_m)) { + _land_approach.height_m = _destination.alt + _param_rtl_descend_alt.get(); + } + + if (!PX4_ISFINITE(_land_approach.loiter_radius_m) || fabsf(_land_approach.loiter_radius_m) <= FLT_EPSILON) { + _land_approach.loiter_radius_m = _param_rtl_loiter_rad.get(); + } + } +} + void RtlDirect::set_rtl_item() { position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon); - const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt); - const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); + HeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); + + if ((rtl_heading_mode == HeadingMode::NAVIGATION_HEADING) && (destination_dist < _param_rtl_min_dist.get())) { + rtl_heading_mode = HeadingMode::DESTINATION_HEADING; + } switch (_rtl_state) { - case RTL_STATE_CLIMB: { + case RTLState::CLIMBING: { + DestinationPosition dest { + .lat = _global_pos_sub.get().lat, + .lon = _global_pos_sub.get().lon, + .alt = _rtl_alt, + }; - _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius(), HeadingMode::CURRENT_HEADING); - _mission_item.lat = _global_pos_sub.get().lat; - _mission_item.lon = _global_pos_sub.get().lon; - _mission_item.altitude = _rtl_alt; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _local_pos_sub.get().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; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t", - (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); - events::send(events::ID("rtl_climb_to"), events::Log::Info, - "RTL: climb to {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); + _rtl_state = RTLState::MOVE_TO_LOITER; break; } - case RTL_STATE_RETURN: { + case RTLState::MOVE_TO_LOITER: { + DestinationPosition dest { + .lat = _land_approach.lat, + .lon = _land_approach.lon, + .alt = _rtl_alt, + .yaw = _destination.yaw, + }; // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status // can be displayed on groundstation and the WP is accepted once within loiter radius if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - + setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m, rtl_heading_mode); } else { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); } - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = _rtl_alt; // Don't change altitude - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING && - destination_dist > _param_rtl_min_dist.get()) { - _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().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 if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _local_pos_sub.get().heading; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); - events::send(events::ID("rtl_return_at"), events::Log::Info, - "RTL: return at {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); + _rtl_state = RTLState::LOITER_DOWN; break; } - 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; - _mission_item.altitude_is_relative = false; + case RTLState::LOITER_DOWN: { + DestinationPosition dest{ + .lat = _land_approach.lat, + .lon = _land_approach.lon, + .alt = loiter_altitude, + .yaw = _destination.yaw, + }; - // Except for vtol which might be still off here and should point towards this location. - const float d_current = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, - _mission_item.lat, _mission_item.lon); + setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m, rtl_heading_mode); - if (_vehicle_status_sub.get().is_vtol && (d_current > _navigator->get_acceptance_radius())) { - _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, - _mission_item.lat, _mission_item.lon); + pos_sp_triplet->next.valid = true; + pos_sp_triplet->next.lat = _destination.lat; + pos_sp_triplet->next.lon = _destination.lon; + pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_LAND; - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _local_pos_sub.get().heading; - - } else { - _mission_item.yaw = _destination.yaw; + if (_force_heading) { + _mission_item.force_heading = true; } - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); - // Disable previous setpoint to prevent drift. pos_sp_triplet->previous.valid = false; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); - events::send(events::ID("rtl_descend_to"), events::Log::Info, - "RTL: descend to {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); + _rtl_state = RTLState::LOITER_HOLD; + break; } - case RTL_STATE_LOITER: { - const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON); + case RTLState::LOITER_HOLD: { + DestinationPosition dest { + .lat = _land_approach.lat, + .lon = _land_approach.lon, + .alt = loiter_altitude, + .yaw = _destination.yaw, + }; - if (autocontinue) { - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t", - (double)_param_rtl_land_delay.get()); - events::send(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get()); + setLoiterHoldMissionItem(_mission_item, dest, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m, + rtl_heading_mode); - } else { - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + if (_param_rtl_land_delay.get() < -FLT_EPSILON) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); 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 = _local_pos_sub.get().heading; + if (_vehicle_status_sub.get().is_vtol + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + _rtl_state = RTLState::MOVE_TO_LAND; } else { - _mission_item.yaw = _destination.yaw; + _rtl_state = RTLState::MOVE_TO_LAND_HOVER; } - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); - _mission_item.autocontinue = autocontinue; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); - 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; + case RTLState::MOVE_TO_LAND: { - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { - _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, - _destination.lon); + DestinationPosition dest{_destination}; + dest.alt = loiter_altitude; - } 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 = _local_pos_sub.get().heading; - } + setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); + // Prepare for transition _mission_item.vtol_back_transition = true; - // acceptance_radius will be overwritten since vtol_back_transition is set, - // set as a default value only - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.force_heading = false; + + // set previous item location to loiter location such that vehicle tracks line between loiter + // location and land location after exiting the loiter circle + pos_sp_triplet->previous.lat = _land_approach.lat; + pos_sp_triplet->previous.lon = _land_approach.lon; + pos_sp_triplet->previous.alt = _mission_item.altitude; + pos_sp_triplet->previous.valid = true; + + _rtl_state = RTLState::TRANSITION_TO_MC; - // Disable previous setpoint to prevent drift. - pos_sp_triplet->previous.valid = false; break; } - case RTL_STATE_TRANSITION_TO_MC: { + case RTLState::TRANSITION_TO_MC: { set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + + _rtl_state = RTLState::MOVE_TO_LAND_HOVER; + break; } - case RTL_MOVE_TO_LAND_HOVER_VTOL: { - _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; + case RTLState::MOVE_TO_LAND_HOVER: { + DestinationPosition dest{_destination}; + dest.alt = loiter_altitude; - // have to reset here because these field were used in set_vtol_transition_item - _mission_item.time_inside = 0.f; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { - _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, - _destination.lon); + _rtl_state = RTLState::LAND; - } 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 = _local_pos_sub.get().heading; - } - - _mission_item.origin = ORIGIN_ONBOARD; break; } - case RTL_STATE_LAND: { - // Land at destination. - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = _destination.alt; - _mission_item.altitude_is_relative = false; + case RTLState::LAND: { - if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _local_pos_sub.get().heading; + setLandMissionItem(_mission_item, _destination, rtl_heading_mode); - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; _mission_item.land_precision = _param_rtl_pld_md.get(); if (_mission_item.land_precision == 1) { @@ -382,13 +331,16 @@ void RtlDirect::set_rtl_item() _navigator->get_precland()->on_activation(); } + _rtl_state = RTLState::IDLE; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); break; } - case RTL_STATE_LANDED: { + case RTLState::IDLE: { set_idle_item(&_mission_item); + _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); break; } @@ -401,85 +353,35 @@ void RtlDirect::set_rtl_item() // Execute command if set. This is required for commands like VTOL transition. if (!MissionBlock::item_contains_position(_mission_item)) { issue_command(_mission_item); - } - // Convert mission item to current position setpoint and make it valid. - mission_apply_limitation(_mission_item); + } else { + // 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)) { - _navigator->set_position_setpoint_triplet_updated(); + if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { + _navigator->set_position_setpoint_triplet_updated(); + } } } -void RtlDirect::advance_rtl() +RtlDirect::RTLState RtlDirect::getActivationLandState() { - // determines if the vehicle should loiter above land - const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA; + _land_detected_sub.update(); - // vehicle is a vtol and currently in fixed wing mode - const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol - && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + RTLState land_state; - switch (_rtl_state) { - case RTL_STATE_CLIMB: - _rtl_state = RTL_STATE_RETURN; - break; + if (_land_detected_sub.get().landed) { + // For safety reasons don't go into RTL if landed. + land_state = RTLState::IDLE; - case RTL_STATE_RETURN: - _rtl_state = RTL_STATE_DESCEND; - break; + } else if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) { + land_state = RTLState::CLIMBING; - case RTL_STATE_DESCEND: - - if (descend_and_loiter) { - _rtl_state = RTL_STATE_LOITER; - - } else if (vtol_in_fw_mode) { - _rtl_state = RTL_STATE_HEAD_TO_CENTER; - - } else { - _rtl_state = RTL_STATE_LAND; - } - - break; - - case RTL_STATE_LOITER: - - if (vtol_in_fw_mode) { - _rtl_state = RTL_STATE_HEAD_TO_CENTER; - - } else { - _rtl_state = RTL_STATE_LAND; - } - - break; - - case RTL_STATE_HEAD_TO_CENTER: - - _rtl_state = RTL_STATE_TRANSITION_TO_MC; - - break; - - case RTL_STATE_TRANSITION_TO_MC: - - _rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL; - - break; - - case RTL_MOVE_TO_LAND_HOVER_VTOL: - - _rtl_state = RTL_STATE_LAND; - - break; - - case RTL_STATE_LAND: - _rtl_state = RTL_STATE_LANDED; - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); - break; - - default: - break; + } else { + land_state = RTLState::MOVE_TO_LOITER; } + + return land_state; } rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() @@ -488,10 +390,13 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() rtl_time_estimate_s rtl_time_estimate{}; - RTLState start_state_for_estimate{RTL_STATE_NONE}; + RTLState start_state_for_estimate; if (isActive()) { start_state_for_estimate = _rtl_state; + + } else { + start_state_for_estimate = getActivationLandState(); } // Calculate RTL time estimate only when there is a valid home position @@ -501,11 +406,13 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() } else { rtl_time_estimate.valid = true; + rtl_time_estimate.time_estimate = 0.f; + + const float loiter_altitude = min(_land_approach.height_m, _rtl_alt); // Sum up time estimate for various segments of the landing procedure switch (start_state_for_estimate) { - case RTL_STATE_NONE: - case RTL_STATE_CLIMB: { + case RTLState::CLIMBING: { // Climb segment is only relevant if the drone is below return altitude const float climb_dist = _global_pos_sub.get().alt < _rtl_alt ? (_rtl_alt - _global_pos_sub.get().alt) : 0; @@ -515,30 +422,24 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() } // FALLTHROUGH - case RTL_STATE_RETURN: + case RTLState::MOVE_TO_LOITER: // Add cruise segment to home rtl_time_estimate.time_estimate += get_distance_to_next_waypoint( - _destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed(); + _land_approach.lat, _land_approach.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed(); // FALLTHROUGH - case RTL_STATE_HEAD_TO_CENTER: - case RTL_STATE_TRANSITION_TO_MC: - case RTL_STATE_DESCEND: { + case RTLState::LOITER_DOWN: { // when descending, the target altitude is stored in the current mission item float initial_altitude = 0.f; - float loiter_altitude = 0.f; - if (start_state_for_estimate == RTL_STATE_DESCEND) { + if (start_state_for_estimate == RTLState::LOITER_DOWN) { // Take current vehicle altitude as the starting point for calculation initial_altitude = _global_pos_sub.get().alt; // TODO: Check if this is in the right frame - loiter_altitude = _mission_item.altitude; // Next waypoint = loiter - } else { // Take the return altitude as the starting point for the calculation initial_altitude = _rtl_alt; // CLIMB and RETURN - loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); } // Add descend segment (first landing phase: return alt to loiter alt) @@ -546,18 +447,38 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() } // FALLTHROUGH - case RTL_STATE_LOITER: + case RTLState::LOITER_HOLD: // Add land delay (the short pause for deploying landing gear) // TODO: Check if landing gear is deployed or not rtl_time_estimate.time_estimate += _param_rtl_land_delay.get(); // FALLTHROUGH - case RTL_MOVE_TO_LAND_HOVER_VTOL: - case RTL_STATE_LAND: { + case RTLState::MOVE_TO_LAND: + case RTLState::TRANSITION_TO_MC: + case RTLState::MOVE_TO_LAND_HOVER: { + // Add cruise segment to home + float move_to_land_dist{0.f}; + + if (start_state_for_estimate >= RTLState::MOVE_TO_LAND) { + move_to_land_dist = get_distance_to_next_waypoint( + _destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + } else { + move_to_land_dist = get_distance_to_next_waypoint( + _destination.lat, _destination.lon, _land_approach.lat, _land_approach.lon); + } + + if (move_to_land_dist > FLT_EPSILON) { + rtl_time_estimate.time_estimate += move_to_land_dist / getCruiseGroundSpeed(); + } + } + + // FALLTHROUGH + case RTLState::LAND: { float initial_altitude; // Add land segment (second landing phase) which comes after LOITER - if (start_state_for_estimate == RTL_STATE_LAND) { + if (start_state_for_estimate == RTLState::LAND) { // If we are in this phase, use the current vehicle altitude instead // of the altitude paramteter to get a continous time estimate initial_altitude = _global_pos_sub.get().alt; @@ -566,7 +487,6 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() } else { // If this phase is not active yet, simply use the loiter altitude, // which is where the LAND phase will start - const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); initial_altitude = loiter_altitude; } @@ -578,7 +498,7 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() break; - case RTL_STATE_LANDED: + case RTLState::IDLE: // Remaining time is 0 break; } diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 94ada8f514..3a5597d4ad 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -41,11 +41,9 @@ #pragma once -#include - -#include "mission_block.h" - #include +#include +#include #include #include #include @@ -53,10 +51,12 @@ #include #include #include -#include #include #include -#include + +#include "mission_block.h" +#include "navigation.h" +#include "safe_point_land.hpp" using namespace time_literals; @@ -65,18 +65,6 @@ class Navigator; class RtlDirect : public MissionBlock, public ModuleParams { public: - /** - * @brief Return to launch position. - * Defines the position and landing yaw for the return to launch destination. - * - */ - struct RtlPosition { - double lat; /**< latitude in WGS84 [rad].*/ - double lon; /**< longitude in WGS84 [rad].*/ - float alt; /**< altitude in MSL [m].*/ - float yaw; /**< final yaw when landed [rad].*/ - }; - RtlDirect(Navigator *navigator); ~RtlDirect() = default; @@ -111,35 +99,24 @@ public: void setReturnAltMin(bool min) { _enforce_rtl_alt = min; } void setRtlAlt(float alt) {_rtl_alt = alt;}; - void setRtlPosition(RtlPosition position) {_destination = position;}; + void setRtlPosition(DestinationPosition position, loiter_point_s loiter_pos); private: - /** - * @brief Return to launch heading mode. - * - */ - enum RTLHeadingMode { - RTL_NAVIGATION_HEADING = 0, - RTL_DESTINATION_HEADING, - RTL_CURRENT_HEADING, - }; - /** * @brief Return to launch state machine. * */ - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LOITER, - RTL_STATE_TRANSITION_TO_MC, - RTL_MOVE_TO_LAND_HOVER_VTOL, - RTL_STATE_LAND, - RTL_STATE_LANDED, - RTL_STATE_HEAD_TO_CENTER, - }; + enum class RTLState { + CLIMBING, + MOVE_TO_LOITER, + LOITER_DOWN, + LOITER_HOLD, + MOVE_TO_LAND, + TRANSITION_TO_MC, + MOVE_TO_LAND_HOVER, + LAND, + IDLE + } _rtl_state{RTLState::IDLE}; /*< Current state in the state machine.*/ private: /** @@ -155,12 +132,6 @@ private: */ void set_rtl_item(); - /** - * @brief Advance the return to launch state machine. - * - */ - void advance_rtl(); - /** * @brief Get the Cruise Ground Speed * @@ -201,17 +172,19 @@ private: */ void parameters_update(); - /** Current state in the state machine.*/ - RTLState _rtl_state{RTL_STATE_NONE}; + RTLState getActivationLandState(); + + void setLoiterPosition(); bool _enforce_rtl_alt{false}; + bool _force_heading{false}; - RtlPosition _destination{}; ///< the RTL position to fly to + DestinationPosition _destination; ///< the RTL position to fly to + loiter_point_s _land_approach; float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position DEFINE_PARAMETERS( - (ParamFloat) _param_rtl_return_alt, (ParamFloat) _param_rtl_descend_alt, (ParamFloat) _param_rtl_land_delay, (ParamFloat) _param_rtl_min_dist, @@ -234,8 +207,8 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - uORB::SubscriptionData _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< vehicle status subscription */ uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; }; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index ef78a37020..7b4b573bc3 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -200,3 +200,13 @@ PARAM_DEFINE_FLOAT(RTL_TIME_FACTOR, 1.1f); * @group Return To Land */ PARAM_DEFINE_INT32(RTL_TIME_MARGIN, 100); + +/** + * RTL force approach landing + * + * Only consider RTL point, if it has an approach defined. + * + * @boolean + * @group Return To Land + */ +PARAM_DEFINE_INT32(RTL_APPR_FORCE, 0); diff --git a/src/modules/navigator/safe_point_land.hpp b/src/modules/navigator/safe_point_land.hpp new file mode 100644 index 0000000000..6b80132175 --- /dev/null +++ b/src/modules/navigator/safe_point_land.hpp @@ -0,0 +1,110 @@ +/*************************************************************************** + * + * Copyright (c) 2021 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file safe_point_land.hpp + * This file defines helper structs that are used to define land approaches which consists of a land location and a number of + * loiter circles. Each loiter circle defines a possible approach for landing at the land location. + * + */ + +#pragma once + +#include +#include + +struct loiter_point_s { + loiter_point_s() { reset(); } + double lat; + double lon; + float height_m; + float loiter_radius_m; + + void reset() + { + lat = lon = static_cast(NAN); + height_m = NAN; + loiter_radius_m = NAN; + } + + bool isValid() const { return PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(height_m); } +}; + +// defines one land location and a maximum of num_approaches_max loiter points +struct land_approaches_s { + + static constexpr uint8_t num_approaches_max = 8; + loiter_point_s approaches[num_approaches_max]; + matrix::Vector2d land_location_lat_lon; + + land_approaches_s() + { + resetAllApproaches(); + } + + void resetAllApproaches() + { + for (uint8_t i = 0; i < num_approaches_max; i++) { + approaches[i].reset(); + } + } + + bool isAnyApproachValid() const + { + for (uint8_t i = 0; i < num_approaches_max; i++) { + if (approaches[i].isValid()) { + return true; + } + } + + return false; + } + + float getMaxDistLandToLoiterCircle() const + { + // returns negative infinity if there is no valid approach + float dist_max = -INFINITY; + + for (uint8_t i = 0; i < num_approaches_max; i++) { + if (approaches[i].isValid()) { + float dist = get_distance_to_next_waypoint(land_location_lat_lon(0), land_location_lat_lon(1), approaches[i].lat, + approaches[i].lon) + approaches[i].loiter_radius_m; + + if (dist > dist_max) { + dist_max = dist; + } + } + } + + return dist_max; + } +};