From 11f2b1a8dc57e4be06bb09b36e3e2050353deca1 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Fri, 17 Sep 2010 18:15:02 +0000 Subject: [PATCH] Flight/Stabilization: Dont cap error at maximum stabilization value - that is to scale control throws. Create another variable if we want this. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1676 ebee16cc-31ac-478f-84a7-5cbb03baadba --- .../Stabilization/simple/Stabilization/stabilization.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c b/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c index 474d2a3fd..2307268e1 100644 --- a/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c +++ b/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c @@ -111,7 +111,7 @@ static void stabilizationTask(void* parameters) AttitudeActualGet(&attitudeActual); // Pitch stabilization control loop - pitchError = bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch; + pitchError = attitudeDesired.Pitch - attitudeActual.Pitch; pitchDerivative = (pitchError - pitchErrorLast) / stabSettings.UpdatePeriod; pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -stabSettings.PitchIntegralLimit, stabSettings.PitchIntegralLimit); actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative; @@ -119,7 +119,7 @@ static void stabilizationTask(void* parameters) pitchErrorLast = pitchError; // Roll stabilization control loop - rollError = bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll; + rollError = attitudeDesired.Roll - attitudeActual.Roll; rollDerivative = (rollError - rollErrorLast) / stabSettings.UpdatePeriod; rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -stabSettings.RollIntegralLimit, stabSettings.RollIntegralLimit); actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;