From 4717c376c0dfe57510894e8012bbeb2e18692e79 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 11 Sep 2012 13:21:53 -0500 Subject: [PATCH] Stabilization: For the inner loop call the setpoint shaping version so that the derivative term can weight the sticks and gyros independently. --- flight/Modules/Stabilization/stabilization.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 2e49f89cf..fc4a5dcc5 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -247,7 +247,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; @@ -263,7 +263,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; @@ -287,7 +287,7 @@ static void stabilizationTask(void* parameters) // Compute desired rate as input biased towards leveling rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; - actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break; @@ -309,7 +309,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); - actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); break;