diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/CMakeLists.txt new file mode 100644 index 0000000000..d5f9f0efa0 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2022 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. +# +############################################################################ + +px4_add_library(FlightTaskAutoPrecisionLanding +FlightTaskAutoPrecisionLanding.cpp +) + +target_link_libraries(FlightTaskAutoPrecisionLanding PUBLIC FlightTaskAuto) +target_include_directories(FlightTaskAutoPrecisionLanding PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/navigator/precland.cpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp similarity index 100% rename from src/modules/navigator/precland.cpp rename to src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp diff --git a/src/modules/navigator/precland.h b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp similarity index 100% rename from src/modules/navigator/precland.h rename to src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 9075f34926..6d2e46f977 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -45,7 +45,6 @@ px4_add_module( rtl.cpp takeoff.cpp land.cpp - precland.cpp mission_feasibility_checker.cpp geofence.cpp enginefailure.cpp diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 77186d89f9..ab63001da8 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -175,10 +175,6 @@ Mission::on_inactivation() _navigator->release_gimbal_control(); - if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } - _time_mission_deactivated = hrt_absolute_time(); /* reset so current mission item gets restarted if mission was paused */ @@ -295,13 +291,6 @@ Mission::on_active() do_abort_landing(); } - - if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) { - _navigator->get_precland()->on_active(); - - } else if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } } bool @@ -978,40 +967,6 @@ Mission::set_mission_items() // if the vehicle drifted off the path during back-transition it should just go straight to the landing point pos_sp_triplet->previous.valid = false; - } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT) { - if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { - new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - - } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); - } - - _navigator->get_precland()->on_activation(); - - } - } - - /* we just moved to the landing waypoint, now descend */ - if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { - new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - - } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); - } - - _navigator->get_precland()->on_activation(); - - } - } /* ignore yaw for landing items */ @@ -1147,10 +1102,8 @@ Mission::set_mission_items() } else { // The mission item is not a gate, set the current position setpoint from mission item (is protected against non-position items) // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) { - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); - } + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); // ELSE: The current position setpoint stays unchanged. } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 24dd8bf1c9..259f9c1b47 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -279,8 +279,7 @@ private: WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, - WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, - WORK_ITEM_TYPE_PRECISION_LAND + WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION } _work_item_type{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL}; /**< the current mode of how the mission is executed,look at mission_result.msg for the definition */ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 3cd0bb5cc3..fff5af1d68 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -45,7 +45,6 @@ #include "follow_target.h" #include "geofence.h" #include "land.h" -#include "precland.h" #include "loiter.h" #include "mission.h" #include "navigator_mode.h" @@ -86,7 +85,7 @@ using namespace time_literals; /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 9 +#define NAVIGATOR_MODE_ARRAY_SIZE 8 class Navigator : public ModuleBase, public ModuleParams { @@ -166,8 +165,6 @@ public: vehicle_local_position_s *get_local_position() { return &_local_pos; } vehicle_status_s *get_vstatus() { return &_vstatus; } - PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */ - const vehicle_roi_s &get_vroi() { return _vroi; } void reset_vroi() { _vroi = {}; } @@ -391,7 +388,6 @@ private: Takeoff _takeoff; /**< class for handling takeoff commands */ VtolTakeoff _vtol_takeoff; /**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */ Land _land; /**< class for handling land commands */ - PrecLand _precland; /**< class for handling precision land commands */ RTL _rtl; /**< class that handles RTL */ EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */ FollowTarget _follow_target; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index db6d5fc340..36b51c2a65 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -76,7 +76,6 @@ Navigator::Navigator() : _takeoff(this), _vtol_takeoff(this), _land(this), - _precland(this), _rtl(this), _engineFailure(this), _follow_target(this) @@ -88,9 +87,8 @@ Navigator::Navigator() : _navigation_mode_array[3] = &_engineFailure; _navigation_mode_array[4] = &_takeoff; _navigation_mode_array[5] = &_land; - _navigation_mode_array[6] = &_precland; - _navigation_mode_array[7] = &_vtol_takeoff; - _navigation_mode_array[8] = &_follow_target; + _navigation_mode_array[6] = &_vtol_takeoff; + _navigation_mode_array[7] = &_follow_target; _handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS"); _handle_reverse_delay = param_find("VT_B_REV_DEL"); @@ -688,12 +686,6 @@ void Navigator::run() navigation_mode_new = &_land; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: - _pos_sp_triplet_published_invalid_once = false; - navigation_mode_new = &_precland; - _precland.set_mode(PrecLandMode::Required); - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_engineFailure; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 54978d8a1f..f2c0c23b31 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -69,9 +69,7 @@ RTL::RTL(Navigator *navigator) : void RTL::on_inactivation() { - if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } + } void RTL::on_inactive() @@ -309,13 +307,6 @@ void RTL::on_active() set_rtl_item(); } - if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { - _navigator->get_precland()->on_active(); - - } else if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } - // Limit rtl time calculation to 1Hz if ((hrt_absolute_time() - _destination_check_time) > 1_s) { _destination_check_time = hrt_absolute_time(); @@ -561,16 +552,6 @@ void RTL::set_rtl_item() _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) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - _navigator->get_precland()->on_activation(); - - } else if (_mission_item.land_precision == 2) { - _navigator->get_precland()->set_mode(PrecLandMode::Required); - _navigator->get_precland()->on_activation(); - } 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"); diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 42f8f320f5..f8484ae623 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -171,7 +171,6 @@ private: (ParamFloat) _param_rtl_min_dist, (ParamInt) _param_rtl_type, (ParamInt) _param_rtl_cone_half_angle_deg, - (ParamInt) _param_rtl_pld_md, (ParamFloat) _param_rtl_loiter_rad, (ParamInt) _param_rtl_hdg_md, (ParamFloat) _param_rtl_time_factor,