From 26ccd3d9eaf6766d5e66c6750bb151d98e0cc172 Mon Sep 17 00:00:00 2001 From: dzalkind Date: Thu, 21 Nov 2024 15:26:43 -0700 Subject: [PATCH] Rate limit the StC force in pitch/roll direction --- rosco/controller/src/Controllers.f90 | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rosco/controller/src/Controllers.f90 b/rosco/controller/src/Controllers.f90 index c466b888..e2debe12 100644 --- 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)