Skip to content

Commit

Permalink
Add error variables, error catching for interp functions
Browse files Browse the repository at this point in the history
  • Loading branch information
@dzalkind
dzalkind committed Apr 5, 2021
1 parent c010d71 commit 37838fb
Show file tree
Hide file tree
Showing 6 changed files with 185 additions and 55 deletions.
36 changes: 27 additions & 9 deletions src/ControllerBlocks.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,11 +101,11 @@ SUBROUTINE StateMachine(CntrPar, LocalVar)
END IF
END SUBROUTINE StateMachine
!-------------------------------------------------------------------------------------------------------------------------------
SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, ErrVar)
! 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 ROSCO_Types, ONLY : LocalVariables, ControlParameters, ObjectInstances, PerformanceData, DebugVariables
USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, ObjectInstances, PerformanceData, DebugVariables, ErrorVariables
IMPLICIT NONE

! Inputs
Expand All @@ -114,6 +114,8 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
TYPE(ObjectInstances), INTENT(INOUT) :: objInst
TYPE(PerformanceData), INTENT(INOUT) :: PerfData
TYPE(DebugVariables), INTENT(INOUT) :: DebugVar
TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar

! Allocate Variables
REAL(8) :: F_WECornerFreq ! Corner frequency (-3dB point) for first order low pass filter for measured hub height wind speed [Hz]

Expand Down Expand Up @@ -152,7 +154,10 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)

! Inversion and Invariance Filter implementation
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))
! Compute AeroDynTorque
Tau_r = AeroDynTorque(LocalVar, CntrPar, PerfData, ErrVar)

LocalVar%WE_VwIdot = CntrPar%WE_Gamma/CntrPar%WE_Jtot*(LocalVar%VS_LastGenTrq*CntrPar%WE_GearboxRatio - Tau_r)
LocalVar%WE_VwI = LocalVar%WE_VwI + LocalVar%WE_VwIdot*LocalVar%DT
LocalVar%WE_Vw = LocalVar%WE_VwI + CntrPar%WE_Gamma*LocalVar%RotSpeedF

Expand Down Expand Up @@ -180,11 +185,13 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)

ELSE
! Find estimated operating Cp and system pole
A_op = interp1d(CntrPar%WE_FOPoles_v,CntrPar%WE_FOPoles,v_h)
A_op = interp1d(CntrPar%WE_FOPoles_v,CntrPar%WE_FOPoles,v_h,ErrVar)

! 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 )

Cp_op = interp2d(PerfData%Beta_vec,PerfData%TSR_vec,PerfData%Cp_mat, LocalVar%BlPitch(1)*R2D, lambda, ErrVar)

Cp_op = max(0.0,Cp_op)

! Update Jacobian
Expand All @@ -200,7 +207,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
Q(3,3) = (2.0**2.0)/600.0

! Prediction update
Tau_r = AeroDynTorque(LocalVar,CntrPar,PerfData)
Tau_r = AeroDynTorque(LocalVar, CntrPar, PerfData, ErrVar)
a = PI * v_m/(2.0*L)
dxh(1,1) = 1.0/CntrPar%WE_Jtot * (Tau_r - CntrPar%WE_GearboxRatio * LocalVar%VS_LastGenTrqF)
dxh(2,1) = -a*v_t
Expand Down Expand Up @@ -236,6 +243,11 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
LocalVar%WE_Vw = LPFilter(LocalVar%HorWindV, LocalVar%DT, F_WECornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF)
ENDIF

! Error Catching
IF (ErrVar%aviFAIL == -1) THEN
ErrVar%ErrMsg = 'WindSpeedEstimator:'//TRIM(ErrVar%ErrMsg)
END IF

END SUBROUTINE WindSpeedEstimator
!-------------------------------------------------------------------------------------------------------------------------------
SUBROUTINE SetpointSmoother(LocalVar, CntrPar, objInst)
Expand Down Expand Up @@ -265,20 +277,26 @@ SUBROUTINE SetpointSmoother(LocalVar, CntrPar, objInst)

END SUBROUTINE SetpointSmoother
!-------------------------------------------------------------------------------------------------------------------------------
REAL FUNCTION PitchSaturation(LocalVar, CntrPar, objInst, DebugVar)
REAL FUNCTION PitchSaturation(LocalVar, CntrPar, objInst, DebugVar, ErrVar)
! PitchSaturation defines a minimum blade pitch angle based on a lookup table provided by DISCON.IN
! SS_Mode = 0, No setpoint smoothing
! SS_Mode = 1, Implement pitch saturation
USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, ObjectInstances, DebugVariables
USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, ObjectInstances, DebugVariables, ErrorVariables
IMPLICIT NONE
! Inputs
TYPE(ControlParameters), INTENT(IN) :: CntrPar
TYPE(LocalVariables), INTENT(INOUT) :: LocalVar
TYPE(ObjectInstances), INTENT(INOUT) :: objInst
TYPE(DebugVariables), INTENT(INOUT) :: DebugVar
TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar

! Define minimum blade pitch angle as a function of estimated wind speed
PitchSaturation = interp1d(CntrPar%PS_WindSpeeds, CntrPar%PS_BldPitchMin, LocalVar%WE_Vw_F)
PitchSaturation = interp1d(CntrPar%PS_WindSpeeds, CntrPar%PS_BldPitchMin, LocalVar%WE_Vw_F, ErrVar)

! Error Catching
IF (ErrVar%aviFAIL == -1) THEN
ErrVar%ErrMsg = 'PitchSaturation:'//TRIM(ErrVar%ErrMsg)
END IF

END FUNCTION PitchSaturation
!-------------------------------------------------------------------------------------------------------------------------------
Expand Down
24 changes: 16 additions & 8 deletions src/Controllers.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,21 +31,23 @@ MODULE Controllers

CONTAINS
!-------------------------------------------------------------------------------------------------------------------------------
SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar)
SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar, ErrVar)
! Blade pitch controller, generally maximizes rotor speed below rated (region 2) and regulates rotor speed above rated (region 3)
! PC_State = 0, fix blade pitch to fine pitch angle (PC_FinePit)
! PC_State = 1, is gain scheduled PI controller
! Additional loops/methods (enabled via switches in DISCON.IN):
! Individual pitch control
! Tower fore-aft damping
! Sine excitation on pitch
USE ROSCO_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances, DebugVariables
USE ROSCO_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances, DebugVariables, ErrorVariables

! Inputs
TYPE(ControlParameters), INTENT(INOUT) :: CntrPar
TYPE(LocalVariables), INTENT(INOUT) :: LocalVar
TYPE(ObjectInstances), INTENT(INOUT) :: objInst
TYPE(DebugVariables), INTENT(INOUT) :: DebugVar
TYPE(DebugVariables), INTENT(INOUT) :: DebugVar
TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar

! Allocate Variables:
REAL(C_FLOAT), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from the DLL controller.
INTEGER(4) :: K ! Index used for looping through blades.
Expand All @@ -60,10 +62,10 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar)
END IF

! Compute (interpolate) the gains based on previously commanded blade pitch angles and lookup table:
LocalVar%PC_KP = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KP, LocalVar%PC_PitComTF) ! Proportional gain
LocalVar%PC_KI = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KI, LocalVar%PC_PitComTF) ! Integral gain
LocalVar%PC_KD = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KD, LocalVar%PC_PitComTF) ! Derivative gain
LocalVar%PC_TF = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_TF, LocalVar%PC_PitComTF) ! TF gains (derivative filter) !NJA - need to clarify
LocalVar%PC_KP = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KP, LocalVar%PC_PitComTF, ErrVar) ! Proportional gain
LocalVar%PC_KI = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KI, LocalVar%PC_PitComTF, ErrVar) ! Integral gain
LocalVar%PC_KD = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KD, LocalVar%PC_PitComTF, ErrVar) ! Derivative gain
LocalVar%PC_TF = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_TF, LocalVar%PC_PitComTF, ErrVar) ! 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
Expand All @@ -88,7 +90,7 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar)

! Pitch Saturation
IF (CntrPar%PS_Mode == 1) THEN
LocalVar%PC_MinPit = PitchSaturation(LocalVar,CntrPar,objInst,DebugVar)
LocalVar%PC_MinPit = PitchSaturation(LocalVar,CntrPar,objInst,DebugVar, ErrVar)
LocalVar%PC_MinPit = max(LocalVar%PC_MinPit, CntrPar%PC_FinePit)
ELSE
LocalVar%PC_MinPit = CntrPar%PC_FinePit
Expand Down Expand Up @@ -124,6 +126,12 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar)
avrSWAP(43) = LocalVar%PitCom(2) ! "
avrSWAP(44) = LocalVar%PitCom(3) ! "
avrSWAP(45) = LocalVar%PitCom(1) ! Use the command angle of blade 1 if using collective pitch

! Error Catching
IF (ErrVar%aviFAIL == -1) THEN
ErrVar%ErrMsg = 'PitchControl:'//TRIM(ErrVar%ErrMsg)
END IF

END SUBROUTINE PitchControl
!-------------------------------------------------------------------------------------------------------------------------------
SUBROUTINE VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst)
Expand Down
17 changes: 13 additions & 4 deletions src/DISCON.F90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,29 +54,38 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME
TYPE(ObjectInstances), SAVE :: objInst
TYPE(PerformanceData), SAVE :: PerfData
TYPE(DebugVariables), SAVE :: DebugVar
TYPE(ErrorVariables), SAVE :: ErrVar

RootName = TRANSFER(avcOUTNAME, RootName)
!------------------------------------------------------------------------------------------------------------------------------
! Main control calculations
!------------------------------------------------------------------------------------------------------------------------------
! Read avrSWAP array into derived types/variables
CALL ReadAvrSWAP(avrSWAP, LocalVar)
CALL SetParameters(avrSWAP, aviFAIL, accINFILE, ErrMsg, SIZE(avcMSG), CntrPar, LocalVar, objInst, PerfData)
CALL SetParameters(avrSWAP, accINFILE, SIZE(avcMSG), CntrPar, LocalVar, objInst, PerfData, ErrVar)
CALL PreFilterMeasuredSignals(CntrPar, LocalVar, objInst)

IF ((LocalVar%iStatus >= 0) .AND. (aviFAIL >= 0)) THEN ! Only compute control calculations if no error has occurred and we are not on the last time step
CALL WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar)
CALL WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, ErrVar)
CALL ComputeVariablesSetpoints(CntrPar, LocalVar, objInst)
CALL StateMachine(CntrPar, LocalVar)
CALL SetpointSmoother(LocalVar, CntrPar, objInst)
CALL ComputeVariablesSetpoints(CntrPar, LocalVar, objInst)
CALL VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst)
CALL PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar)
CALL PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar, ErrVar)
CALL YawRateControl(avrSWAP, CntrPar, LocalVar, objInst)
CALL FlapControl(avrSWAP, CntrPar, LocalVar, objInst)
CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME))
END IF

avcMSG = TRANSFER(TRIM(ErrMsg)//C_NULL_CHAR, avcMSG, SIZE(avcMSG))
! Error Catching
IF (ErrVar%aviFAIL == -1) THEN
ErrVar%ErrMsg = 'ROSCO:'//TRIM(ErrVar%ErrMsg)
print * , TRIM(ErrVar%ErrMsg)
END IF
ErrMsg = ErrVar%ErrMsg
avcMSG = TRANSFER(TRIM(ErrVar%ErrMsg)//C_NULL_CHAR, avcMSG, SIZE(avcMSG))
aviFAIL = ErrVar%aviFAIL

RETURN
END SUBROUTINE DISCON
Loading

0 comments on commit 37838fb

Please sign in to comment.