From 0e53a8a915be162c65e1f60a542fda9d7ca31295 Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Thu, 6 Jan 2022 12:32:57 +0100 Subject: [PATCH] moved precland from navigator to flighttasks PrecisionLanding flight task is not being compiled yet. See next commit. --- .../tasks/AutoPrecisionLanding/CMakeLists.txt | 39 ++++++++++++++ .../FlightTaskAutoPrecisionLanding.cpp} | 0 .../FlightTaskAutoPrecisionLanding.hpp} | 0 src/modules/navigator/CMakeLists.txt | 1 - src/modules/navigator/mission.cpp | 51 +------------------ src/modules/navigator/mission.h | 3 +- src/modules/navigator/navigator.h | 6 +-- src/modules/navigator/navigator_main.cpp | 10 +--- src/modules/navigator/rtl.cpp | 21 +------- src/modules/navigator/rtl.h | 1 - 10 files changed, 45 insertions(+), 87 deletions(-) create mode 100644 src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/CMakeLists.txt rename src/modules/{navigator/precland.cpp => flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp} (100%) rename src/modules/{navigator/precland.h => flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp} (100%) 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 30725a05d0..cf9c2e8798 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -46,7 +46,6 @@ px4_add_module( rtl.cpp takeoff.cpp land.cpp - precland.cpp mission_feasibility_checker.cpp geofence.cpp vtol_takeoff.cpp diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index baef5b3304..0c6ab11e7b 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -176,10 +176,6 @@ Mission::on_inactivation() _navigator->stop_capturing_images(); _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 */ @@ -296,13 +292,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 */ @@ -1145,10 +1100,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 821b6b25a7..3b004a8b48 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -287,8 +287,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_TRANSITION_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 d80beb9333..a37ffaeb77 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -43,7 +43,6 @@ #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 8 +#define NAVIGATOR_MODE_ARRAY_SIZE 6 class Navigator : public ModuleBase, public ModuleParams { @@ -159,8 +158,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 = {}; } @@ -374,7 +371,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 */ AdsbConflict _adsb_conflict; /**< class that handles ADSB conflict avoidance */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 10349a6cb8..2bde309e32 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -77,7 +77,6 @@ Navigator::Navigator() : _takeoff(this), _vtol_takeoff(this), _land(this), - _precland(this), _rtl(this) { /* Create a list of our possible navigation types */ @@ -86,8 +85,7 @@ Navigator::Navigator() : _navigation_mode_array[2] = &_rtl; _navigation_mode_array[3] = &_takeoff; _navigation_mode_array[4] = &_land; - _navigation_mode_array[5] = &_precland; - _navigation_mode_array[6] = &_vtol_takeoff; + _navigation_mode_array[5] = &_vtol_takeoff; /* iterate through navigation modes and initialize _mission_item for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { @@ -780,12 +778,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_MANUAL: case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index ccc4bc4d8f..b0856d8ab4 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() @@ -314,13 +312,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(); @@ -589,16 +580,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 159addd71f..ca14771308 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -170,7 +170,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,