From 6945f17ebae9523b4adcbac646a522b44a289978 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 11 Sep 2011 12:27:17 -0500 Subject: [PATCH] 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: