diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 86fc07e3e1..0ff7c0cf4a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -63,6 +63,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_onboard_enabled(this, "MIS_ONBOARD_EN", false), _param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false), _param_dist_1wp(this, "MIS_DIST_1WP", false), + _param_dist_between_wps(this, "MIS_DIST_WPS", false), _param_altmode(this, "MIS_ALTMODE", false), _param_yawmode(this, "MIS_YAWMODE", false), _param_force_vtol(this, "NAV_FORCE_VT", false), @@ -1439,7 +1440,10 @@ Mission::check_mission_valid(bool force) if ((!_home_inited && _navigator->home_position_valid()) || force) { _navigator->get_mission_result()->valid = - _missionFeasibilityChecker.checkMissionFeasible(_offboard_mission, _param_dist_1wp.get(), false); + _missionFeasibilityChecker.checkMissionFeasible(_offboard_mission, + _param_dist_1wp.get(), + _param_dist_between_wps.get(), + false); _navigator->get_mission_result()->seq_total = _offboard_mission.count; _navigator->increment_mission_instance_count(); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 572e64ea51..2c28d7ae7a 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -239,6 +239,7 @@ private: control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; + control::BlockParamFloat _param_dist_between_wps; control::BlockParamInt _param_altmode; control::BlockParamInt _param_yawmode; control::BlockParamInt _param_force_vtol; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 54003a24db..f0ca35eb9c 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -51,7 +51,8 @@ #include bool -MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, float max_waypoint_distance, +MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, + float max_distance_to_1st_waypoint, float max_distance_between_waypoints, bool land_start_req) { dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id); @@ -78,15 +79,14 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, float mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock."); } else { - const double lat = _navigator->get_home_position()->lat; - const double lon = _navigator->get_home_position()->lon; - - failed = failed - || !check_dist_1wp(dm_current, nMissionItems, lat, lon, max_waypoint_distance, warning_issued); + failed = failed || + !checkDistanceToFirstWaypoint(dm_current, nMissionItems, max_distance_to_1st_waypoint, warning_issued); } // check if all mission item commands are supported failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, landed); + failed = failed + || !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints, warning_issued); failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid); failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); @@ -476,51 +476,120 @@ MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nM } bool -MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, - float dist_first_wp, bool &warning_issued) +MissionFeasibilityChecker::checkDistanceToFirstWaypoint(dm_item_t dm_current, size_t nMissionItems, + float max_distance, bool &warning_issued) { - /* check if first waypoint is not too far from home */ - if (dist_first_wp > 0.0f) { - struct mission_item_s mission_item = {}; + if (max_distance <= 0.0f) { + /* param not set, check is ok */ + return true; + } - /* find first waypoint (with lat/lon) item in datamanager */ - for (size_t i = 0; i < nMissionItems; i++) { - if (dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { - if (MissionBlock::item_contains_position(mission_item)) { - /* check only items with valid lat/lon */ + /* find first waypoint (with lat/lon) item in datamanager */ + for (size_t i = 0; i < nMissionItems; i++) { - /* check distance from current position to item */ - float dist_to_1wp = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, curr_lat, curr_lon); + struct mission_item_s mission_item {}; - if (dist_to_1wp < dist_first_wp) { - if (dist_to_1wp > ((dist_first_wp * 3) / 2)) { - /* allow at 2/3 distance, but warn */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission entry point very far: %d m.", (int)dist_to_1wp); - warning_issued = true; - } + if (!(dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) { + /* error reading, mission is invalid */ + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission."); + return false; + } - return true; + /* check only items with valid lat/lon */ + if (!MissionBlock::item_contains_position(mission_item)) { + continue; + } - } else { - /* item is too far from home */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission entry point too far: %dm, %dm max.", - (int)dist_to_1wp, (int)dist_first_wp); - warning_issued = true; - return false; - } + /* check distance from current position to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); + + if (dist_to_1wp < max_distance) { + + if (dist_to_1wp > ((max_distance * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "First waypoint far away: %d meters.", (int)dist_to_1wp); + warning_issued = true; + } + + return true; + + } else { + /* item is too far from home */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "First waypoint too far away: %d meters, %d max.", + (int)dist_to_1wp, (int)max_distance); + warning_issued = true; + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + return true; +} + +bool +MissionFeasibilityChecker::checkDistancesBetweenWaypoints(dm_item_t dm_current, size_t nMissionItems, + float max_distance, bool &warning_issued) +{ + if (max_distance <= 0.0f) { + /* param not set, check is ok */ + return true; + } + + double last_lat = NAN; + double last_lon = NAN; + + /* Go through all waypoints */ + for (size_t i = 0; i < nMissionItems; i++) { + + struct mission_item_s mission_item {}; + + if (!(dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) { + /* error reading, mission is invalid */ + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission."); + return false; + } + + /* check only items with valid lat/lon */ + if (!MissionBlock::item_contains_position(mission_item)) { + continue; + } + + /* Compare it to last waypoint if already available. */ + if (PX4_ISFINITE(last_lat) && PX4_ISFINITE(last_lon)) { + + /* check distance from current position to item */ + float dist_between_waypoints = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + last_lat, last_lon); + + if (dist_between_waypoints < max_distance) { + + if (dist_between_waypoints > ((max_distance * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "Distance between waypoints very far: %d meters.", + (int)dist_between_waypoints); + warning_issued = true; } } else { - /* error reading, mission is invalid */ - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission."); + /* item is too far from home */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "Distance between waypoints too far: %d meters, %d max.", + (int)dist_between_waypoints, (int)max_distance); + warning_issued = true; return false; } } - /* no waypoints found in mission, then we will not fly far away */ - return true; - - } else { - return true; + last_lat = mission_item.lat; + last_lon = mission_item.lon; } + + /* We ran through all waypoints and have not found any distances between waypoints that are too far. */ + return true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index bec7cc3695..880fd0178c 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -62,8 +62,9 @@ private: bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed); - bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, - float dist_first_wp, bool &warning_issued); + bool checkDistanceToFirstWaypoint(dm_item_t dm_current, size_t nMissionItems, float max_distance, bool &warning_issued); + bool checkDistancesBetweenWaypoints(dm_item_t dm_current, size_t nMissionItems, float max_distance, + bool &warning_issued); /* Checks specific to fixedwing airframes */ bool checkFixedwing(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status, @@ -88,7 +89,9 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(const mission_s &mission, float max_waypoint_distance, bool land_start_req); + bool checkMissionFeasible(const mission_s &mission, + float max_distance_to_1st_waypoint, float max_distance_between_waypoints, + bool land_start_req); }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index c6f4593583..cfff98c88b 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -99,6 +99,22 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); */ 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); + /** * Altitude setpoint mode *