dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-03-07 15:55:34 +01:00
parent 9c001f2e40
commit 4d2a31afe3
12 changed files with 2 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal
View File

@ -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

1
ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad
View File

@ -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

1
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

@ -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

1
ROMFS/px4fmu_common/init.d/rc.vtol_defaults
View File

@ -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

32
src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
View File

@ -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<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)) {
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

1
src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp
View File

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

36
src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp
View File

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

1
src/modules/navigator/mission.h
View File

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

16
src/modules/navigator/mission_params.c
View File

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