rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

OP-163 Flight/Guidance: Take current position as desired position

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2111 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-11-12 16:57:16 +00:00 committed by peabody124
parent 6ebc8ccd57
commit 54aa09ce27
1 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
flight/OpenPilot/Modules/Guidance/guidance.c
View File

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