From 81105d256ac434f24256365816278d4fe4396257 Mon Sep 17 00:00:00 2001 From: Konrad Date: Fri, 13 Jan 2023 10:20:06 +0100 Subject: [PATCH] [RTL] Add VTOL land approach for home with designated loiter points in the RTL mode. --- src/modules/navigator/CMakeLists.txt | 1 + src/modules/navigator/navigator_main.cpp | 10 +- src/modules/navigator/rtl.cpp | 71 ++++- src/modules/navigator/rtl.h | 14 +- src/modules/navigator/safe_point_land.hpp | 110 ++++++++ src/modules/navigator/vtol_land.cpp | 329 ++++++++++++++++++++++ src/modules/navigator/vtol_land.h | 106 +++++++ 7 files changed, 629 insertions(+), 12 deletions(-) create mode 100644 src/modules/navigator/safe_point_land.hpp create mode 100644 src/modules/navigator/vtol_land.cpp create mode 100644 src/modules/navigator/vtol_land.h diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index c0c27916c6..8682b7154b 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -51,6 +51,7 @@ px4_add_module( precland.cpp mission_feasibility_checker.cpp geofence.cpp + vtol_land.cpp vtol_takeoff.cpp DEPENDS geo diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 79581ce25f..e6b2c01f1d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -666,7 +666,15 @@ void Navigator::run() case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; - navigation_mode_new = &_rtl; + + // 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 { + navigation_mode_new = &_rtl; + } + break; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b482601787..8a63730e47 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -54,7 +54,8 @@ RTL::RTL(Navigator *navigator) : ModuleParams(navigator), _rtl_direct(navigator), _rtl_mission(navigator), - _rtl_mission_reverse(navigator) + _rtl_mission_reverse(navigator), + _rtl_vtol_land(navigator) { initMission(); } @@ -74,6 +75,10 @@ void RTL::on_inactivation() _rtl_direct.on_inactivation(); break; + case RtlType::RTL_DIRECT_SAFE_VTOL: + _rtl_vtol_land.on_inactivation(); + break; + default: break; } @@ -88,6 +93,7 @@ void RTL::on_inactive() _rtl_mission.on_inactive(); _rtl_mission_reverse.on_inactive(); _rtl_direct.on_inactive(); + _rtl_vtol_land.on_inactive(); // Limit inactive calculation to 1Hz hrt_abstime now{hrt_absolute_time()}; @@ -111,6 +117,10 @@ void RTL::on_inactive() estimated_time = _rtl_mission_reverse.calc_rtl_time_estimate(); break; + case RtlType::RTL_DIRECT_SAFE_VTOL: + _rtl_vtol_land.calc_rtl_time_estimate(); + break; + default: break; } @@ -136,6 +146,10 @@ void RTL::on_activation() _rtl_direct.on_activation(_enforce_rtl_alt); break; + case RtlType::RTL_DIRECT_SAFE_VTOL: + _rtl_vtol_land.on_activation(); + break; + default: break; } @@ -152,18 +166,28 @@ void RTL::on_active() _rtl_mission.on_active(); _rtl_mission_reverse.on_inactive(); _rtl_direct.on_inactive(); + _rtl_vtol_land.on_inactive(); break; case RtlType::RTL_MISSION_FAST_REVERSE: _rtl_mission_reverse.on_active(); _rtl_mission.on_inactive(); _rtl_direct.on_inactive(); + _rtl_vtol_land.on_inactive(); break; case RtlType::RTL_DIRECT: _rtl_direct.on_active(); _rtl_mission_reverse.on_inactive(); _rtl_mission.on_inactive(); + _rtl_vtol_land.on_inactive(); + break; + + case RtlType::RTL_DIRECT_SAFE_VTOL: + _rtl_vtol_land.on_active(); + _rtl_mission_reverse.on_inactive(); + _rtl_mission.on_inactive(); + _rtl_direct.on_inactive(); break; default: @@ -185,31 +209,58 @@ void RTL::setRtlTypeAndDestination() } else { // check the closest allowed destination. - bool isMissionLanding{false}; + DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME}; RtlDirect::RtlPosition 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_mission.setInitialMissionIndex(_land_start_index); _rtl_type = RtlType::RTL_MISSION_FAST; + break; - } else { + case DestinationType::DESTINATION_TYPE_SAFE_POINT: _rtl_direct.setRtlAlt(rtl_alt); _rtl_direct.setRtlPosition(rtl_position); _rtl_type = RtlType::RTL_DIRECT; + break; + + case DestinationType::DESTINATION_TYPE_HOME: { + // check if we can apply vtol land. + bool vtol_land{_vehicle_status_sub.get().is_vtol && + _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && + _rtl_vtol_land.hasVtolHomeLandApproach()}; + + if (vtol_land) { + _rtl_type = RtlType::RTL_DIRECT_SAFE_VTOL; + + } else { + _rtl_direct.setRtlAlt(rtl_alt); + _rtl_direct.setRtlPosition(rtl_position); + _rtl_type = RtlType::RTL_DIRECT; + } + + break; + } + + default: + _rtl_direct.setRtlAlt(rtl_alt); + _rtl_direct.setRtlPosition(rtl_position); + _rtl_type = RtlType::RTL_DIRECT; + break; } } } -void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt) +void RTL::findRtlDestination(DestinationType &destination_type, RtlDirect::RtlPosition &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; // get distance to home position float min_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)}; @@ -226,12 +277,12 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl // Use the mission landing destination. min_dist = dist; setLandPosAsDestination(rtl_position); - isMissionLanding = true; + destination_type = DestinationType::DESTINATION_TYPE_MISSION_LAND; } else if (dist < min_dist) { min_dist = dist; setLandPosAsDestination(rtl_position); - isMissionLanding = true; + destination_type = DestinationType::DESTINATION_TYPE_MISSION_LAND; } } @@ -263,7 +314,7 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl if (dist < min_dist) { min_dist = dist; setSafepointAsDestination(rtl_position, mission_safe_point); - isMissionLanding = false; + destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT; } } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 47425035b9..c11df483fb 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -49,6 +49,7 @@ #include "rtl_direct.h" #include "rtl_mission_fast.h" #include "rtl_mission_fast_reverse.h" +#include "vtol_land.h" #include #include @@ -65,8 +66,10 @@ public: enum class RtlType { RTL_DIRECT, + RTL_DIRECT_SAFE_VTOL, RTL_MISSION_FAST, RTL_MISSION_FAST_REVERSE, + VTOL_LAND }; void on_inactivation() override; @@ -78,6 +81,13 @@ public: void set_return_alt_min(bool min) { _enforce_rtl_alt = min; } +private: + enum class DestinationType { + DESTINATION_TYPE_HOME, + DESTINATION_TYPE_MISSION_LAND, + DESTINATION_TYPE_SAFE_POINT, + }; + private: void onMissionUpdate(bool has_mission_items_changed) override {}; @@ -87,7 +97,7 @@ private: * @brief Find RTL destination. * */ - void findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt); + void findRtlDestination(DestinationType &destination_type, RtlDirect::RtlPosition &rtl_position, float &rtl_alt); /** * @brief Set the position of the land start marker in the planned mission as destination. @@ -120,6 +130,8 @@ private: RtlMissionFastReverse _rtl_mission_reverse; + VtolLand _rtl_vtol_land; + bool _enforce_rtl_alt{false}; DEFINE_PARAMETERS( 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; + } +}; diff --git a/src/modules/navigator/vtol_land.cpp b/src/modules/navigator/vtol_land.cpp new file mode 100644 index 0000000000..62daa43cbf --- /dev/null +++ b/src/modules/navigator/vtol_land.cpp @@ -0,0 +1,329 @@ +/**************************************************************************** + * + * 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 vtol_land.cpp + * + * Helper class to do a VTOL landing using a loiter down to altitude landing pattern. + * + */ + +#include "vtol_land.h" +#include "navigator.h" +#include "navigation.h" + +#include "modules/dataman/dataman.h" + +using matrix::wrap_pi; + +VtolLand::VtolLand(Navigator *navigator) : + MissionBlock(navigator), + ModuleParams(navigator) +{ +} + +void +VtolLand::on_activation() +{ + _global_pos_sub.update(); + _home_pos_sub.update(); + _wind_sub.update(); + + _land_pos_lat_lon = matrix::Vector2(_home_pos_sub.get().lat, _home_pos_sub.get().lon); + readVtolHomeLandApproachesFromStorage(); + set_loiter_position(); + _land_state = vtol_land_state::MOVE_TO_LOITER; + +} + +void VtolLand::on_inactive() +{ + _global_pos_sub.update(); + _home_pos_sub.update(); + _wind_sub.update(); +} + +void +VtolLand::on_active() +{ + _global_pos_sub.update(); + _home_pos_sub.update(); + _wind_sub.update(); + + if (is_mission_item_reached_or_completed()) { + switch (_land_state) { + case vtol_land_state::MOVE_TO_LOITER: { + _mission_item.altitude = _home_pos_sub.get().alt + _land_approach.height_m; + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + _navigator->get_mission_result()->finished = false; + _navigator->set_mission_result_updated(); + reset_mission_item_reached(); + + // convert mission item to current setpoint + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + pos_sp_triplet->next.valid = true; + pos_sp_triplet->next.lat = _land_pos_lat_lon(0); + pos_sp_triplet->next.lon = _land_pos_lat_lon(1); + pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_LAND; + + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.yaw_valid = true; + + _land_state = vtol_land_state::LOITER_DOWN; + break; + } + + case vtol_land_state::LOITER_DOWN: { + _mission_item.lat = _land_pos_lat_lon(0); + _mission_item.lon = _land_pos_lat_lon(1); + + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.vtol_back_transition = true; + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + // 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 = _loiter_pos_lat_lon(0); + pos_sp_triplet->previous.lon = _loiter_pos_lat_lon(1); + pos_sp_triplet->previous.alt = _mission_item.altitude; + pos_sp_triplet->previous.valid = true; + + //publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + // issue_command(_mission_item); + reset_mission_item_reached(); + + _land_state = vtol_land_state::TRANSITION_TO_MC; + + break; + } + + case vtol_land_state::TRANSITION_TO_MC: { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.vtol_back_transition = true; + + issue_command(_mission_item); + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + //publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + // issue_command(_mission_item); + reset_mission_item_reached(); + + _land_state = vtol_land_state::LAND; + + break; + } + + case vtol_land_state::LAND: { + _mission_item.nav_cmd = NAV_CMD_LAND; + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + //publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + // issue_command(_mission_item); + reset_mission_item_reached(); + + _land_state = vtol_land_state::IDLE; + + break; + } + + default: { + + break; + } + } + } +} + +bool VtolLand::hasVtolHomeLandApproach() +{ + readVtolHomeLandApproachesFromStorage(); + return _vtol_home_land_approaches.isAnyApproachValid(); +} + +void +VtolLand::set_loiter_position() +{ + + _land_approach = chooseBestLandingApproach(); + + if (PX4_ISFINITE(_land_approach.lat) && PX4_ISFINITE(_land_approach.lon)) { + _loiter_pos_lat_lon(0) = _land_approach.lat; + _loiter_pos_lat_lon(1) = _land_approach.lon; + + } else { + _loiter_pos_lat_lon(0) = _home_pos_sub.get().lat; + _loiter_pos_lat_lon(1) = _home_pos_sub.get().lon; + } + + + _mission_item.lat = _loiter_pos_lat_lon(0); + _mission_item.lon = _loiter_pos_lat_lon(1); + _mission_item.altitude = math::max(_home_pos_sub.get().alt + _param_return_alt_rel_m.get(), + _global_pos_sub.get().alt); + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.force_heading = true; + _mission_item.autocontinue = false; + _mission_item.time_inside = _min_loiter_time_before_land; + + _mission_item.altitude_is_relative = false; + + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->get_mission_result()->finished = false; + _navigator->set_mission_result_updated(); + reset_mission_item_reached(); + + // convert mission item to current setpoint + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + _navigator->set_can_loiter_at_sp(true); + + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +loiter_point_s VtolLand::chooseBestLandingApproach() +{ + 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_home_land_approaches.num_approaches_max; i++) { + + if (_vtol_home_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_home_land_approaches.approaches[i].lat, + _vtol_home_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_home_land_approaches.approaches[min_index]; + + } else { + + return loiter_point_s(); + } +} + +void VtolLand::readVtolHomeLandApproachesFromStorage() +{ + + // 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 + _vtol_home_land_approaches.resetAllApproaches(); + + if (!(_home_pos_sub.get().valid_alt && _home_pos_sub.get().valid_hpos)) { + return; + } + + mission_stats_entry_s stats; + int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + int num_mission_items = 0; + bool foundHomeLandApproaches = false; + uint8_t sector_counter = 0; + + if (ret == sizeof(mission_stats_entry_s)) { + num_mission_items = stats.num_items; + } + + for (int current_seq = 1; current_seq <= num_mission_items; ++current_seq) { + mission_item_s mission_item{}; + + if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_item, sizeof(mission_item_s)) != + sizeof(mission_item_s)) { + PX4_ERR("dm_read failed"); + break; + } + + if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) { + + if (foundHomeLandApproaches) { + break; + } + + const float dist_to_home = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, _home_pos_sub.get().lat, + _home_pos_sub.get().lon); + + if (!mission_item.is_mission_rally_point && dist_to_home < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { + foundHomeLandApproaches = true; + _vtol_home_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_home_land_approaches.approaches[sector_counter].lat = mission_item.lat; + _vtol_home_land_approaches.approaches[sector_counter].lon = mission_item.lon; + _vtol_home_land_approaches.approaches[sector_counter].height_m = mission_item.altitude; + _vtol_home_land_approaches.approaches[sector_counter].loiter_radius_m = mission_item.loiter_radius; + sector_counter++; + } + } +} + +rtl_time_estimate_s VtolLand::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/vtol_land.h b/src/modules/navigator/vtol_land.h new file mode 100644 index 0000000000..d053c71e0f --- /dev/null +++ b/src/modules/navigator/vtol_land.h @@ -0,0 +1,106 @@ +/*************************************************************************** + * + * 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 vtol_land.h + * + * Helper class to do a VTOL landing using a loiter down to altitude landing pattern. + * + */ + +#pragma once + +#include "navigator_mode.h" +#include "mission_block.h" +#include "safe_point_land.hpp" + +#include + +#include +#include +#include +#include +#include + +#include + +class VtolLand : public MissionBlock, public ModuleParams +{ +public: + VtolLand(Navigator *navigator); + ~VtolLand() = default; + + void on_activation() override; + void on_active() override; + void on_inactive() override; + + bool hasVtolHomeLandApproach(); + + rtl_time_estimate_s calc_rtl_time_estimate(); +private: + + enum class vtol_land_state { + MOVE_TO_LOITER = 0, + LOITER_DOWN, + TRANSITION_TO_MC, + LAND, + IDLE + } _land_state; + + matrix::Vector2 _land_pos_lat_lon; + matrix::Vector2 _loiter_pos_lat_lon; + loiter_point_s _land_approach; + + static constexpr float _min_loiter_time_before_land = 10.0f; + + DEFINE_PARAMETERS( + (ParamFloat) _param_loiter_alt, + (ParamFloat) _param_return_alt_rel_m, + (ParamFloat) _param_rtl_loiter_rad + ) + + void set_loiter_position(); + + loiter_point_s chooseBestLandingApproach(); + + land_approaches_s _vtol_home_land_approaches{}; + + void readVtolHomeLandApproachesFromStorage(); + + static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES = + 10.0; // [m] We don't consider home land approaches valid if the distance from the current home to the land location is greater than this distance + + 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 _wind_sub{ORB_ID(wind)}; + +};