diff --git a/src/ControllerBlocks.f90 b/src/ControllerBlocks.f90 index d9613b920..4432ea44a 100644 --- a/src/ControllerBlocks.f90 +++ b/src/ControllerBlocks.f90 @@ -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 @@ -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] @@ -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 @@ -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 @@ -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 @@ -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) @@ -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 !------------------------------------------------------------------------------------------------------------------------------- diff --git a/src/Controllers.f90 b/src/Controllers.f90 index 48ddadc78..504fed56b 100644 --- a/src/Controllers.f90 +++ b/src/Controllers.f90 @@ -31,7 +31,7 @@ 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 @@ -39,13 +39,15 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar) ! 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. @@ -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 @@ -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 @@ -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) diff --git a/src/DISCON.F90 b/src/DISCON.F90 index 7142e3473..d5119a71a 100644 --- a/src/DISCON.F90 +++ b/src/DISCON.F90 @@ -54,6 +54,7 @@ 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) !------------------------------------------------------------------------------------------------------------------------------ @@ -61,22 +62,30 @@ 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, 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 diff --git a/src/Functions.f90 b/src/Functions.f90 index 9d0c330db..697999982 100644 --- a/src/Functions.f90 +++ b/src/Functions.f90 @@ -156,15 +156,40 @@ REAL(8) FUNCTION PIIController(error, error2, kp, ki, ki2, minValue, maxValue, D END FUNCTION PIIController !------------------------------------------------------------------------------------------------------------------------------- - REAL FUNCTION interp1d(xData, yData, xq) - ! interp1d 1-D interpolation (table lookup), xData should be monotonically increasing - + REAL FUNCTION interp1d(xData, yData, xq, ErrVar) + ! interp1d 1-D interpolation (table lookup), xData should be strictly increasing + + USE ROSCO_Types, ONLY : ErrorVariables IMPLICIT NONE + ! Inputs REAL(8), DIMENSION(:), INTENT(IN) :: xData ! Provided x data (vector), to be interpolated REAL(8), DIMENSION(:), INTENT(IN) :: yData ! Provided y data (vector), to be interpolated REAL(8), INTENT(IN) :: xq ! x-value for which the y value has to be interpolated INTEGER(4) :: I ! Iteration index + + ! Error Catching + TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar + INTEGER(4) :: I_DIFF + + + ! Catch Errors + ! Are xData and yData the same size? + IF (SIZE(xData) .NE. SIZE(yData)) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'interp1d: xData and yData are not the same size' + WRITE(ErrVar%ErrMsg,"(A,I2,A,I2,A)") "interp1d: SIZE(xData) =", SIZE(xData), & + ' and SIZE(yData) =', SIZE(yData),' are not the same' + END IF + + ! Is xData non decreasing + DO I_DIFF = 1, size(xData) - 1 + IF (xData(I_DIFF + 1) - xData(I_DIFF) <= 0) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'interp1d: xData is not strictly increasing' + EXIT + END IF + END DO ! Interpolate IF (xq <= MINVAL(xData)) THEN @@ -184,7 +209,7 @@ REAL FUNCTION interp1d(xData, yData, xq) END FUNCTION interp1d !------------------------------------------------------------------------------------------------------------------------------- - REAL FUNCTION interp2d(xData, yData, zData, xq, yq) + REAL FUNCTION interp2d(xData, yData, zData, xq, yq, ErrVar) ! interp2d 2-D interpolation (table lookup). Query done using bilinear interpolation. ! Note that the interpolated matrix with associated query vectors may be different than "standard", - zData should be formatted accordingly ! - xData follows the matrix from left to right @@ -196,13 +221,17 @@ REAL FUNCTION interp2d(xData, yData, zData, xq, yq) ! 5| d e f ! 6| g H i + USE ROSCO_Types, ONLY : ErrorVariables + IMPLICIT NONE + ! Inputs - REAL(8), DIMENSION(:), INTENT(IN) :: xData ! Provided x data (vector), to find query point (should be monotonically increasing) - REAL(8), DIMENSION(:), INTENT(IN) :: yData ! Provided y data (vector), to find query point (should be monotonically increasing) + REAL(8), DIMENSION(:), INTENT(IN) :: xData ! Provided x data (vector), to find query point (should be strictly increasing) + REAL(8), DIMENSION(:), INTENT(IN) :: yData ! Provided y data (vector), to find query point (should be strictly increasing) REAL(8), DIMENSION(:,:), INTENT(IN) :: zData ! Provided z data (vector), to be interpolated REAL(8), INTENT(IN) :: xq ! x-value for which the z value has to be interpolated REAL(8), INTENT(IN) :: yq ! y-value for which the z value has to be interpolated + ! Allocate variables INTEGER(4) :: i ! Iteration index & query index, x-direction INTEGER(4) :: ii ! Iteration index & second que . ry index, x-direction @@ -210,26 +239,64 @@ REAL FUNCTION interp2d(xData, yData, zData, xq, yq) INTEGER(4) :: jj ! Iteration index & second query index, y-direction REAL(8), DIMENSION(2,2) :: fQ ! zData value at query points for bilinear interpolation REAL(8), DIMENSION(1) :: fxy ! Interpolated z-data point to be returned - REAL(8) :: fxy1 ! zData value at query point for bilinear interpolation - REAL(8) :: fxy2 ! zData value at query point for bilinear interpolation + REAL(8) :: fxy1 ! zData value at query point for bilinear interpolation + REAL(8) :: fxy2 ! zData value at query point for bilinear interpolation + LOGICAL :: edge + + ! Error Catching + TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar + INTEGER(4) :: I_DIFF + ! Error catching + ! Are xData and zData(:,1) the same size? + IF (SIZE(xData) .NE. SIZE(zData,2)) THEN + ErrVar%aviFAIL = -1 + WRITE(ErrVar%ErrMsg,"(A,I4,A,I4,A)") "interp2d: SIZE(xData) =", SIZE(xData), & + ' and SIZE(zData,1) =', SIZE(zData,2),' are not the same' + END IF + + ! Are yData and zData(1,:) the same size? + IF (SIZE(yData) .NE. SIZE(zData,1)) THEN + ErrVar%aviFAIL = -1 + WRITE(ErrVar%ErrMsg,"(A,I4,A,I4,A)") "interp2d: SIZE(yData) =", SIZE(yData), & + ' and SIZE(zData,2) =', SIZE(zData,1),' are not the same' + END IF + + ! Is xData non decreasing + DO I_DIFF = 1, size(xData) - 1 + IF (xData(I_DIFF + 1) - xData(I_DIFF) <= 0) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'interp2d: xData is not strictly increasing' + EXIT + END IF + END DO + + ! Is yData non decreasing + DO I_DIFF = 1, size(yData) - 1 + IF (yData(I_DIFF + 1) - yData(I_DIFF) <= 0) THEN + ErrVar%aviFAIL = -1 + ErrVar%ErrMsg = 'interp2d: yData is not strictly increasing' + EXIT + END IF + END DO + ! ---- Find corner indices surrounding desired interpolation point ----- ! x-direction IF (xq <= MINVAL(xData)) THEN ! On lower x-bound, just need to find zData(yq) j = 1 jj = 1 - interp2d = interp1d(yData,zData(:,j),yq) + interp2d = interp1d(yData,zData(:,j),yq,ErrVar) RETURN ELSEIF (xq >= MAXVAL(xData)) THEN ! On upper x-bound, just need to find zData(yq) j = size(xData) jj = size(xData) - interp2d = interp1d(yData,zData(:,j),yq) + interp2d = interp1d(yData,zData(:,j),yq,ErrVar) RETURN ELSE DO j = 1,size(xData) IF (xq == xData(j)) THEN ! On axis, just need 1d interpolation jj = j - interp2d = interp1d(yData,zData(:,j),yq) + interp2d = interp1d(yData,zData(:,j),yq,ErrVar) RETURN ELSEIF (xq < xData(j)) THEN jj = j @@ -244,18 +311,18 @@ REAL FUNCTION interp2d(xData, yData, zData, xq, yq) IF (yq <= MINVAL(yData)) THEN ! On lower y-bound, just need to find zData(xq) i = 1 ii = 1 - interp2d = interp1d(xData,zData(i,:),xq) + interp2d = interp1d(xData,zData(i,:),xq,ErrVar) RETURN ELSEIF (yq >= MAXVAL(yData)) THEN ! On upper y-bound, just need to find zData(xq) i = size(yData) ii = size(yData) - interp2d = interp1d(xData,zData(i,:),xq) + interp2d = interp1d(xData,zData(i,:),xq,ErrVar) RETURN ELSE DO i = 1,size(yData) IF (yq == yData(i)) THEN ! On axis, just need 1d interpolation ii = i - interp2d = interp1d(xData,zData(i,:),xq) + interp2d = interp1d(xData,zData(i,:),xq,ErrVar) RETURN ELSEIF (yq < yData(i)) THEN ii = i @@ -280,6 +347,11 @@ REAL FUNCTION interp2d(xData, yData, zData, xq, yq) interp2d = fxy(1) + ! Error catching + IF (ErrVar%aviFAIL == -1) THEN + ErrVar%ErrMsg = 'interp2:'//TRIM(ErrVar%ErrMsg) + END IF + END FUNCTION interp2d !------------------------------------------------------------------------------------------------------------------------------- FUNCTION matinv3(A) RESULT(B) @@ -413,29 +485,37 @@ REAL FUNCTION CPfunction(CP, lambda) END FUNCTION CPfunction !------------------------------------------------------------------------------------------------------------------------------- - REAL FUNCTION AeroDynTorque(LocalVar, CntrPar, PerfData) + REAL FUNCTION AeroDynTorque(LocalVar, CntrPar, PerfData, ErrVar) ! Function for computing the aerodynamic torque, divided by the effective rotor torque of the turbine, for use in wind speed estimation - USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, PerformanceData + USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, PerformanceData, ErrorVariables IMPLICIT NONE ! Inputs TYPE(ControlParameters), INTENT(IN) :: CntrPar TYPE(LocalVariables), INTENT(IN) :: LocalVar TYPE(PerformanceData), INTENT(IN) :: PerfData + TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar ! Local REAL(8) :: RotorArea REAL(8) :: Cp REAL(8) :: Lambda - + ! Find Torque RotorArea = PI*CntrPar%WE_BladeRadius**2 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%PC_PitComT*R2D, Lambda) + + ! Compute Cp + Cp = interp2d(PerfData%Beta_vec,PerfData%TSR_vec,PerfData%Cp_mat, LocalVar%PC_PitComT*R2D, Lambda, ErrVar) + AeroDynTorque = 0.5*(CntrPar%WE_RhoAir*RotorArea)*(LocalVar%WE_Vw**3/LocalVar%RotSpeedF)*Cp AeroDynTorque = MAX(AeroDynTorque, 0.0) + + ! Error Catching + IF (ErrVar%aviFAIL == -1) THEN + ErrVar%ErrMsg = 'AeroDynTorque:'//TRIM(ErrVar%ErrMsg) + END IF END FUNCTION AeroDynTorque !------------------------------------------------------------------------------------------------------------------------------- diff --git a/src/ROSCO_Types.f90 b/src/ROSCO_Types.f90 index ba2000da2..24e7729a7 100644 --- a/src/ROSCO_Types.f90 +++ b/src/ROSCO_Types.f90 @@ -19,8 +19,10 @@ MODULE ROSCO_Types ! Define Types +USE, INTRINSIC :: ISO_C_Binding IMPLICIT NONE + TYPE, PUBLIC :: ControlParameters INTEGER(4) :: LoggingLevel ! 0 = write no debug files, 1 = write standard output .dbg-file, 2 = write standard output .dbg-file and complete avrSWAP-array .dbg2-file @@ -231,4 +233,12 @@ MODULE ROSCO_Types END TYPE DebugVariables +TYPE, PUBLIC :: ErrorVariables + ! Error Catching + INTEGER(4) :: size_avcMSG + INTEGER(C_INT) :: 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(:), ALLOCATABLE :: ErrMsg ! a Fortran version of the C string argument (not considered an array here) [subtract 1 for the C null-character] + CHARACTER(999), ALLOCATABLE :: ErrMsg ! a Fortran version of the C string argument (not considered an array here) [subtract 1 for the C null-character] +END TYPE ErrorVariables + END MODULE ROSCO_Types diff --git a/src/ReadSetParameters.f90 b/src/ReadSetParameters.f90 index c4ea668fc..202ccc614 100644 --- a/src/ReadSetParameters.f90 +++ b/src/ReadSetParameters.f90 @@ -490,23 +490,28 @@ SUBROUTINE Assert(LocalVar, CntrPar, avrSWAP, aviFAIL, ErrMsg, size_avcMSG) END SUBROUTINE Assert ! ----------------------------------------------------------------------------------- ! Define parameters for control actions - SUBROUTINE SetParameters(avrSWAP, aviFAIL, accINFILE, ErrMsg, size_avcMSG, CntrPar, LocalVar, objInst, PerfData) - USE ROSCO_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances, PerformanceData + SUBROUTINE SetParameters(avrSWAP, accINFILE, size_avcMSG, CntrPar, LocalVar, objInst, PerfData, ErrVar) + + USE ROSCO_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances, PerformanceData, ErrorVariables 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 + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + TYPE(ObjectInstances), INTENT(INOUT) :: objInst + TYPE(PerformanceData), INTENT(INOUT) :: PerfData + TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar - 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. + REAL(C_FLOAT), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. 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. - CHARACTER(200) :: git_version + + INTEGER(4) :: K ! Index used for looping through blades. + CHARACTER(200) :: git_version + + ! Error Catching Variables ! Set aviFAIL to 0 in each iteration: - aviFAIL = 0 + ErrVar%aviFAIL = 0 + ! ALLOCATE(ErrVar%ErrMsg(size_avcMSG-1)) + ErrVar%size_avcMSG = size_avcMSG ! Initialize all filter instance counters at 1 objInst%instLPF = 1 @@ -534,7 +539,7 @@ SUBROUTINE SetParameters(avrSWAP, aviFAIL, accINFILE, ErrMsg, size_avcMSG, CntrP IF (LocalVar%iStatus == 0) THEN ! .TRUE. if we're on the first call to the DLL ! Inform users that we are using this user-defined routine: - aviFAIL = 1 + ErrVar%aviFAIL = 1 git_version = QueryGitVersion() ! ErrMsg = ' '//NEW_LINE('A')// & ! '------------------------------------------------------------------------------'//NEW_LINE('A')// & @@ -548,7 +553,7 @@ SUBROUTINE SetParameters(avrSWAP, aviFAIL, accINFILE, ErrMsg, size_avcMSG, CntrP ! 'Visit our GitHub-page to contribute to this project: '//NEW_LINE('A')// & ! 'https://github.com/NREL/ROSCO '//NEW_LINE('A')// & ! '------------------------------------------------------------------------------' - ErrMsg = ' '//NEW_LINE('A')// & + ErrVar%ErrMsg = ' '//NEW_LINE('A')// & '------------------------------------------------------------------------------'//NEW_LINE('A')// & 'Running ROSCO-'//TRIM(git_version)//NEW_LINE('A')// & 'A wind turbine controller framework for public use in the scientific field '//NEW_LINE('A')// & @@ -585,7 +590,7 @@ SUBROUTINE SetParameters(avrSWAP, aviFAIL, accINFILE, ErrMsg, size_avcMSG, CntrP LocalVar%VS_LastGenTrq = LocalVar%GenTq LocalVar%VS_MaxTq = CntrPar%VS_MaxTq ! Check validity of input parameters: - CALL Assert(LocalVar, CntrPar, avrSWAP, aviFAIL, ErrMsg, size_avcMSG) + CALL Assert(LocalVar, CntrPar, avrSWAP, ErrVar%aviFAIL, ErrVar%ErrMsg, size_avcMSG) ENDIF