Skip to content

Commit

Permalink
Merge branch 'master' into f/floating
Browse files Browse the repository at this point in the history
  • Loading branch information
@nikhar-abbas
nikhar-abbas committed Dec 18, 2019
2 parents 71ed9de + 9e53a23 commit 13a78e4
Show file tree
Hide file tree
Showing 5 changed files with 33 additions and 37 deletions.
25 changes: 5 additions & 20 deletions src/ControllerBlocks.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,11 +129,6 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData)
REAL(4) :: Tau_r ! Estimated rotor torque [Nm]
REAL(4) :: a ! wind variance
REAL(4) :: lambda ! tip-speed-ratio [rad]

! REAL(4), DIMENSION(1,23) :: WE_EKF_Vref
! REAL(4) :: WE_EKF_Vref(23)
! REAL(4) :: WE_EKF_Aref(23)
! REAL(4) :: WE_EKF_Cpref(23)
! - Covariance matrices
REAL(4), DIMENSION(3,3) :: F ! First order system jacobian
REAL(4), DIMENSION(3,3), SAVE :: P ! Covariance estiamte
Expand All @@ -144,12 +139,6 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData)
REAL(4), DIMENSION(1,1) :: S ! Innovation covariance
REAL(4), DIMENSION(3,1), SAVE :: K ! Kalman gain matrix
REAL(4) :: R_m ! Measurement noise covariance [(rad/s)^2]
REAL(4) :: temp ! temp variable

! WE_EKF_Vref = (/ 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25 /)
! WE_EKF_Aref = (/ -0.0203, -0.0270, -0.0338, -0.0405, -0.0473, -0.0540, -0.0608, -0.0675, -0.0743, -0.0671, -0.0939, -0.1257, -0.1601, -0.1973, -0.2364, -0.2783, -0.3223, -0.3678, -0.4153, -0.4632, -0.5122, -0.5629, -0.6194 /)
! WE_EKF_Cpref = (/ 0.4957, 0.4957, 0.4957, 0.4957, 0.4957, 0.4957, 0.4957, 0.4957, 0.4957, 0.4193, 0.3298, 0.2641, 0.2147, 0.1769, 0.1475, 0.1242, 0.1056, 0.0906, 0.0782, 0.0680, 0.0596, 0.0524, 0.0464 /)


! ---- Define wind speed estimate ----

Expand Down Expand Up @@ -178,11 +167,8 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData)
xh = RESHAPE((/om_r, v_t, v_m/),(/3,1/))
P = RESHAPE((/0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 1.0/),(/3,3/))
K = RESHAPE((/0.0,0.0,0.0/),(/3,1/))

ELSE
! Update estimated values
! v_h = v_m + v_t
a = PI * v_m/(2.0*L)

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

Expand All @@ -204,8 +190,8 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData)
Q(3,3) = (2.0**2.0)/600.0

! Prediction update
! Tau_r = 0.5 * CntrPar%WE_RhoAir * PI *CntrPar%WE_BladeRadius**3 * Cp_op * v_h**2 * 1.0/(lambda)
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(2,1) = -a*v_t
dxh(3,1) = 0.0
Expand All @@ -216,7 +202,8 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData)
! 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 - xh(1,1))
! xh = xh + K*(LocalVar%GenSpeedF/CntrPar%WE_GearboxRatio - xh(1,1))
xh = xh + K*(LocalVar%GenSpeedF/CntrPar%WE_GearboxRatio - om_r)
P = MATMUL(identity(3) - MATMUL(K,H),P)

! Wind Speed Estimate
Expand All @@ -227,9 +214,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData)
v_t = xh(2,1)
v_m = xh(3,1)
v_h = v_t + v_m
LocalVar%TestType = v_m + v_t
LocalVar%WE_Vw = v_m + v_t
! LocalVar%WE_Vw = LPFilter(v_m + v_t,LocalVar%DT,0.5,LocalVar%iStatus,.FALSE.,objInst%instLPF)
ENDIF

ELSE
Expand Down Expand Up @@ -259,7 +244,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_RtPwr - LocalVar%VS_GenPwr))/CntrPar%VS_RtPwr * CntrPar%SS_PCGain ! Normalize to 30 degrees for now
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 = DelOmega * CntrPar%PC_RefSpd
! Filter
LocalVar%SS_DelOmegaF = LPFilter(DelOmega, LocalVar%DT, CntrPar%F_SSCornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF)
Expand Down
2 changes: 1 addition & 1 deletion src/DISCON.F90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME
!------------------------------------------------------------------------------------------------------------------------------
! Read avrSWAP array into derived types/variables
CALL ReadAvrSWAP(avrSWAP, LocalVar)
CALL SetParameters(avrSWAP, aviFAIL, ErrMsg, SIZE(avcMSG), CntrPar, LocalVar, objInst, PerfData)
CALL SetParameters(avrSWAP, aviFAIL, accINFILE, ErrMsg, SIZE(avcMSG), CntrPar, LocalVar, objInst, PerfData)
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
Expand Down
12 changes: 9 additions & 3 deletions src/Filters.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,9 @@ MODULE Filters
CONTAINS
!-------------------------------------------------------------------------------------------------------------------------------
REAL FUNCTION LPFilter(InputSignal, DT, CornerFreq, iStatus, reset, inst)
! Discrete time Low-Pass Filter
! Discrete time Low-Pass Filter of the form:
! H(z) = (b1z + b0) / (a1*z + a0)
!
REAL(4), INTENT(IN) :: InputSignal
REAL(4), INTENT(IN) :: DT ! time step [s]
REAL(4), INTENT(IN) :: CornerFreq ! corner frequency [rad/s]
Expand All @@ -36,13 +38,17 @@ REAL FUNCTION LPFilter(InputSignal, DT, CornerFreq, iStatus, reset, inst)
LOGICAL(4), INTENT(IN) :: reset ! Reset the filter to the input signal

! Local
REAL(4), SAVE :: a1 ! Denominator coefficient 1
REAL(4), SAVE :: a0 ! Denominator coefficient 0
REAL(4), SAVE :: b1 ! Numerator coefficient 1
REAL(4), SAVE :: b0 ! Numerator coefficient 0

REAL(4), DIMENSION(99), SAVE :: InputSignalLast ! Input signal the last time this filter was called. Supports 99 separate instances.
REAL(4), DIMENSION(99), SAVE :: OutputSignalLast ! Output signal the last time this filter was called. Supports 99 separate instances.

! Initialization

IF ((iStatus == 0) .OR. reset) THEN
IF ((iStatus == 0) .OR. reset) THEN
! a1 =
OutputSignalLast(inst) = InputSignal
InputSignalLast(inst) = InputSignal
ENDIF
Expand Down
3 changes: 2 additions & 1 deletion src/Functions.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,7 +206,8 @@ REAL FUNCTION interp2d(xData, yData, zData, xq, yq)
DO i = 1,size(yData)
IF (yq == yData(i)) THEN ! On axis, just need 1d interpolation
ii = i
interp2d = interp1d(yData,zData(i,:),xq)
interp2d = interp1d(xData,zData(i,:),xq)
! interp2d = interp1d(yData,zData(i,:),xq)
RETURN
ELSEIF (yq <= yData(i)) THEN
ii = i
Expand Down
28 changes: 16 additions & 12 deletions src/ReadSetParameters.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,13 +31,17 @@ MODULE ReadSetParameters
CONTAINS
! -----------------------------------------------------------------------------------
! Read all constant control parameters from DISCON.IN parameter file
SUBROUTINE ReadControlParameterFileSub(CntrPar)
SUBROUTINE ReadControlParameterFileSub(CntrPar, accINFILE, accINFILE_size)!, accINFILE_size)
USE, INTRINSIC :: ISO_C_Binding
USE ROSCO_Types, ONLY : ControlParameters

INTEGER(4), PARAMETER :: UnControllerParameters = 89
TYPE(ControlParameters), INTENT(INOUT) :: CntrPar

OPEN(unit=UnControllerParameters, file='DISCON.IN', status='old', action='read')
INTEGER(4) :: accINFILE_size ! size of DISCON input filename
CHARACTER(accINFILE_size), INTENT(IN) :: accINFILE(accINFILE_size) ! DISCON input filename
INTEGER(4), PARAMETER :: UnControllerParameters = 89 ! Unit number to open file
TYPE(ControlParameters), INTENT(INOUT) :: CntrPar ! Control parameter type


OPEN(unit=UnControllerParameters, file=accINFILE(1), status='old', action='read')

!----------------------- HEADER ------------------------
READ(UnControllerParameters, *)
Expand Down Expand Up @@ -246,8 +250,8 @@ 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.6283, LocalVar%iStatus, .FALSE., objInst%instLPF)
! WE_Vw_f = LocalVar%We_Vw
! 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 = saturate(VS_RefSpd,CntrPar%VS_MinOMSpd, CntrPar%VS_RefSpd)
ELSE
Expand Down Expand Up @@ -474,17 +478,18 @@ SUBROUTINE Assert(LocalVar, CntrPar, avrSWAP, aviFAIL, ErrMsg, size_avcMSG)
END SUBROUTINE Assert
! -----------------------------------------------------------------------------------
! Define parameters for control actions
SUBROUTINE SetParameters(avrSWAP, aviFAIL, ErrMsg, size_avcMSG, CntrPar, LocalVar, objInst, PerfData)
SUBROUTINE SetParameters(avrSWAP, aviFAIL, accINFILE, ErrMsg, size_avcMSG, CntrPar, LocalVar, objInst, PerfData)
USE ROSCO_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances, PerformanceData

INTEGER(4), INTENT(IN) :: size_avcMSG
TYPE(ControlParameters), INTENT(INOUT) :: CntrPar
TYPE(LocalVariables), INTENT(INOUT) :: LocalVar
TYPE(ObjectInstances), INTENT(INOUT) :: objInst
TYPE(PerformanceData), INTENT(INOUT) :: PerfData

REAL(C_FLOAT), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller.
INTEGER(C_INT), INTENT(OUT) :: aviFAIL ! A flag used to indicate the success of this DLL call set as follows: 0 if the DLL call was successful, >0 if the DLL call was successful but cMessage should be issued as a warning messsage, <0 if the DLL call was unsuccessful or for any other reason the simulation is to be stopped at this point with cMessage as the error message.
CHARACTER(KIND=C_CHAR), INTENT(IN) :: accINFILE(NINT(avrSWAP(50))) ! The name of the parameter input file
CHARACTER(size_avcMSG-1), INTENT(OUT) :: ErrMsg ! a Fortran version of the C string argument (not considered an array here) [subtract 1 for the C null-character]
INTEGER(4) :: K ! Index used for looping through blades.

Expand Down Expand Up @@ -530,8 +535,7 @@ SUBROUTINE SetParameters(avrSWAP, aviFAIL, ErrMsg, size_avcMSG, CntrPar, LocalVa
'Visit our GitHub-page to contribute to this project: '//NEW_LINE('A')// &
'https://github.com/NREL/ROSCO '//NEW_LINE('A')// &
'------------------------------------------------------------------------------'

CALL ReadControlParameterFileSub(CntrPar)
CALL ReadControlParameterFileSub(CntrPar, accINFILE, NINT(avrSWAP(50)))

IF (CntrPar%WE_Mode > 0) THEN
CALL READCpFile(CntrPar,PerfData)
Expand Down Expand Up @@ -620,4 +624,4 @@ SUBROUTINE ReadCpFile(CntrPar,PerfData)
END DO

END SUBROUTINE ReadCpFile
END MODULE ReadSetParameters
END MODULE ReadSetParameters

0 comments on commit 13a78e4

Please sign in to comment.