diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 992a426bfe..242b0faedc 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -259,7 +259,7 @@ Mission::update_offboard_mission() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius()); _navigator->get_mission_result()->valid = !failed; _navigator->increment_mission_instance_count(); @@ -843,7 +843,7 @@ Mission::check_mission_valid() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius()); _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index fd9a206c86..2cf1c45f56 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -64,7 +64,8 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued) + float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, + float default_acceptance_rad) { bool failed = false; bool warned = false; @@ -88,7 +89,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { - failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); + failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad); } else { failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); } @@ -96,9 +97,41 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota return !failed; } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, + Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad) { - /* no custom rotary wing checks yet */ + /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ + for (size_t i = 0; i < nMissionItems; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + // look for a takeoff waypoint + if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) { + // 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 + float takeoff_alt = missionitem.altitude_is_relative + ? missionitem.altitude + : missionitem.altitude - home_alt; + // check if we should use default acceptance radius + float acceptance_radius = default_acceptance_rad; + + if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) { + acceptance_radius = missionitem.acceptance_radius; + } + + if (takeoff_alt - 1.0f < acceptance_radius) { + mavlink_log_critical(_mavlink_fd, "Mission rejected: Takeoff altitude too low!"); + return false; + } + } + } + + // all checks have passed return true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 4586f75a47..a1831999ac 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -72,7 +72,7 @@ private: void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad); public: MissionFeasibilityChecker(); @@ -83,7 +83,7 @@ public: */ bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, - double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued); + double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad); };