Skip to content

Commit

Permalink
Wind Speed Estimator & Related Updates (NREL#10)
Browse files Browse the repository at this point in the history 
* Filter WSE signal

* Update Q

* Typo fix

* Update WSE tuning

* Move WSE Filter so only GenTq controller uses it

* Move WSE so it initializes as well

* Parameter updates

* Filtered GenTq for possible WSE use

* filter rotor speed, blade pitch, generator torque

* Update parameters

* Minor re-org and cleanup

* Build DISCON

* Remove tower-top motion from wind speed estimate

* Update commenting
  • Loading branch information
@nikhar-abbas
nikhar-abbas authored Jul 13, 2020
1 parent 70ff9d0 commit 4e383cf
Show file tree
Hide file tree
Showing 8 changed files with 39 additions and 37 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,3 +31,4 @@ set(SOURCES
)

add_library(discon SHARED ${SOURCES})
# add_library(discon_yawAPI SHARED ${SOURCES})
32 changes: 14 additions & 18 deletions src/ControllerBlocks.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,21 +150,21 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
IF (CntrPar%WE_Mode == 1) THEN
LocalVar%WE_VwIdot = CntrPar%WE_Gamma/CntrPar%WE_Jtot*(LocalVar%VS_LastGenTrq*CntrPar%WE_GearboxRatio - AeroDynTorque(LocalVar, CntrPar, PerfData))
LocalVar%WE_VwI = LocalVar%WE_VwI + LocalVar%WE_VwIdot*LocalVar%DT
LocalVar%WE_Vw = LocalVar%WE_VwI + CntrPar%WE_Gamma*LocalVar%RotSpeed
LocalVar%WE_Vw = LocalVar%WE_VwI + CntrPar%WE_Gamma*LocalVar%RotSpeedF

! Extended Kalman Filter (EKF) implementation
ELSEIF (CntrPar%WE_Mode == 2) THEN
! Define contant values
L = 4.0 * CntrPar%WE_BladeRadius
Ti = 0.2
L = 6.0 * CntrPar%WE_BladeRadius
Ti = 0.18
R_m = 0.02
H = RESHAPE((/1.0 , 0.0 , 0.0/),(/1,3/))
! Define matrices to be filled
F = RESHAPE((/0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0/),(/3,3/))
Q = RESHAPE((/0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0/),(/3,3/))
IF (LocalVar%iStatus == 0) THEN
! Initialize recurring values
om_r = LocalVar%RotSpeed
om_r = LocalVar%RotSpeedF
v_t = 0.0
v_m = LocalVar%HorWindV
v_h = LocalVar%HorWindV
Expand All @@ -176,9 +176,9 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
! Find estimated operating Cp and system pole
A_op = interp1d(CntrPar%WE_FOPoles_v,CntrPar%WE_FOPoles,v_h)

! TEST INTERP2D
lambda = LocalVar%RotSpeed * CntrPar%WE_BladeRadius/v_h
Cp_op = interp2d(PerfData%Beta_vec,PerfData%TSR_vec,PerfData%Cp_mat, LocalVar%BlPitch(1)*R2D, lambda )
! Find Cp
lambda = LocalVar%RotSpeedF * CntrPar%WE_BladeRadius/v_h
Cp_op = interp2d(PerfData%Beta_vec,PerfData%TSR_vec,PerfData%Cp_mat, LocalVar%PC_PitComTF*R2D, lambda)
Cp_op = max(0.0,Cp_op)

! Update Jacobian
Expand All @@ -196,7 +196,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
! Prediction update
Tau_r = AeroDynTorque(LocalVar,CntrPar,PerfData)
a = PI * v_m/(2.0*L)
dxh(1,1) = 1.0/CntrPar%WE_Jtot * (Tau_r - CntrPar%WE_GearboxRatio * LocalVar%VS_LastGenTrq)
dxh(1,1) = 1.0/CntrPar%WE_Jtot * (Tau_r - CntrPar%WE_GearboxRatio * LocalVar%VS_LastGenTrqF)
dxh(2,1) = -a*v_t
dxh(3,1) = 0.0

Expand All @@ -206,7 +206,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
! Measurement update
S = MATMUL(H,MATMUL(P,TRANSPOSE(H))) + R_m ! NJA: (H*T*H') \approx 0
K = MATMUL(P,TRANSPOSE(H))/S(1,1)
xh = xh + K*(LocalVar%GenSpeedF/CntrPar%WE_GearboxRatio - om_r)
xh = xh + K*(LocalVar%RotSpeedF - om_r)
P = MATMUL(identity(3) - MATMUL(K,H),P)

! Wind Speed Estimate
Expand All @@ -222,7 +222,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)

ELSE
! Define Variables
F_WECornerFreq = 0.0333 ! Fix to 30 second time constant for now
F_WECornerFreq = 0.20944 ! Fix to 30 second time constant for now

! 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)
Expand All @@ -247,7 +247,7 @@ SUBROUTINE SetpointSmoother(LocalVar, CntrPar, objInst)
! ------ Setpoint Smoothing ------
IF ( CntrPar%SS_Mode == 1) THEN
! Find setpoint shift amount
DelOmega = ((LocalVar%BlPitch(1) - CntrPar%PC_MinPit)/0.524) * CntrPar%SS_VSGain - ((CntrPar%VS_RtTq - LocalVar%VS_LastGenTrq))/CntrPar%VS_RtTq * CntrPar%SS_PCGain ! Normalize to 30 degrees for now
DelOmega = ((LocalVar%PC_PitComT - CntrPar%PC_MinPit)/0.524) * CntrPar%SS_VSGain - ((CntrPar%VS_RtTq - LocalVar%VS_LastGenTrq))/CntrPar%VS_RtTq * CntrPar%SS_PCGain ! Normalize to 30 degrees for now
DelOmega = DelOmega * CntrPar%PC_RefSpd
! Filter
LocalVar%SS_DelOmegaF = LPFilter(DelOmega, LocalVar%DT, CntrPar%F_SSCornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF)
Expand All @@ -272,13 +272,9 @@ REAL FUNCTION PitchSaturation(LocalVar, CntrPar, objInst)
REAL(4) :: Vhat ! Estimated wind speed without towertop motion [m/s]
REAL(4) :: Vhatf ! 30 second low pass filtered Estimated wind speed without towertop motion [m/s]

! Account for towertop motions in wind speed estimate
! Integrate Towertop Acceleration
! dV_towertop =
! V_towertop = PIController(LocalVar%FA_Acc, 0.0, 1.0, -100.00, 100.00, LocalVar%DT, 0.0, .FALSE., objInst%instPI)

Vhat = LocalVar%WE_Vw
Vhatf = LPFilter(Vhat,LocalVar%DT,0.2,LocalVar%iStatus,.FALSE.,objInst%instLPF)
V_towertop = PIController(LocalVar%FA_Acc, 0.0, 1.0, -100.0, 100.0, LocalVar%DT, 0.0, .FALSE., objInst%instPI)
Vhat = LocalVar%WE_Vw_F + V_towertop
Vhatf = SecLPFilter(Vhat,LocalVar%DT,0.21,0.7,LocalVar%iStatus,.FALSE.,objInst%instSecLPF) ! 30 second time constant

! Define minimum blade pitch angle as a function of estimated wind speed
PitchSaturation = interp1d(CntrPar%PS_WindSpeeds, CntrPar%PS_BldPitchMin, Vhatf)
Expand Down
9 changes: 2 additions & 7 deletions src/Controllers.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,11 +65,7 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst)
LocalVar%PC_TF = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_TF, LocalVar%PC_PitComT) ! TF gains (derivative filter) !NJA - need to clarify

! Compute the collective pitch command associated with the proportional and integral gains:
IF (LocalVar%iStatus == 0) THEN
LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_FinePit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%PitCom(1), .TRUE., objInst%instPI)
ELSE
LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_FinePit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%BlPitch(1), .FALSE., objInst%instPI)
END IF
LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_FinePit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%BlPitch(1), .FALSE., objInst%instPI)

! Find individual pitch control contribution
IF ((CntrPar%IPC_ControlMode >= 1) .OR. (CntrPar%Y_ControlMode == 2)) THEN
Expand All @@ -85,7 +81,7 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst)
LocalVar%FA_PitCom = 0.0 ! THIS IS AN ARRAY!!
ENDIF

! Peak Shaving
! Pitch Saturation
IF (CntrPar%PS_Mode == 1) THEN
LocalVar%PC_MinPit = PitchSaturation(LocalVar,CntrPar,objInst)
LocalVar%PC_MinPit = max(LocalVar%PC_MinPit, CntrPar%PC_MinPit)
Expand Down Expand Up @@ -162,7 +158,6 @@ SUBROUTINE VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst)
! K*Omega^2 control law with PI torque control in transition regions
ELSE
! Update PI loops for region 1.5 and 2.5 PI control
! LocalVar%GenArTq = PIController(LocalVar%VS_SpdErrAr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MaxOMTq, CntrPar%VS_ArSatTq, LocalVar%DT, CntrPar%VS_RtTq, .TRUE., objInst%instPI)
LocalVar%GenArTq = PIController(LocalVar%VS_SpdErrAr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MaxOMTq, CntrPar%VS_ArSatTq, LocalVar%DT, CntrPar%VS_MaxOMTq, .FALSE., objInst%instPI)
LocalVar%GenBrTq = PIController(LocalVar%VS_SpdErrBr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, CntrPar%VS_MinOMTq, LocalVar%DT, CntrPar%VS_MinOMTq, .FALSE., objInst%instPI)

Expand Down
3 changes: 1 addition & 2 deletions src/DISCON.F90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,9 +68,8 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME

CALL StateMachine(CntrPar, LocalVar)
CALL WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)

CALL SetpointSmoother(LocalVar, CntrPar, objInst)

CALL ComputeVariablesSetpoints(CntrPar, LocalVar, objInst)
CALL VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst)
CALL PitchControl(avrSWAP, CntrPar, LocalVar, objInst)
CALL YawRateControl(avrSWAP, CntrPar, LocalVar, objInst)
Expand Down
12 changes: 11 additions & 1 deletion src/Filters.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -258,12 +258,14 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst)
TYPE(LocalVariables), INTENT(INOUT) :: LocalVar
TYPE(ObjectInstances), INTENT(INOUT) :: objInst

! Filter the HSS (generator) speed measurement:
! Filter the HSS (generator) and LSS (rotor) speed measurement:
! Apply Low-Pass Filter (choice between first- and second-order low-pass filter)
IF (CntrPar%F_LPFType == 1) THEN
LocalVar%GenSpeedF = LPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF)
LocalVar%RotSpeedF = LPFilter(LocalVar%RotSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF)
ELSEIF (CntrPar%F_LPFType == 2) THEN
LocalVar%GenSpeedF = SecLPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, CntrPar%F_LPFDamping, LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! Second-order low-pass filter on generator speed
LocalVar%RotSpeedF = SecLPFilter(LocalVar%RotSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, CntrPar%F_LPFDamping, LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! Second-order low-pass filter on generator speed
ENDIF
! Apply Notch Fitler
IF (CntrPar%F_NotchType == 1 .OR. CntrPar%F_NotchType == 3) THEN
Expand All @@ -282,5 +284,13 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst)

LocalVar%FA_AccHPF = HPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%FA_HPFCornerFreq, LocalVar%iStatus, .FALSE., objInst%instHPF)

! Wind Speed Estimator
! LocalVar%We_Vw_F = SecLPFilter(LocalVar%We_Vw, LocalVar%DT, 0.62831, 0.7, LocalVar%iStatus, .FALSE., objInst%instSecLPF)
LocalVar%We_Vw_F = LPFilter(LocalVar%We_Vw, LocalVar%DT, CntrPar%F_LPFCornerFreq/2.0, LocalVar%iStatus, .FALSE., objInst%instLPF)

! Control commands (used by WSE, mostly)
LocalVar%VS_LastGenTrqF = SecLPFilter(LocalVar%VS_LastGenTrq, LocalVar%dt, CntrPar%F_LPFCornerFreq, 0.7, LocalVar%iStatus, .FALSE., objInst%instSecLPF)
LocalVar%PC_PitComTF = LPFilter(LocalVar%PC_PitComT, LocalVar%dt, CntrPar%F_LPFCornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF)

END SUBROUTINE PreFilterMeasuredSignals
END MODULE Filters
6 changes: 3 additions & 3 deletions src/Functions.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -431,10 +431,10 @@ REAL FUNCTION AeroDynTorque(LocalVar, CntrPar, PerfData)

! Find Torque
RotorArea = PI*CntrPar%WE_BladeRadius**2
Lambda = LocalVar%RotSpeed*CntrPar%WE_BladeRadius/LocalVar%WE_Vw
Lambda = LocalVar%RotSpeedF*CntrPar%WE_BladeRadius/LocalVar%WE_Vw
! Cp = CPfunction(CntrPar%WE_CP, Lambda)
Cp = interp2d(PerfData%Beta_vec,PerfData%TSR_vec,PerfData%Cp_mat, LocalVar%BlPitch(1)*R2D, Lambda)
AeroDynTorque = 0.5*(CntrPar%WE_RhoAir*RotorArea)*(LocalVar%WE_Vw**3/LocalVar%RotSpeed)*Cp
Cp = interp2d(PerfData%Beta_vec,PerfData%TSR_vec,PerfData%Cp_mat, LocalVar%PC_PitComTF*R2D, Lambda)
AeroDynTorque = 0.5*(CntrPar%WE_RhoAir*RotorArea)*(LocalVar%WE_Vw**3/LocalVar%RotSpeedF)*Cp
AeroDynTorque = MAX(AeroDynTorque, 0.0)

END FUNCTION AeroDynTorque
Expand Down
4 changes: 4 additions & 0 deletions src/ROSCO_Types.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,6 +154,7 @@ MODULE ROSCO_Types
REAL(4) :: FA_AccHPF ! High-pass filtered fore-aft acceleration [m/s^2]
REAL(4) :: FA_AccHPFI ! Tower velocity, high-pass filtered and integrated fore-aft acceleration [m/s]
REAL(4) :: FA_PitCom(3) ! Tower fore-aft vibration damping pitch contribution [rad]
REAL(4) :: RotSpeedF ! Filtered LSS (generator) speed [rad/s].
REAL(4) :: GenSpeedF ! Filtered HSS (generator) speed [rad/s].
REAL(4) :: GenTq ! Electrical generator torque, [Nm].
REAL(4) :: GenTqMeas ! Measured generator torque [Nm]
Expand All @@ -168,6 +169,7 @@ MODULE ROSCO_Types
REAL(4) :: PC_MaxPit ! Maximum pitch setting in pitch controller (variable) [rad].
REAL(4) :: PC_MinPit ! Minimum pitch setting in pitch controller (variable) [rad].
REAL(4) :: PC_PitComT ! Total command pitch based on the sum of the proportional and integral terms [rad].
REAL(4) :: PC_PitComTF ! Filtered Total command pitch based on the sum of the proportional and integral terms [rad].
REAL(4) :: PC_PitComT_IPC(3) ! Total command pitch based on the sum of the proportional and integral terms, including IPC term [rad].
REAL(4) :: PC_PwrErr ! Power error with respect to rated power [W]
REAL(4) :: PC_SineExcitation ! Sine contribution to pitch signal
Expand All @@ -183,8 +185,10 @@ MODULE ROSCO_Types
REAL(4) :: VS_SpdErr ! Current speed error for tip-speed-ratio tracking controller (generator torque control) [rad/s].
INTEGER(4) :: VS_State ! State of the torque control system
REAL(4) :: WE_Vw ! Estimated wind speed [m/s]
REAL(4) :: WE_Vw_F ! Filtered estimated wind speed [m/s]
REAL(4) :: WE_VwI ! Integrated wind speed quantity for estimation [m/s]
REAL(4) :: WE_VwIdot ! Differentiated integrated wind speed quantity for estimation [m/s]
REAL(4) :: VS_LastGenTrqF ! Differentiated integrated wind speed quantity for estimation [m/s]
REAL(4) :: Y_AccErr ! Accumulated yaw error [rad].
REAL(4) :: Y_ErrLPFFast ! Filtered yaw error by fast low pass filter [rad].
REAL(4) :: Y_ErrLPFSlow ! Filtered yaw error by slow low pass filter [rad].
Expand Down
9 changes: 3 additions & 6 deletions src/ReadSetParameters.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,7 +236,6 @@ SUBROUTINE ComputeVariablesSetpoints(CntrPar, LocalVar, objInst)
REAL(4) :: VS_RefSpd ! Referece speed for variable speed torque controller, [rad/s]
REAL(4) :: PC_RefSpd ! Referece speed for pitch controller, [rad/s]
REAL(4) :: Omega_op ! Optimal TSR-tracking generator speed, [rad/s]
REAL(4) :: WE_Vw_f ! Filtered Wind Speed Estimate
! temp
! REAL(4) :: VS_TSRop = 7.5

Expand All @@ -257,9 +256,7 @@ SUBROUTINE ComputeVariablesSetpoints(CntrPar, LocalVar, objInst)
! ----- Torque controller reference errors -----
! Define VS reference generator speed [rad/s]
IF (CntrPar%VS_ControlMode == 2) THEN
! WE_Vw_f = LPFilter(LocalVar%We_Vw, LocalVar%DT, 0.625, LocalVar%iStatus, .FALSE., objInst%instLPF)
WE_Vw_f = LocalVar%We_Vw
VS_RefSpd = (CntrPar%VS_TSRopt * WE_Vw_f / CntrPar%WE_BladeRadius) * CntrPar%WE_GearboxRatio
VS_RefSpd = (CntrPar%VS_TSRopt * LocalVar%We_Vw_F / CntrPar%WE_BladeRadius) * CntrPar%WE_GearboxRatio
VS_RefSpd = saturate(VS_RefSpd,CntrPar%VS_MinOMSpd, CntrPar%VS_RefSpd)
ELSE
VS_RefSpd = CntrPar%VS_RefSpd
Expand Down Expand Up @@ -404,12 +401,12 @@ SUBROUTINE Assert(LocalVar, CntrPar, avrSWAP, aviFAIL, ErrMsg, size_avcMSG)

IF (CntrPar%VS_KP(1) > 0.0) THEN
aviFAIL = -1
ErrMsg = 'VS_KP must be greater than zero.'
ErrMsg = 'VS_KP must be less than zero.'
ENDIF

IF (CntrPar%VS_KI(1) > 0.0) THEN
aviFAIL = -1
ErrMsg = 'VS_KI must be greater than zero.'
ErrMsg = 'VS_KI must be less than zero.'
ENDIF

IF (CntrPar%PC_RefSpd <= 0.0) THEN
Expand Down

0 comments on commit 4e383cf

Please sign in to comment.