diff --git a/flight/OpenPilot/Modules/Guidance/guidance.c b/flight/OpenPilot/Modules/Guidance/guidance.c
index 61287525b..ef9ae7ba7 100644
--- a/flight/OpenPilot/Modules/Guidance/guidance.c
+++ b/flight/OpenPilot/Modules/Guidance/guidance.c
@@ -91,6 +91,7 @@ static float eastIntegral = 0;
 static float eastErrorLast = 0;
 static float downIntegral = 0;
 static float downErrorLast = 0;
+static uint8_t positionHoldLast = 0;
 
 /**
  * Module thread, should not return.
@@ -115,6 +116,18 @@ static void guidanceTask(void *parameters)
 		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) ||
 		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)))
 		{
+			if(positionHoldLast == 0) {
+				/* When enter position hold mode save current position */
+				PositionDesiredData positionDesired;
+				PositionActualData positionActual;
+				PositionDesiredGet(&positionDesired);
+				PositionActualGet(&positionActual);
+				positionDesired.North = positionActual.North;
+				positionDesired.East = positionActual.East;
+				PositionDesiredSet(&positionDesired);
+				positionHoldLast = 1;
+			}
+			
 			if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_POSITION_PID) {
 				positionPIDcontrol();
 			} else {
@@ -133,6 +146,7 @@ static void guidanceTask(void *parameters)
 			eastErrorLast = 0;
 			downIntegral = 0;
 			downErrorLast = 0;
+			positionHoldLast = 0;
 		}			
 		vTaskDelayUntil(&lastSysTime, guidanceSettings.VelUpdatePeriod / portTICK_RATE_MS);
 	}