diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde
index 35c095ff0a..0824944e0e 100644
--- a/ArduPlane/navigation.pde
+++ b/ArduPlane/navigation.pde
@@ -115,10 +115,15 @@ static void calc_airspeed_errors()
 
 static void calc_gndspeed_undershoot()
 {
-    // Function is overkill, but here in case we want to add filtering
-    // later
+ 	// Use the component of ground speed in the forward direction
+	// This prevents flyaway if wind takes plane backwards
     if (g_gps && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
-        groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - g_gps->ground_speed) : 0;
+	    Vector2f gndVel = ahrs.groundspeed_vector();
+		Matrix3f rotMat = ahrs.get_dcm_matrix();
+		Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x);
+		yawVect = (yawVect).normalized();
+		float gndSpdFwd = yawVect * gndVel;
+        groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - gndSpdFwd*100) : 0;
     }
 }