From 54aa09ce27534000c90539f4e5aeb3390125f4e1 Mon Sep 17 00:00:00 2001
From: peabody124 <peabody124>
Date: Fri, 12 Nov 2010 16:57:16 +0000
Subject: [PATCH] OP-163 Flight/Guidance: Take current position as desired
 position

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2111 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 flight/OpenPilot/Modules/Guidance/guidance.c | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

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);
 	}