diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 33b08da772..5b0b4431a0 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -329,6 +329,13 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
     return true;
 }
 
+void GCS_MAVLINK_Rover::packetReceived(const mavlink_status_t &status, mavlink_message_t &msg)
+{
+    // pass message to follow library
+    rover.g2.follow.handle_msg(msg);
+    GCS_MAVLINK::packetReceived(status, msg);
+}
+
 /*
   default stream rates to 1Hz
  */
diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h
index 4afad47680..0531b7c5c6 100644
--- a/APMrover2/GCS_Mavlink.h
+++ b/APMrover2/GCS_Mavlink.h
@@ -40,6 +40,8 @@ private:
     void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
     bool try_send_message(enum ap_message id) override;
 
+    void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) override;
+
     MAV_TYPE frame_type() const override;
     MAV_MODE base_mode() const override;
     uint32_t custom_mode() const override;
diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp
index 05075090f5..abd2ae8815 100644
--- a/APMrover2/control_modes.cpp
+++ b/APMrover2/control_modes.cpp
@@ -21,6 +21,9 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num)
     case Mode::Number::LOITER:
         ret = &mode_loiter;
         break;
+    case Mode::Number::FOLLOW:
+        ret = &mode_follow;
+        break;
     case Mode::Number::AUTO:
         ret = &mode_auto;
         break;
@@ -33,9 +36,6 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num)
     case Mode::Number::GUIDED:
        ret = &mode_guided;
         break;
-    case Mode::Number::FOLLOW:
-        ret = &mode_follow;
-        break;
     case Mode::Number::INITIALISING:
         ret = &mode_initializing;
         break;
diff --git a/APMrover2/mode.h b/APMrover2/mode.h
index 3b87bfa9fa..8df11a9089 100644
--- a/APMrover2/mode.h
+++ b/APMrover2/mode.h
@@ -467,7 +467,7 @@ public:
     bool attitude_stabilized() const override { return false; }
 };
 
-class ModeFollow : public ModeGuided
+class ModeFollow : public Mode
 {
 public:
 
@@ -480,5 +480,4 @@ public:
 protected:
 
     bool _enter() override;
-    uint32_t last_log_ms;   // system time of last time desired velocity was logging
 };
diff --git a/APMrover2/mode_follow.cpp b/APMrover2/mode_follow.cpp
index a87d82aefe..7a031433ce 100644
--- a/APMrover2/mode_follow.cpp
+++ b/APMrover2/mode_follow.cpp
@@ -4,54 +4,70 @@
 // initialize follow mode
 bool ModeFollow::_enter()
 {
-    return ModeGuided::enter();
+    if (!g2.follow.enabled()) {
+        return false;
+    }
+
+    // initialise waypoint speed
+    set_desired_speed_to_default();
+
+    // initialise heading to current heading
+    _desired_yaw_cd = ahrs.yaw_sensor;
+    _yaw_error_cd = 0.0f;
+
+    return true;
 }
 
 void ModeFollow::update()
 {
-    // variables to be sent to velocity controller
-    Vector3f desired_velocity_neu_cms;
-    float yaw_cd = 0.0f;
+    // stop vehicle if no speed estimate
+    float speed;
+    if (!attitude_control.get_forward_speed(speed)) {
+        // no valid speed so stop
+        g2.motors.set_throttle(0.0f);
+        g2.motors.set_steering(0.0f);
+        return;
+    }
 
-    Vector3f dist_vec;  //vector to lead vehicle
+    Vector3f dist_vec;  // vector to lead vehicle
     Vector3f dist_vec_offs; // vector to lead vehicle + offset
     Vector3f vel_of_target; // velocity of lead vehicle
 
-    if (g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) {
-
-        // convert dist_vec_offs to cm in NEU
-        const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f);
-
-        // calculate desired velocity vector in cm/s in NEU
-        const float kp = g2.follow.get_pos_p().kP();
-        desired_velocity_neu_cms.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
-        desired_velocity_neu_cms.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp);
-
-        // scaled desired velocity to stay within horizontal speed limit
-        float desired_speed = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y));
-        if (!is_zero(desired_speed) && (desired_speed > _desired_speed)) {
-            const float scalar_xy = _desired_speed / desired_speed;
-            desired_velocity_neu_cms.x *= scalar_xy;
-            desired_velocity_neu_cms.y *= scalar_xy;
-            desired_speed = _desired_speed;
-        }
-
-        // unit vector towards target position (i.e. vector to lead vehicle + offset)
-        Vector3f dir_to_target_neu = dist_vec_offs_neu;
-        const float dir_to_target_neu_len = dir_to_target_neu.length();
-        if (!is_zero(dir_to_target_neu_len)) {
-            dir_to_target_neu /= dir_to_target_neu_len;
-        }
-
-        // calculate vehicle heading
-        const Vector3f dist_vec_xy(dist_vec_offs.x, dist_vec_offs.y, 0.0f);
-        if (dist_vec_xy.length() > 1.0f) {
-            yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
-        }
+    // if no target simply stop the vehicle
+    if (!g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) {
+        _reached_destination = true;
+        stop_vehicle();
+        return;
+    }
+
+    // calculate desired velocity vector
+    Vector2f desired_velocity_ne;
+    const float kp = g2.follow.get_pos_p().kP();
+    desired_velocity_ne.x = vel_of_target.x + (dist_vec_offs.x * kp);
+    desired_velocity_ne.y = vel_of_target.y + (dist_vec_offs.y * kp);
+
+    // if desired velocity is zero stop vehicle
+    if (is_zero(desired_velocity_ne.x) && is_zero(desired_velocity_ne.y)) {
+        _reached_destination = true;
+        stop_vehicle();
+        return;
+    }
+
+    // we have not reached the target
+    _reached_destination = false;
+
+    // scale desired velocity to stay within horizontal speed limit
+    float desired_speed = safe_sqrt(sq(desired_velocity_ne.x) + sq(desired_velocity_ne.y));
+    if (!is_zero(desired_speed) && (desired_speed > _desired_speed)) {
+        const float scalar_xy = _desired_speed / desired_speed;
+        desired_velocity_ne *= scalar_xy;
+        desired_speed = _desired_speed;
     }
 
-    // re-use guided mode's heading and speed controller
-    ModeGuided::set_desired_heading_and_speed(yaw_cd, safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y)));
+    // calculate vehicle heading
+    _desired_yaw_cd = wrap_180_cd(atan2f(desired_velocity_ne.y, desired_velocity_ne.x) * DEGX100);
 
-    ModeGuided::update();
+    // run steering and throttle controllers
+    calc_steering_to_heading(_desired_yaw_cd, desired_speed < 0);
+    calc_throttle(calc_reduced_speed_for_turn_or_distance(desired_speed), false, true);
 }