rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Add altitude control to the path navigation

Browse Source
This commit is contained in:
James Cotton 2012-04-10 01:59:26 -05:00
parent 26b73e3c8b
commit 1aec5b8deb
1 changed files with 22 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
flight/Modules/Guidance/guidance.c
View File

@ -212,6 +212,16 @@ static void guidanceTask(void *parameters)
*/
static void updatePathVelocity()
{
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();;
float dT = 0;
float downCommand;
// Check how long since last update
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
@ -231,7 +241,6 @@ static void updatePathVelocity()
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
velocityDesired.Down = 0;
float error_speed = progress.error * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP];
float correction_velocity[2] = {progress.correction_direction[0] * error_speed,
@ -245,6 +254,18 @@ static void updatePathVelocity()
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound(progress.fractional_progress,0,1);
float downError = altitudeSetpoint - positionActual.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],
-guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand,
-guidanceSettings.VerticalVelMax,
guidanceSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
}