Merge remote-tracking branch 'upstream/master' into mtecs

ROMFS/px4fmu_common/init.d/rc.mc_defaults

@@ -41,6 +41,7 @@ then
 	param set PE_POSNE_NOISE 0.5
 	param set PE_POSD_NOISE 1.0
 
+	param set NAV_ACCEPT_RAD 2.0
 fi
 
 set PWM_RATE 400

src/modules/navigator/navigator_main.cpp

@@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached()
 
 	/* XXX TODO count turns */
 	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
 	    _mission_item.loiter_radius > 0.01f) {
 
@@ -1477,27 +1476,27 @@ Navigator::check_mission_item_reached()
 			acceptance_radius = _parameters.acceptance_radius;
 		}
 
-		float dist = -1.0f;
-		float dist_xy = -1.0f;
-		float dist_z = -1.0f;
-
-		/* calculate AMSL altitude for this waypoint */
-		float wp_alt_amsl = _mission_item.altitude;
-
-		if (_mission_item.altitude_is_relative)
-			wp_alt_amsl += _home_pos.alt;
-
-		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
-				(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
-				&dist_xy, &dist_z);
-
 		if (_do_takeoff) {
-			if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
-				/* require only altitude for takeoff */
+			/* require only altitude for takeoff */
+			if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
 				_waypoint_position_reached = true;
 			}
 
 		} else {
+			float dist = -1.0f;
+			float dist_xy = -1.0f;
+			float dist_z = -1.0f;
+
+			/* calculate AMSL altitude for this waypoint */
+			float wp_alt_amsl = _mission_item.altitude;
+
+			if (_mission_item.altitude_is_relative)
+				wp_alt_amsl += _home_pos.alt;
+
+			dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
+					(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
+					&dist_xy, &dist_z);
+
 			if (dist >= 0.0f && dist <= acceptance_radius) {
 				_waypoint_position_reached = true;
 			}
@@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached()
 		}
 
 		if (_mission.current_mission_available()) {
-			set_mission_item();
+			if (_mission_item.autocontinue) {
+				/* continue mission */
+				set_mission_item();
+
+			} else {
+				/* autocontinue disabled for this item */
+				request_loiter_or_ready();
+			}
 
 		} else {
 			/* if no more mission items available then finish mission */