Navigator: remove max distance between WP mission feasibility check (MIS_DIST_WPS)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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

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

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

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

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

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
 

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

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,23 +646,7 @@ bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mi
 				_last_lat, _last_lon);
 
 
-		if (dist_between_waypoints > _param_mis_dist_wps) {
+		if (dist_between_waypoints < 0.05f &&
-			/* 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<uint32_t, uint32_t>(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)) {
 
 			/* Waypoints and gate are at the exact same position, which indicates an

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};
 

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;

src/modules/navigator/mission.h

@@ -242,7 +242,6 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
-		(ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps,
 		(ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mis_mnt_yaw_ctl
 	)
 

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)
 *