diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 334fcd7217..6102eb9dde 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -358,6 +358,12 @@ private:
     static const AP_Param::Info var_info[];
     static const LogStructure log_structure[];
 
+    // Loiter control
+    uint16_t loiter_time_max; // How long we should loiter at the nav_waypoint (time in seconds)
+    uint32_t loiter_time;     // How long have we been loitering - The start time in millis
+
+    float distance_past_wp; // record the distance we have gone past the wp
+
 private:
     // private member functions
     void ahrs_update();
diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp
index d185ed7cba..4c7de23018 100644
--- a/APMrover2/Steering.cpp
+++ b/APMrover2/Steering.cpp
@@ -85,8 +85,10 @@ bool Rover::use_pivot_steering(void)
   calculate the throtte for auto-throttle modes
  */
 void Rover::calc_throttle(float target_speed)
-{  
-    if (!auto_check_trigger()) {
+{
+    // If not autostarting OR we are loitering at a waypoint
+    // then set the throttle to minimum
+    if (!auto_check_trigger() || ((loiter_time > 0) && (control_mode == AUTO))) {
         channel_throttle->servo_out = g.throttle_min.get();
         return;
     }
@@ -196,6 +198,12 @@ void Rover::calc_lateral_acceleration()
  */
 void Rover::calc_nav_steer()
 {
+    // check to see if the rover is loitering
+    if ((loiter_time > 0) && (control_mode == AUTO)) {
+        channel_steer->servo_out = 0;
+        return;
+    }
+
     // add in obstacle avoidance
     lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
 
diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp
index 825d0b16ff..54d4017ac5 100644
--- a/APMrover2/commands_logic.cpp
+++ b/APMrover2/commands_logic.cpp
@@ -176,6 +176,14 @@ void Rover::do_RTL(void)
 
 void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
+    // this will be used to remember the time in millis after we reach or pass the WP.
+    loiter_time = 0;
+    // this is the delay, stored in seconds
+    loiter_time_max = abs(cmd.p1);
+
+    // this is the distance we travel past the waypoint - not there yet so 0 initially
+    distance_past_wp = 0;
+
 	set_next_WP(cmd.content.location);
 }
 
@@ -185,17 +193,45 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
     if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
-        gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
-                          (unsigned)cmd.index,
-                          (unsigned)get_distance(current_loc, next_WP));
+        // Check if we need to loiter at this waypoint
+        if (loiter_time_max > 0) {
+            if (loiter_time == 0) {  // check if we are just starting loiter
+                gcs_send_text_fmt(PSTR("Reached Waypoint #%i - Loiter for %i seconds"),
+                                  (unsigned)cmd.index,
+                                  (unsigned)loiter_time_max);
+                // record the current time i.e. start timer
+                loiter_time = millis();
+            }
+            // Check if we have loiter long enough
+            if (((millis() - loiter_time) / 1000) < loiter_time_max) {
+                return false;
+            }
+        } else {
+            gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
+                              (unsigned)cmd.index,
+                              (unsigned)get_distance(current_loc, next_WP));
+        }
         return true;
     }
 
     // have we gone past the waypoint?
+    // We should always go through the waypoint i.e. the above code
+    // first before we go past it.
     if (location_passed_point(current_loc, prev_WP, next_WP)) {
-        gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
-                          (unsigned)cmd.index,
-                          (unsigned)get_distance(current_loc, next_WP));
+        // check if we have gone futher past the wp then last time and output new message if we have
+        if ((uint32_t)distance_past_wp != (uint32_t)get_distance(current_loc, next_WP)) {
+            distance_past_wp = get_distance(current_loc, next_WP);
+            gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
+                              (unsigned)cmd.index,
+                              (unsigned)distance_past_wp);
+        }
+        // Check if we need to loiter at this waypoint
+        if (loiter_time_max > 0) {
+            if (((millis() - loiter_time) / 1000) < loiter_time_max) {
+                return false;
+            }
+        }
+        distance_past_wp = 0;
         return true;
     }
 
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index 9aba30288d..4d70ff3029 100644
--- a/APMrover2/system.cpp
+++ b/APMrover2/system.cpp
@@ -277,6 +277,11 @@ void Rover::set_mode(enum mode mode)
 		// don't switch modes if we are already in the correct mode.
 		return;
 	}
+
+    // If we are changing out of AUTO mode reset the loiter timer
+    if (control_mode == AUTO)
+        loiter_time = 0;
+
 	control_mode = mode;
     throttle_last = 0;
     throttle = 500;