diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index be88b87948..3175e64ce2 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -167,9 +167,9 @@ bool set_special_fields(float param1, float param2, float param3, float param4,
 	sp->loiter_direction = (param3 >= 0) ? 1 : -1;
 
 	sp->param1 = param1;
-	sp->param1 = param2;
-	sp->param1 = param3;
-	sp->param1 = param4;
+	sp->param2 = param2;
+	sp->param3 = param3;
+	sp->param4 = param4;
 }
 
 /**
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 16a7c2d355..c6c2eac68b 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -417,6 +417,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			}
 
 			if (time_elapsed) {
+
 				if (cur_wp->autocontinue) {
 					cur_wp->current = 0;
 
@@ -425,9 +426,10 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 
 						if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
 							/* the last waypoint was reached, if auto continue is
-							 * activated restart the waypoint list from the beginning
+							 * activated keep the system loitering there.
 							 */
-							wpm->current_active_wp_id = 0;
+							cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
+							cur_wp->param3 = 15.0f; // XXX magic number 15 m loiter radius
 
 						} else {
 							if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)