diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 67802c6af7..ff76b5620d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -264,7 +264,8 @@ 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, _navigator->get_acceptance_radius()); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius(), + _navigator->get_vstatus()->condition_landed); _navigator->get_mission_result()->valid = !failed; _navigator->increment_mission_instance_count(); @@ -848,7 +849,8 @@ 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, _navigator->get_acceptance_radius()); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius(), + _navigator->get_vstatus()->condition_landed); _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 2cf1c45f56..d8ee9c8628 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -65,7 +65,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 default_acceptance_rad) + float default_acceptance_rad, + bool condition_landed) { bool failed = false; bool warned = false; @@ -84,7 +85,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota } // check if all mission item commands are supported - failed = failed || !checkMissionItemValidity(dm_current, nMissionItems); + failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed); failed = failed || !checkGeofence(dm_current, nMissionItems, geofence); failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); @@ -211,7 +212,7 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, return true; } -bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems) { +bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed) { // do not allow mission if we find unsupported item for (size_t i = 0; i < nMissionItems; i++) { struct mission_item_s missionitem; @@ -223,7 +224,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s return false; } - // check if we find unsupported item and reject mission if so + // check if we find unsupported items and reject mission if so if (missionitem.nav_cmd != NAV_CMD_IDLE && missionitem.nav_cmd != NAV_CMD_WAYPOINT && missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && @@ -240,6 +241,17 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1)); return false; } + + // check if the mission starts with a land command while the vehicle is landed + if (missionitem.nav_cmd == NAV_CMD_LAND && + i == 0 && + condition_landed) { + + mavlink_log_critical(_mavlink_fd, "Rejecting mission that starts with LAND command while vehicle is landed."); + return false; + } + + } return true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index a1831999ac..7b44c3246d 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -63,7 +63,7 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false); - bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); + 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); /* Checks specific to fixedwing airframes */ @@ -83,7 +83,8 @@ 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, float default_acceptance_rad); + double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad, + bool condition_landed); };