diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index af7b6736ab..0b07c32fc5 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -672,6 +672,8 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &missi
 
 	double last_lat = (double)NAN;
 	double last_lon = (double)NAN;
+	float last_alt = NAN;
+	int last_cmd = 0;
 
 	/* Go through all waypoints */
 	for (size_t i = 0; i < mission.count; i++) {
@@ -697,19 +699,38 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &missi
 					mission_item.lat, mission_item.lon,
 					last_lat, last_lon);
 
+
 			if (dist_between_waypoints > max_distance) {
-				/* item is too far from home */
+				/* distance between waypoints is too high */
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
 						     "Distance between waypoints too far: %d meters, %d max.",
 						     (int)dist_between_waypoints, (int)max_distance);
 
 				_navigator->get_mission_result()->warning = true;
 				return false;
+
+				/* do not allow waypoints that are literally on top of each other */
+
+			} else if ((dist_between_waypoints < 0.05f && fabsf(last_alt - mission_item.altitude) < 0.05f) ||
+				   /* and do not allow condition gates that are at the same position as a navigation waypoint */
+				   (dist_between_waypoints < 0.05f && (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE
+						   || last_cmd == NAV_CMD_CONDITION_GATE))) {
+				/* waypoints are at the exact same position,
+				 * which indicates an invalid mission and makes calculating
+				 * the direction from one waypoint to another impossible. */
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+						     "Distance between waypoints too close: %d meters",
+						     (int)dist_between_waypoints);
+
+				_navigator->get_mission_result()->warning = true;
+				return false;
 			}
 		}
 
 		last_lat = mission_item.lat;
 		last_lon = mission_item.lon;
+		last_alt = mission_item.altitude;
+		last_cmd = mission_item.nav_cmd;
 	}
 
 	/* We ran through all waypoints and have not found any distances between waypoints that are too far. */