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

Merge branch 'max_rate_for_attitude' into next

Browse Source
This commit is contained in:
James Cotton 2011-09-12 11:51:03 -05:00
commit 63bb80649b
1 changed files with 24 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
flight/Modules/Stabilization/stabilization.c
View File

@ -235,6 +235,9 @@ static void stabilizationTask(void* parameters)
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
rateDesiredAxis[i] = attitudeDesiredAxis[i];
// Zero attitude and axis lock accumulators
pids[PID_ROLL + i] = 0;
axis_lock_accum[i] = 0;
break;
@ -249,11 +252,20 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
// Zero attitude and axis lock accumulators
pids[PID_ROLL + i] = 0;
axis_lock_accum[i] = 0;
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]);
if(rateDesiredAxis[i] > settings.MaximumRate[i])
rateDesiredAxis[i] = settings.MaximumRate[i];
else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct])
rateDesiredAxis[i] = -settings.MaximumRate[i];
axis_lock_accum[i] = 0;
break;
@ -272,6 +284,12 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i]);
}
if(rateDesiredAxis[i] > settings.MaximumRate[i])
rateDesiredAxis[i] = settings.MaximumRate[i];
else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct])
rateDesiredAxis[i] = -settings.MaximumRate[i];
break;
}
}
@ -284,11 +302,6 @@ static void stabilizationTask(void* parameters)
//Calculate desired command
for(int8_t ct=0; ct< MAX_AXES; ct++)
{
if(rateDesiredAxis[ct] > settings.MaximumRate[ct])
rateDesiredAxis[ct] = settings.MaximumRate[ct];
else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct])
rateDesiredAxis[ct] = -settings.MaximumRate[ct];
switch(stabDesired.StabilizationMode[ct])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
@ -306,14 +319,20 @@ static void stabilizationTask(void* parameters)
case ROLL:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
shouldUpdate = 1;
pids[PID_RATE_ROLL].iAccumulator = 0;
pids[PID_ROLL].iAccumulator = 0;
break;
case PITCH:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
shouldUpdate = 1;
pids[PID_RATE_PITCH].iAccumulator = 0;
pids[PID_PITCH].iAccumulator = 0;
break;
case YAW:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
shouldUpdate = 1;
pids[PID_RATE_YAW].iAccumulator = 0;
pids[PID_YAW].iAccumulator = 0;
break;
}
break;