From 759d33d00e80ed7b121abd68843d37dd1d980d7b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 16 Feb 2023 13:28:49 +0300 Subject: [PATCH] Revert "cleanup" This reverts commit b00efcd9663e93f739046a66934e9f12d5e2e1af. --- .../MissionFeasibility/FeasibilityChecker.cpp | 68 +++++------ .../MissionFeasibility/FeasibilityChecker.hpp | 108 +----------------- .../FeasibilityCheckerTest.cpp | 21 +--- .../navigator/mission_feasibility_checker.cpp | 5 +- 4 files changed, 41 insertions(+), 161 deletions(-) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index bb8b3b9f61..a1eb0e51ad 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -150,22 +150,13 @@ void FeasibilityChecker::updateData() } } -bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int current_index, const int total_count) +void FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int current_index, const int total_count) { if (current_index == 0) { reset(); updateData(); } - if (!_mission_validity_failed) { - _mission_validity_failed = !checkMissionItemValidity(mission_item, current_index); - } - - if (_mission_validity_failed) { - // if a mission item is not valid then abort the other checks - return false; - } - doCommonChecks(mission_item, current_index); if (_vehicle_type == VehicleType::Vtol) { @@ -183,12 +174,13 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int } _mission_item_previous = mission_item; - - return true; } void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int current_index) { + if (!_mission_validity_failed) { + _mission_validity_failed = !checkMissionItemValidity(mission_item, current_index); + } if (!_distance_between_waypoints_failed) { _distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); @@ -235,20 +227,6 @@ void FeasibilityChecker::doMulticopterChecks(mission_item_s &mission_item, const bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item, const int current_index) { - /* reject relative alt without home set */ - if (mission_item.altitude_is_relative && !PX4_ISFINITE(_home_alt_msl) - && MissionBlock::item_contains_position(mission_item)) { - - - - mavlink_log_critical(_mavlink_log_pub, "Mission rejected: No home pos, WP %d uses rel alt\t", current_index + 1); - events::send(events::ID("navigator_mis_no_home_rel_alt"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: No home position, waypoint {1} uses relative altitude", - current_index + 1); - return false; - - } - // check if we find unsupported items and reject mission if so if (mission_item.nav_cmd != NAV_CMD_IDLE && mission_item.nav_cmd != NAV_CMD_WAYPOINT && @@ -339,16 +317,23 @@ bool FeasibilityChecker::checkTakeoff(mission_item_s &mission_item) // look for a takeoff waypoint if (mission_item.nav_cmd == NAV_CMD_TAKEOFF || mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { - // make sure that the altitude of the waypoint is above the home altitude - const float takeoff_alt = mission_item.altitude_is_relative - ? mission_item.altitude - : mission_item.altitude - _home_alt_msl; + // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius + // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air - if (takeoff_alt < FLT_EPSILON) { - mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff altitude below home altitude!\t"); + float takeoff_alt = mission_item.altitude_is_relative + ? mission_item.altitude + : mission_item.altitude - _home_alt_msl; + + float acceptance_radius = _param_nav_acc_rad; + + if (mission_item.acceptance_radius > NAV_EPSILON_POSITION) { + acceptance_radius = mission_item.acceptance_radius; + } + + if (takeoff_alt - 1.0f < acceptance_radius) { + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff altitude too low!\t"); events::send(events::ID("navigator_mis_takeoff_too_low"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: takeoff altitude too low! Minimum: {1:.1m_v}", - mission_item.altitude_is_relative ? 0.0f : _home_alt_msl); + "Mission rejected: takeoff altitude too low! Minimum: {1:.1m_v}", acceptance_radius + 1.f); return false; } @@ -725,6 +710,21 @@ bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mi bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index) { + + /* reject relative alt without home set */ + if (mission_item.altitude_is_relative && !PX4_ISFINITE(_home_alt_msl) + && MissionBlock::item_contains_position(mission_item)) { + + + + mavlink_log_critical(_mavlink_log_pub, "Mission rejected: No home pos, WP %d uses rel alt\t", current_index + 1); + events::send(events::ID("navigator_mis_no_home_rel_alt"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: No home position, waypoint {1} uses relative altitude", + current_index + 1); + return false; + + } + /* calculate the global waypoint altitude */ float wp_alt = (mission_item.altitude_is_relative) ? mission_item.altitude + _home_alt_msl : mission_item.altitude; diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index 2f5f380250..facaa25765 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -54,25 +54,13 @@ public: Other }; - /** - * @brief Run validity checks for mission item - * - * - * @param mission_item The mission item to run the checks on - * @param current_index The index of the current mission item in the mission - * @param total_count The total number of mission items in the mission - * @return False if fatal error occured and no other checks should be run. - */ - bool processNextItem(mission_item_s &mission_item, const int current_index, const int total_count); + void processNextItem(mission_item_s &mission_item, const int current_index, const int total_count); void setMavlinkLogPub(orb_advert_t *mavlink_log_pub) { _mavlink_log_pub = mavlink_log_pub; } - /** - * @return True At least one check failed. - */ bool someCheckFailed() { return _takeoff_failed || @@ -85,9 +73,6 @@ public: _takeoff_land_available_failed; } - /** - * @brief Reset all data - */ void reset(); private: @@ -138,113 +123,22 @@ private: double _last_lon{(double)NAN}; int _last_cmd{-1}; - /** - * @brief Update data from external topics, e.g home position - */ void updateData(); // methods which are called for each mission item - - /** - * @brief Check general mission item validity, e.g. supported commands. - * - * @param mission_item The current mission item - * @return False if the check failed. - */ bool checkMissionItemValidity(mission_item_s &mission_item, const int current_index); - - /** - * @brief Check if takeoff is valid - * - * @param mission_item The current mission item - * @return False if the check failed. - */ bool checkTakeoff(mission_item_s &mission_item); - - /** - * @brief Check validity of landing pattern (fixed wing & vtol) - * - * @param mission_item The current mission item - * @param current_index The current mission index - * @param last_index The last index of the mission - * @return False if the check failed. - */ bool checkLandPatternValidity(mission_item_s &mission_item, const int current_index, const int last_index); - - /** - * @brief Check distance to first waypoint. - * - * @param mission_item The current mission item - * @return False if the check failed. - */ bool checkDistanceToFirstWaypoint(mission_item_s &mission_item); - - /** - * @brief Check distances between waypoints - * - * @param mission_item The current mission item - * @return False if the check failed. - */ bool checkDistancesBetweenWaypoints(const mission_item_s &mission_item); - - /** - * @brief Check if any waypoint is below the home altitude. Issues warning only. - * - * @param mission_item The current mission item - * @param current_index The current mission index - * @return Always returns true, only issues warning. - */ bool checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index); - - /** - * @brief Check fixed wing land approach (fixed wing only) - * - * @param mission_item The current mission item - * @param current_index The current mission index - * @return False if the check failed. - */ bool checkFixedWindLandApproach(mission_item_s &mission_item, const int current_index); // methods which are called once at the end - /** - * @brief Check if takeoff/landing are available according to MIS_TKO_LAND_REQ parameter - * - * @return False if the check failed. - */ bool checkTakeoffLandAvailable(); - /** - * @brief Run checks which are common for all vehicle types - * - * @param mission_item The current mission item - * @param current_index The current mission index - */ void doCommonChecks(mission_item_s &mission_item, const int current_index); - - /** - * @brief Run checks which are only related to VTOL vehicles. - * - * @param mission_item The current mission item - * @param current_index The current mission index - */ void doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index); - - /** - * @brief Run checks which are only related to fixed wing vehicles. - * - * @param mission_item The current mission item - * @param current_index The current mission index - * @param last_index The last mission index - * @return False if the check failed. - */ void doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index); - - /** - * @brief Run checks which are only related to multirotor vehicles. - * - * @param mission_item The current mission item - * @param current_index The current mission index - * @return False if the check failed. - */ void doMulticopterChecks(mission_item_s &mission_item, const int current_index); }; diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index 666aebdb30..ba8ea34643 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -105,22 +105,10 @@ TEST_F(FeasibilityCheckerTest, mission_item_validity) // when landed the first item cannot be a land item checker.reset(); - checker.publishHomePosition(0, 0, 100.f); checker.publishLanded(true); mission_item.nav_cmd = NAV_CMD_LAND; - bool ret = checker.processNextItem(mission_item, 0, 1); + checker.processNextItem(mission_item, 0, 1); ASSERT_EQ(checker.someCheckFailed(), true); - - // mission item validity failed - ASSERT_EQ(ret, false); - - checker.reset(); - mission_item.nav_cmd = NAV_CMD_TAKEOFF; - mission_item.altitude_is_relative = true; - ret = checker.processNextItem(mission_item, 0, 5); - - // home alt is not valid but we have a relative mission item, should fail immediately - ASSERT_EQ(ret, false); } TEST_F(FeasibilityCheckerTest, max_dist_between_waypoints) @@ -226,8 +214,9 @@ TEST_F(FeasibilityCheckerTest, check_takeoff) // takeoff altitude is smaller than acceptance radius, should fail mission_item_s mission_item = {}; mission_item.nav_cmd = NAV_CMD_TAKEOFF; - mission_item.altitude = -5.0f; + mission_item.altitude = 5.0f; mission_item.altitude_is_relative = true; + mission_item.acceptance_radius = 10.0f; checker.processNextItem(mission_item, 0, 1); @@ -238,8 +227,8 @@ TEST_F(FeasibilityCheckerTest, check_takeoff) checker.publishHomePosition(0, 0, 100); - // takeoff altitude is larger than home altitude, should pass - mission_item.altitude = 0.1f; + // takeoff altitude is larger than acceptance radius, should pass + mission_item.acceptance_radius = 3.0f; checker.processNextItem(mission_item, 0, 1); ASSERT_EQ(checker.someCheckFailed(), false); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index ba21850c6c..459ac1fdc0 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -86,10 +86,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission) return false; } - if (!_feasibility_checker.processNextItem(missionitem, i, mission.count)) { - failed = true; - break; - } + _feasibility_checker.processNextItem(missionitem, i, mission.count); }