rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-04-08 00:53:48 +02:00
Code Issues Releases Activity

Flight/Guidance: Merge back into one task because memory requirements of a task

Browse Source 
exceed CPU requirements of PID.  Also add add lesstabilization UAVObject to Makefile
to fix compile errors.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1870 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-10-04 02:01:34 +00:00 committed by peabody124
parent d71e9df8c5
commit f03669733b
2 changed files with 118 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/OpenPilot/Makefile
View File

@ -177,7 +177,7 @@ SRC += $(OPUAVOBJ)/attituderaw.c
SRC += $(OPUAVOBJ)/homelocation.c SRC += $(OPUAVOBJ)/homelocation.c
SRC += $(OPUAVOBJ)/mixersettings.c SRC += $(OPUAVOBJ)/mixersettings.c
SRC += $(OPUAVOBJ)/mixerstatus.c SRC += $(OPUAVOBJ)/mixerstatus.c
#SRC += $(OPUAVOBJ)/lesstabilizationsettings.c SRC += $(OPUAVOBJ)/lesstabilizationsettings.c
SRC += $(OPUAVOBJ)/positiondesired.c SRC += $(OPUAVOBJ)/positiondesired.c
SRC += $(OPUAVOBJ)/velocitydesired.c SRC += $(OPUAVOBJ)/velocitydesired.c
SRC += $(OPUAVOBJ)/velocityactual.c SRC += $(OPUAVOBJ)/velocityactual.c

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

@ -63,13 +63,14 @@
// Private variables // Private variables
static xTaskHandle guidanceTaskHandle; static xTaskHandle guidanceTaskHandle;
static xTaskHandle velocityPIDTaskHandle;
// Private functions // Private functions
static void guidanceTask(void *parameters); static void guidanceTask(void *parameters);
static void velocityPIDTask(void *parameters);
static float bound(float val, float min, float max); static float bound(float val, float min, float max);
static void updateVtolDesiredVelocity();
static void updateVtolDesiredAttitude();
/** /**
* Initialise the module, called on startup * Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed * \returns 0 on success or -1 if initialisation failed
@ -78,11 +79,17 @@ int32_t GuidanceInitialize()
{ {
// Start main task // Start main task
xTaskCreate(guidanceTask, (signed char *)"Guidance", STACK_SIZE, NULL, TASK_PRIORITY, &guidanceTaskHandle); xTaskCreate(guidanceTask, (signed char *)"Guidance", STACK_SIZE, NULL, TASK_PRIORITY, &guidanceTaskHandle);
xTaskCreate(velocityPIDTask, (signed char *)"VelocityPID", STACK_SIZE, NULL, TASK_PRIORITY, &velocityPIDTaskHandle);
return 0; return 0;
} }
static float northIntegral = 0;
static float northErrorLast = 0;
static float eastIntegral = 0;
static float eastErrorLast = 0;
static float downIntegral = 0;
static float downErrorLast = 0;
/** /**
* Module thread, should not return. * Module thread, should not return.
*/ */
@ -91,9 +98,6 @@ static void guidanceTask(void *parameters)
SystemSettingsData systemSettings; SystemSettingsData systemSettings;
GuidanceSettingsData guidanceSettings; GuidanceSettingsData guidanceSettings;
ManualControlCommandData manualControl; ManualControlCommandData manualControl;
PositionActualData positionActual;
PositionDesiredData positionDesired;
VelocityDesiredData velocityDesired;
portTickType lastSysTime; portTickType lastSysTime;
@ -106,37 +110,61 @@ static void guidanceTask(void *parameters)
if ((manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) && if ((manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) &&
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL)) { (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL)) {
updateVtolDesiredVelocity();
PositionActualGet(&positionActual); updateVtolDesiredAttitude();
PositionDesiredGet(&positionDesired); } else {
// Be cleaner and get rid of global variables
// Note all distances in cm northIntegral = 0;
float dNorth = positionDesired.North - positionActual.North; northErrorLast = 0;
float dEast = positionDesired.East - positionActual.East; eastIntegral = 0;
float distance = sqrt(pow(dNorth, 2) + pow(dEast, 2)); eastErrorLast = 0;
float groundspeed = guidanceSettings.GroundVelocityP * distance; //bound(guidanceSettings.GroundVelocityP * distance, 0, guidanceSettings.MaxGroundspeed); downIntegral = 0;
float heading = atan2f(dEast, dNorth); downErrorLast = 0;
}
velocityDesired.North = groundspeed * cosf(heading);
velocityDesired.East = groundspeed * sinf(heading);
float dDown = positionDesired.Down - positionActual.Down;
velocityDesired.Down =
bound(guidanceSettings.VertVelocityP * dDown, -guidanceSettings.MaxVerticalSpeed, guidanceSettings.MaxVerticalSpeed);
VelocityDesiredSet(&velocityDesired);
}
vTaskDelayUntil(&lastSysTime, guidanceSettings.VelUpdatePeriod / portTICK_RATE_MS); vTaskDelayUntil(&lastSysTime, guidanceSettings.VelUpdatePeriod / portTICK_RATE_MS);
} }
} }
void updateVtolDesiredVelocity()
{
GuidanceSettingsData guidanceSettings;
PositionActualData positionActual;
PositionDesiredData positionDesired;
VelocityDesiredData velocityDesired;
GuidanceSettingsGet(&guidanceSettings);
PositionActualGet(&positionActual);
PositionDesiredGet(&positionDesired);
VelocityDesiredGet(&velocityDesired);
// Note all distances in cm
float dNorth = positionDesired.North - positionActual.North;
float dEast = positionDesired.East - positionActual.East;
float distance = sqrt(pow(dNorth, 2) + pow(dEast, 2));
float heading = atan2f(dEast, dNorth);
float groundspeed = bound(guidanceSettings.GroundVelocityP * distance,
0, guidanceSettings.MaxGroundspeed);
velocityDesired.North = groundspeed * cosf(heading);
velocityDesired.East = groundspeed * sinf(heading);
float dDown = positionDesired.Down - positionActual.Down;
velocityDesired.Down = bound(guidanceSettings.VertVelocityP * dDown,
-guidanceSettings.MaxVerticalSpeed,
guidanceSettings.MaxVerticalSpeed);
VelocityDesiredSet(&velocityDesired);
}
/** /**
* Module thread, should not return. * Module thread, should not return.
*/ */
static void velocityPIDTask(void *parameters) static void updateVtolDesiredAttitude()
{ {
portTickType lastSysTime; static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();;
float dT;
VelocityDesiredData velocityDesired; VelocityDesiredData velocityDesired;
VelocityActualData velocityActual; VelocityActualData velocityActual;
AttitudeDesiredData attitudeDesired; AttitudeDesiredData attitudeDesired;
@ -144,83 +172,75 @@ static void velocityPIDTask(void *parameters)
GuidanceSettingsData guidanceSettings; GuidanceSettingsData guidanceSettings;
StabilizationSettingsData stabSettings; StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings; SystemSettingsData systemSettings;
ManualControlCommandData manualControl;
float northError; float northError;
float northDerivative; float northDerivative;
float northIntegral;
float northErrorLast = 0;
float northCommand; float northCommand;
float eastError; float eastError;
float eastDerivative; float eastDerivative;
float eastIntegral = 0;
float eastErrorLast = 0;
float eastCommand; float eastCommand;
float downError; float downError;
float downDerivative; float downDerivative;
float downIntegral = 0;
float downErrorLast = 0; // Check how long since last update
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
// Main task loop dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = xTaskGetTickCount(); lastSysTime = thisSysTime;
while (1) {
ManualControlCommandGet(&manualControl); SystemSettingsGet(&systemSettings);
SystemSettingsGet(&systemSettings); GuidanceSettingsGet(&guidanceSettings);
GuidanceSettingsGet(&guidanceSettings);
if ((manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) && VelocityActualGet(&velocityActual);
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL)) { VelocityDesiredGet(&velocityDesired);
VelocityActualGet(&velocityActual); AttitudeDesiredGet(&attitudeDesired);
VelocityDesiredGet(&velocityDesired); VelocityDesiredGet(&velocityDesired);
AttitudeDesiredGet(&attitudeDesired); AttitudeActualGet(&attitudeActual);
VelocityDesiredGet(&velocityDesired); StabilizationSettingsGet(&stabSettings);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings); attitudeDesired.Yaw = 0; // try and face north
attitudeDesired.Yaw = 0; // try and face north // Yaw and pitch output from ground speed PID loop
northError = velocityDesired.North - velocityActual.North;
// Yaw and pitch output from ground speed PID loop northDerivative = (northError - northErrorLast) / dT;
northError = velocityDesired.North - velocityActual.North; northIntegral =
northDerivative = (northError - northErrorLast) / guidanceSettings.VelPIDUpdatePeriod; bound(northIntegral + northError * dT, -guidanceSettings.MaxVelIntegral,
northIntegral = guidanceSettings.MaxVelIntegral);
bound(northIntegral + northError * guidanceSettings.VelPIDUpdatePeriod, -guidanceSettings.MaxVelIntegral, northErrorLast = northError;
guidanceSettings.MaxVelIntegral); northCommand =
northErrorLast = northError; northError * guidanceSettings.VelP + northDerivative * guidanceSettings.VelD + northIntegral * guidanceSettings.VelI;
northCommand =
northError * guidanceSettings.VelP + northDerivative * guidanceSettings.VelD + northIntegral * guidanceSettings.VelI; eastError = velocityDesired.East - velocityActual.East;
eastDerivative = (eastError - eastErrorLast) / dT;
eastError = velocityDesired.East - velocityActual.East; eastIntegral = bound(eastIntegral + eastError * dT,
eastDerivative = (eastError - eastErrorLast) / guidanceSettings.VelPIDUpdatePeriod; -guidanceSettings.MaxVelIntegral,
eastIntegral = guidanceSettings.MaxVelIntegral);
bound(eastIntegral + eastError * guidanceSettings.VelPIDUpdatePeriod, -guidanceSettings.MaxVelIntegral, eastErrorLast = eastError;
guidanceSettings.MaxVelIntegral); eastCommand = eastError * guidanceSettings.VelP + eastDerivative * guidanceSettings.VelD + eastIntegral * guidanceSettings.VelI;
eastErrorLast = eastError;
eastCommand = // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
eastError * guidanceSettings.VelP + eastDerivative * guidanceSettings.VelD + eastIntegral * guidanceSettings.VelI; // craft should move similarly for 5 deg roll versus 5 deg pitch
attitudeDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) +
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
// craft should move similarly for 5 deg roll versus 5 deg pitch -stabSettings.PitchMax, stabSettings.PitchMax);
attitudeDesired.Pitch = attitudeDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) +
bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + eastCommand * sinf(attitudeActual.Yaw * M_PI / 180), eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-stabSettings.PitchMax, stabSettings.PitchMax); -stabSettings.RollMax, stabSettings.RollMax);
attitudeDesired.Roll =
bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + eastCommand * cosf(attitudeActual.Yaw * M_PI / 180), downError = velocityDesired.Down - velocityActual.Down;
-stabSettings.RollMax, stabSettings.RollMax); downDerivative = (downError - downErrorLast) / guidanceSettings.VelPIDUpdatePeriod;
downIntegral = bound(downIntegral + downError * guidanceSettings.VelPIDUpdatePeriod,
downError = velocityDesired.Down - velocityActual.Down; -guidanceSettings.MaxThrottleIntegral,
downDerivative = (downError - downErrorLast) / guidanceSettings.VelPIDUpdatePeriod; guidanceSettings.MaxThrottleIntegral);
downIntegral = downErrorLast = downError;
bound(downIntegral + downError * guidanceSettings.VelPIDUpdatePeriod, -guidanceSettings.MaxThrottleIntegral, attitudeDesired.Throttle = bound(downError * guidanceSettings.DownP + downDerivative * guidanceSettings.DownD +
guidanceSettings.MaxThrottleIntegral); downIntegral * guidanceSettings.DownI, 0, 1);
downErrorLast = downError;
attitudeDesired.Throttle = // For now override throttle with manual control. Disable at your risk, quad goes to China.
bound(downError * guidanceSettings.DownP + downDerivative * guidanceSettings.DownD + ManualControlCommandData manualControl;
downIntegral * guidanceSettings.DownI, 0, 1); ManualControlCommandGet(&manualControl);
attitudeDesired.Throttle = manualControl.Throttle;
AttitudeDesiredSet(&attitudeDesired);
AttitudeDesiredSet(&attitudeDesired);
}
vTaskDelayUntil(&lastSysTime, guidanceSettings.VelPIDUpdatePeriod / portTICK_RATE_MS);
}
} }
/** /**