From 6945f17ebae9523b4adcbac646a522b44a289978 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 11 Sep 2011 12:27:17 -0500 Subject: [PATCH 1/2] Make the StabilizationSettings.MaxRate field only apply to the stabilized modes. That way ManualRate can exceed MaxRate. --- flight/Modules/Stabilization/stabilization.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 7142c9770..50a8c253a 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -254,6 +254,13 @@ static void stabilizationTask(void* parameters) } 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 +279,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 +297,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: From b38081bb1bb331f36fe5395e6f75244ab4d07af7 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 11 Sep 2011 16:33:38 -0500 Subject: [PATCH 2/2] Stabilization: When in `none` mode zero the integral accumulators for rate and attitude loops. When not using outer loop zero that accumulator. --- flight/Modules/Stabilization/stabilization.c | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 50a8c253a..5669df9d5 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -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,6 +252,8 @@ 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; } @@ -314,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;