From 4d2a31afe34bcf36964ecbd6af787a77c39c4d78 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 7 Mar 2023 15:55:34 +0100 Subject: [PATCH] Navigator: remove max distance between WP mission feasibility check (MIS_DIST_WPS) Signed-off-by: Silvan Fuhrer --- .../init.d-posix/airframes/1033_jsbsim_rascal | 1 - .../airframes/1034_flightgear_rascal-electric | 1 - .../init.d-posix/airframes/1036_jsbsim_malolo | 1 - .../airframes/1039_flightgear_rascal | 1 - .../init.d/airframes/13013_deltaquad | 1 - ROMFS/px4fmu_common/init.d/rc.fw_defaults | 1 - ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 1 - .../MissionFeasibility/FeasibilityChecker.cpp | 32 ++--------------- .../MissionFeasibility/FeasibilityChecker.hpp | 1 - .../FeasibilityCheckerTest.cpp | 36 ------------------- src/modules/navigator/mission.h | 1 - src/modules/navigator/mission_params.c | 16 --------- 12 files changed, 2 insertions(+), 91 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal index 5eb6f7a863..89675aed62 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal @@ -25,7 +25,6 @@ param set-default FW_W_EN 1 param set-default MIS_TAKEOFF_ALT 20 param set-default MIS_DIST_1WP 2500 -param set-default MIS_DIST_WPS 10000 param set-default NAV_ACC_RAD 15 param set-default NAV_DLL_ACT 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric index 9e92e5249d..e90fb1c657 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric @@ -25,7 +25,6 @@ param set-default FW_W_EN 1 param set-default MIS_TAKEOFF_ALT 20 param set-default MIS_DIST_1WP 2500 -param set-default MIS_DIST_WPS 10000 param set-default NAV_ACC_RAD 15 param set-default NAV_DLL_ACT 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo index 5eb6f7a863..89675aed62 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo @@ -25,7 +25,6 @@ param set-default FW_W_EN 1 param set-default MIS_TAKEOFF_ALT 20 param set-default MIS_DIST_1WP 2500 -param set-default MIS_DIST_WPS 10000 param set-default NAV_ACC_RAD 15 param set-default NAV_DLL_ACT 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal index 261f8cc1bc..42ef4f9b02 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal @@ -27,7 +27,6 @@ param set-default FW_W_EN 1 param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 20 param set-default MIS_DIST_1WP 2500 -param set-default MIS_DIST_WPS 10000 param set-default NAV_ACC_RAD 15 param set-default NAV_DLL_ACT 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index 9d0844dd7a..08f668b067 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad @@ -79,7 +79,6 @@ param set-default MC_YAWRATE_MAX 20 param set-default MC_AIRMODE 1 param set-default MIS_DIST_1WP 100 -param set-default MIS_DIST_WPS 100000 param set-default MIS_TAKEOFF_ALT 15 param set-default MPC_XY_P 0.8 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index ab8ab68744..77b10d8cfc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -46,7 +46,6 @@ param set-default RTL_LAND_DELAY -1 # param set-default NAV_ACC_RAD 10 -param set-default MIS_DIST_WPS 5000 param set-default MIS_TAKEOFF_ALT 25 param set-default MIS_TKO_LAND_REQ 2 diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 6a0bc29952..0f3c4d98c0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -23,7 +23,6 @@ param set-default EKF2_FUSE_BETA 1 param set-default HTE_VXY_THR 2.0 -param set-default MIS_DIST_WPS 5000 param set-default MIS_TKO_LAND_REQ 2 param set-default MPC_ACC_HOR_MAX 2 diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index 3965fa6ad5..7d542bc063 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -131,12 +131,6 @@ void FeasibilityChecker::updateData() param_get(handle, &_param_mis_dist_1wp); } - handle = param_find("MIS_DIST_WPS"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_param_mis_dist_wps); - } - handle = param_find("NAV_ACC_RAD"); if (handle != PARAM_INVALID) { @@ -639,12 +633,6 @@ bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_it bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mission_item) { - if (_param_mis_dist_wps <= 0.0f) { - /* param not set, check is ok */ - return true; - } - - /* check only items with valid lat/lon */ if (!MissionBlock::item_contains_position(mission_item)) { return true; @@ -658,24 +646,8 @@ bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mi _last_lat, _last_lon); - if (dist_between_waypoints > _param_mis_dist_wps) { - /* distance between waypoints is too high */ - mavlink_log_critical(_mavlink_log_pub, - "Distance between waypoints too far: %d meters, %d max.\t", - (int)dist_between_waypoints, (int)_param_mis_dist_wps); - events::send(events::ID("navigator_mis_wp_dist_too_far"), {events::Log::Error, events::LogInternal::Info}, - "Distance between waypoints too far: {1m}, (maximum: {2m})", (uint32_t)dist_between_waypoints, - (uint32_t)_param_mis_dist_wps); - - - return false; - - /* do not allow waypoints that are literally on top of each other */ - - /* and do not allow condition gates that are at the same position as a navigation waypoint */ - - } else if (dist_between_waypoints < 0.05f && - (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || _last_cmd == NAV_CMD_CONDITION_GATE)) { + if (dist_between_waypoints < 0.05f && + (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || _last_cmd == NAV_CMD_CONDITION_GATE)) { /* Waypoints and gate are at the exact same position, which indicates an * invalid mission and makes calculating the direction from one waypoint diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index 2f5f380250..02e1a0cb5f 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -100,7 +100,6 @@ private: // parameters float _param_fw_lnd_ang{0.f}; float _param_mis_dist_1wp{0.f}; - float _param_mis_dist_wps{0.f}; float _param_nav_acc_rad{0.f}; int32_t _param_mis_takeoff_land_req{0}; diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index 858905a3b7..f43a609586 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -123,42 +123,6 @@ TEST_F(FeasibilityCheckerTest, mission_item_validity) ASSERT_EQ(ret, false); } -TEST_F(FeasibilityCheckerTest, max_dist_between_waypoints) -{ - TestFeasibilityChecker checker; - checker.publishLanded(true); - - param_t param = param_handle(px4::params::MIS_DIST_WPS); - - float max_dist = 1000.0f; - param_set(param, &max_dist); - checker.paramsChanged(); - - mission_item_s mission_item = {}; - mission_item.nav_cmd = NAV_CMD_WAYPOINT; - - checker.processNextItem(mission_item, 0, 3); - - // waypoints are within limits, check should pass - double lat_new, lon_new; - waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 999, &lat_new, &lon_new); - mission_item.lat = lat_new; - mission_item.lon = lon_new; - checker.processNextItem(mission_item, 1, 3); - - ASSERT_EQ(checker.someCheckFailed(), false); - - // waypoints are above limits, check should fail - waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1001, &lat_new, &lon_new); - - mission_item.lat = lat_new; - mission_item.lon = lon_new; - checker.processNextItem(mission_item, 2, 3); - - ASSERT_EQ(checker.someCheckFailed(), true); -} - - TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint) { TestFeasibilityChecker checker; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 989a875425..821b6b25a7 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -242,7 +242,6 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_mis_dist_1wp, - (ParamFloat) _param_mis_dist_wps, (ParamInt) _param_mis_mnt_yaw_ctl ) diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index c5c07b355c..98ddd0e628 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -88,22 +88,6 @@ PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0); */ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900); -/** - * Maximal horizontal distance between waypoint - * - * Failsafe check to prevent running missions which are way too big. - * Set a value of zero or less to disable. The mission will not be started if any distance between - * two subsequent waypoints is greater than MIS_DIST_WPS. - * - * @unit m - * @min 0 - * @max 10000 - * @decimal 1 - * @increment 100 - * @group Mission - */ -PARAM_DEFINE_FLOAT(MIS_DIST_WPS, 900); - /** * Enable yaw control of the mount. (Only affects multicopters and ROI mission items) *