Rate limit the StC force in pitch/roll direction

dzalkind · Nov 21, 2024 · 26ccd3d · 26ccd3d
1 parent 62db94d
commit 26ccd3d
Showing 1 changed file with 5 additions and 3 deletions.
diff --git a/rosco/controller/src/Controllers.f90 b/rosco/controller/src/Controllers.f90
@@ -846,13 +846,15 @@ SUBROUTINE StructuralControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar)
 
             ! Pitch and roll force commands
 
-            ! PIDController(error, kp, ki, kd, tf, minValue, maxValue, DT, I0, piP, reset, objInst, LocalVar)
-
             LocalVar%Force_Roll = PIDController(LocalVar%PtfmRDX_F, CntrPar%StC_F_PID(1), CntrPar%StC_F_PID(2), CntrPar%StC_F_PID(3), 100.0_DbKi, CntrPar%StC_F_Lims(1), CntrPar%StC_F_Lims(2), LocalVar%DT, 0.0, LocalVar%piP, LocalVar%restart, objInst, LocalVar)
             LocalVar%Force_Pitch = PIDController(LocalVar%PtfmRDY_F, CntrPar%StC_F_PID(1), CntrPar%StC_F_PID(2), CntrPar%StC_F_PID(3), 100.0_DbKi, CntrPar%StC_F_Lims(1), CntrPar%StC_F_Lims(2), LocalVar%DT, 0.0, LocalVar%piP, LocalVar%restart, objInst, LocalVar)
 
+
+            ! Rate limit
+            LocalVar%Force_Roll = ratelimit(LocalVar%Force_Roll,CntrPar%StC_F_Rates(1),CntrPar%StC_F_Rates(2), LocalVar%DT, LocalVar%restart,LocalVar%rlP, objInst%instRL)
+            LocalVar%Force_Pitch = ratelimit(LocalVar%Force_Pitch,CntrPar%StC_F_Rates(1),CntrPar%StC_F_Rates(2), LocalVar%DT, LocalVar%restart,LocalVar%rlP, objInst%instRL)
+
             T = RESHAPE((/CntrPar%StC_T_Roll, CntrPar%StC_T_Pitch/),(/6,2/))
-            ! T = RESHAPE((/0.0, 0.0, 0.0, 0.0, -0.0, -0.0, -1.0, -0.5, 0.5, 1.0, 0.5, -0.5/),(/6,2/))  ! 0 roll correction
             Inp = RESHAPE( (/LocalVar%Force_Roll, LocalVar%Force_Pitch/) , (/2,1/))
             F_Out = matmul(T,Inp)