Add filtered hub wind speed option to wind speed estimator

Ben-Mertz · Sep 19, 2019 · cc017f8 · cc017f8
1 parent 593e30a
commit cc017f8
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 14 deletions.
diff --git a/Parameter_files/NREL5MW/DISCON.IN b/Parameter_files/NREL5MW/DISCON.IN
@@ -12,6 +12,7 @@
 1                   ! PC_ControlMode    - Blade pitch control mode {0: No pitch, fix to fine pitch, 1: active PI blade pitch control}
 0					! Y_ControlMode		- Yaw control mode {0: no yaw control, 1: yaw rate control, 2: yaw-by-IPC}
 1                   ! SS_Mode           - Setpoint Smoother mode {0: no setpoint smoothing, 1: introduce setpoint smoothing}
+0                   ! WE_Mode           - Wind speed estimator mode {0: One-second low pass filtered hub height wind speed, 1: Imersion and Invariance Estimator (Ortega et al.)}
 
 !------- FILTERS ----------------------------------------------------------
 1.570796326			! F_LPFCornerFreq	- Corner frequency (-3dB point) in the low-pass filters, [Hz]

diff --git a/Source/ControllerBlocks.f90 b/Source/ControllerBlocks.f90
@@ -29,7 +29,6 @@ SUBROUTINE StateMachine(CntrPar, LocalVar)
     !       VS_State = 3, Region 2.5, transition between below and above-rated operating conditions (near-rated region) using PI torque control
     !       VS_State = 4, above-rated operation using pitch control (constant torque mode)
     !       VS_State = 5, above-rated operation using pitch and torque control (constant power mode)
-    SUBROUTINE StateMachine(CntrPar, LocalVar)
         USE DRC_Types, ONLY : LocalVariables, ControlParameters
         IMPLICIT NONE
 
@@ -88,20 +87,33 @@ SUBROUTINE StateMachine(CntrPar, LocalVar)
         END IF
     END SUBROUTINE StateMachine
 !-------------------------------------------------------------------------------------------------------------------------------
-    SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar)
-        USE DRC_Types, ONLY : LocalVariables, ControlParameters
+    SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst)
+    ! Wind Speed Estimator estimates wind speed at hub height. Currently implements two types of estimators
+    !       WE_Mode = 0, Filter hub height wind speed as passed from servodyn using first order low pass filter with 1Hz cornering frequency
+    !       WE_Mode = 1, Use Inversion and Inveriance filter as defined by Ortege et. al. 
+        USE DRC_Types!, ONLY : LocalVariables, ControlParameters, Obj
         IMPLICIT NONE
 
         ! Inputs
-        TYPE(ControlParameters), INTENT(IN) :: CntrPar
-        TYPE(LocalVariables), INTENT(INOUT) :: LocalVar 
-
-        ! Body
-        LocalVar%WE_VwIdot = CntrPar%WE_Gamma/CntrPar%WE_Jtot*(LocalVar%VS_LastGenTrq*CntrPar%WE_GearboxRatio - AeroDynTorque(LocalVar, CntrPar))
-
-        LocalVar%WE_VwI = LocalVar%WE_VwI + LocalVar%WE_VwIdot*LocalVar%DT
-        LocalVar%WE_Vw = LocalVar%WE_VwI + CntrPar%WE_Gamma*LocalVar%RotSpeed
-
+        TYPE(ControlParameters), INTENT(IN)     :: CntrPar
+        TYPE(LocalVariables), INTENT(INOUT)     :: LocalVar 
+        TYPE(ObjectInstances), INTENT(INOUT)    :: objInst
+        ! Allocate Variables
+        REAL(4)             :: F_WECornerFreq ! Corner frequency (-3dB point) for first order low pass filter for measured hub height wind speed [Hz]
+        ! Define Variables
+        F_WECornerFreq = 1  ! Fix to 1Hz for now
+
+        ! Define wind speed estimate
+        IF (CntrPar%WE_Mode == 1) THEN      
+            ! Inversion and Invariance Filter implementation
+            LocalVar%WE_VwIdot = CntrPar%WE_Gamma/CntrPar%WE_Jtot*(LocalVar%VS_LastGenTrq*CntrPar%WE_GearboxRatio - AeroDynTorque(LocalVar, CntrPar))
+            LocalVar%WE_VwI = LocalVar%WE_VwI + LocalVar%WE_VwIdot*LocalVar%DT
+            LocalVar%WE_Vw = LocalVar%WE_VwI + CntrPar%WE_Gamma*LocalVar%RotSpeed
+        ELSE                                
+            ! Filter wind speed at hub height as directly passed from OpenFAST
+            LocalVar%WE_Vw = LPFilter(LocalVar%HorWindV, LocalVar%DT, F_WECornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF)
+        END IF 
+
     END SUBROUTINE WindSpeedEstimator
 !-------------------------------------------------------------------------------------------------------------------------------
     SUBROUTINE SetpointSmoother(LocalVar, CntrPar, objInst)

diff --git a/Source/DRC_Types.f90 b/Source/DRC_Types.f90
@@ -54,9 +54,10 @@ MODULE DRC_Types
     REAL(4), DIMENSION(:), ALLOCATABLE  :: VS_KI                        ! Integral gain for generator PI torque controller, used in the transitional 2.5 region
 
     INTEGER(4)                          :: SS_Mode                      ! Setpoint Smoother mode {0: no setpoint smoothing, 1: introduce setpoint smoothing}
-    REAL(4)                             :: SS_VSGainBias                !  Variable speed torque controller gain bias, [(rad/s)/rad].
-    REAL(4)                             :: SS_PCGainBias                !  Collective pitch controller gain bias, [(rad/s)/Nm].
+    REAL(4)                             :: SS_VSGainBias                ! Variable speed torque controller gain bias, [(rad/s)/rad].
+    REAL(4)                             :: SS_PCGainBias                ! Collective pitch controller gain bias, [(rad/s)/Nm].
 
+    INTEGER(4)                          :: WE_Mode                      ! Wind speed estimator mode {0: One-second low pass filtered hub height wind speed, 1: Imersion and Invariance Estimator (Ortega et al.)
     REAL(4)                             :: WE_BladeRadius               ! Blade length [m]
     INTEGER(4)                          :: WE_CP_n                      ! Amount of parameters in the Cp array
     REAL(4), DIMENSION(:), ALLOCATABLE  :: WE_CP                        ! Parameters that define the parameterized CP(\lambda) function

diff --git a/Source/ReadSetParameters.f90 b/Source/ReadSetParameters.f90
@@ -34,6 +34,7 @@ SUBROUTINE ReadControlParameterFileSub(CntrPar)
         READ(UnControllerParameters, *) CntrPar%PC_ControlMode
         READ(UnControllerParameters, *) CntrPar%Y_ControlMode        
         READ(UnControllerParameters, *) CntrPar%SS_Mode        
+        READ(UnControllerParameters, *) CntrPar%WE_Mode        
         READ(UnControllerParameters, *)
 
         !----------------- FILTER CONSTANTS ---------------------