From 337ecf68c018d697b04afcea38da82e24a201935 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 10:20:09 -0700 Subject: [PATCH 01/40] Convert WE saved variables to WE type --- ROSCO/src/ControllerBlocks.f90 | 81 ++++++++++++++++------------------ 1 file changed, 37 insertions(+), 44 deletions(-) diff --git a/ROSCO/src/ControllerBlocks.f90 b/ROSCO/src/ControllerBlocks.f90 index c065759a..aca14ae0 100644 --- a/ROSCO/src/ControllerBlocks.f90 +++ b/ROSCO/src/ControllerBlocks.f90 @@ -175,10 +175,6 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er REAL(DbKi) :: F_WECornerFreq ! Corner frequency (-3dB point) for first order low pass filter for measured hub height wind speed [Hz] ! Only used in EKF, if WE_Mode = 2 - REAL(DbKi), SAVE :: om_r ! Estimated rotor speed [rad/s] - REAL(DbKi), SAVE :: v_t ! Estimated wind speed, turbulent component [m/s] - REAL(DbKi), SAVE :: v_m ! Estimated wind speed, 10-minute averaged [m/s] - REAL(DbKi), SAVE :: v_h ! Combined estimated wind speed [m/s] REAL(DbKi) :: L ! Turbulent length scale parameter [m] REAL(DbKi) :: Ti ! Turbulent intensity, [-] ! REAL(DbKi), DIMENSION(3,3) :: I @@ -192,13 +188,10 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er ! - Covariance matrices REAL(DbKi), DIMENSION(3,3) :: F ! First order system jacobian - REAL(DbKi), DIMENSION(3,3), SAVE :: P ! Covariance estiamte REAL(DbKi), DIMENSION(1,3) :: H ! Output equation jacobian - REAL(DbKi), DIMENSION(3,1), SAVE :: xh ! Estimated state matrix REAL(DbKi), DIMENSION(3,1) :: dxh ! Estimated state matrix deviation from previous timestep REAL(DbKi), DIMENSION(3,3) :: Q ! Process noise covariance matrix REAL(DbKi), DIMENSION(1,1) :: S ! Innovation covariance - REAL(DbKi), DIMENSION(3,1), SAVE :: K ! Kalman gain matrix REAL(DbKi) :: R_m ! Measurement noise covariance [(rad/s)^2] REAL(DbKi) :: WE_Inp_Pitch @@ -256,75 +249,75 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er 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 = WE_Inp_Speed - v_t = 0.0 - v_m = LocalVar%HorWindV - v_h = LocalVar%HorWindV - lambda = WE_Inp_Speed * CntrPar%WE_BladeRadius/v_h - 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/)) + LocalVar%WE%om_r = WE_Inp_Speed + LocalVar%WE%v_t = 0.0 + LocalVar%WE%v_m = LocalVar%HorWindV + LocalVar%WE%v_h = LocalVar%HorWindV + lambda = WE_Inp_Speed * CntrPar%WE_BladeRadius/LocalVar%WE%v_h + LocalVar%WE%xh = RESHAPE((/LocalVar%WE%om_r, LocalVar%WE%v_t, LocalVar%WE%v_m/),(/3,1/)) + LocalVar%WE%P = RESHAPE((/0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 1.0/),(/3,3/)) + LocalVar%WE%K = RESHAPE((/0.0,0.0,0.0/),(/3,1/)) Cp_op = 0.25 ! initialize so debug output doesn't give ***** ELSE ! Find estimated operating Cp and system pole - A_op = interp1d(CntrPar%WE_FOPoles_v,CntrPar%WE_FOPoles,v_h,ErrVar) + A_op = interp1d(CntrPar%WE_FOPoles_v,CntrPar%WE_FOPoles,LocalVar%WE%v_h,ErrVar) ! TEST INTERP2D - lambda = max(WE_Inp_Speed, EPSILON(1.0_DbKi)) * CntrPar%WE_BladeRadius/v_h + lambda = max(WE_Inp_Speed, EPSILON(1.0_DbKi)) * CntrPar%WE_BladeRadius/LocalVar%WE%v_h Cp_op = interp2d(PerfData%Beta_vec,PerfData%TSR_vec,PerfData%Cp_mat, WE_Inp_Pitch*R2D, lambda , ErrVar) Cp_op = max(0.0,Cp_op) ! Update Jacobian F(1,1) = A_op - F(1,2) = 1.0/(2.0*CntrPar%WE_Jtot) * CntrPar%WE_RhoAir * PI *CntrPar%WE_BladeRadius**2.0 * 1/om_r * 3.0 * Cp_op * v_h**2.0 - F(1,3) = 1.0/(2.0*CntrPar%WE_Jtot) * CntrPar%WE_RhoAir * PI *CntrPar%WE_BladeRadius**2.0 * 1/om_r * 3.0 * Cp_op * v_h**2.0 - F(2,2) = - PI * v_m/(2.0*L) - F(2,3) = - PI * v_t/(2.0*L) + F(1,2) = 1.0/(2.0*CntrPar%WE_Jtot) * CntrPar%WE_RhoAir * PI *CntrPar%WE_BladeRadius**2.0 * 1/LocalVar%WE%om_r * 3.0 * Cp_op * LocalVar%WE%v_h**2.0 + F(1,3) = 1.0/(2.0*CntrPar%WE_Jtot) * CntrPar%WE_RhoAir * PI *CntrPar%WE_BladeRadius**2.0 * 1/LocalVar%WE%om_r * 3.0 * Cp_op * LocalVar%WE%v_h**2.0 + F(2,2) = - PI * LocalVar%WE%v_m/(2.0*L) + F(2,3) = - PI * LocalVar%WE%v_t/(2.0*L) ! Update process noise covariance Q(1,1) = 0.00001 - Q(2,2) =(PI * (v_m**3.0) * (Ti**2.0)) / L + Q(2,2) =(PI * (LocalVar%WE%v_m**3.0) * (Ti**2.0)) / L Q(3,3) = (2.0**2.0)/600.0 ! Prediction update Tau_r = AeroDynTorque(WE_Inp_Speed, WE_Inp_Pitch, LocalVar, CntrPar, PerfData, ErrVar) - a = PI * v_m/(2.0*L) + a = PI * LocalVar%WE%v_m/(2.0*L) dxh(1,1) = 1.0/CntrPar%WE_Jtot * (Tau_r - CntrPar%WE_GearboxRatio * WE_Inp_Torque) - dxh(2,1) = -a*v_t + dxh(2,1) = -a*LocalVar%WE%v_t dxh(3,1) = 0.0 - xh = xh + LocalVar%DT * dxh ! state update - P = P + LocalVar%DT*(MATMUL(F,P) + MATMUL(P,TRANSPOSE(F)) + Q - MATMUL(K * R_m, TRANSPOSE(K))) + LocalVar%WE%xh = LocalVar%WE%xh + LocalVar%DT * dxh ! state update + LocalVar%WE%P = LocalVar%WE%P + LocalVar%DT*(MATMUL(F,LocalVar%WE%P) + MATMUL(LocalVar%WE%P,TRANSPOSE(F)) + Q - MATMUL(LocalVar%WE%K * R_m, TRANSPOSE(LocalVar%WE%K))) ! 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*(WE_Inp_Speed - om_r) - P = MATMUL(identity(3) - MATMUL(K,H),P) + S = MATMUL(H,MATMUL(LocalVar%WE%P,TRANSPOSE(H))) + R_m ! NJA: (H*T*H') \approx 0 + LocalVar%WE%K = MATMUL(LocalVar%WE%P,TRANSPOSE(H))/S(1,1) + LocalVar%WE%xh = LocalVar%WE%xh + LocalVar%WE%K*(WE_Inp_Speed - LocalVar%WE%om_r) + LocalVar%WE%P = MATMUL(identity(3) - MATMUL(LocalVar%WE%K,H),LocalVar%WE%P) ! Wind Speed Estimate - om_r = max(xh(1,1), EPSILON(1.0_DbKi)) - v_t = xh(2,1) - v_m = xh(3,1) - v_h = v_t + v_m - LocalVar%WE_Vw = v_m + v_t - - IF (ieee_is_nan(v_h)) THEN - om_r = WE_Inp_Speed - v_t = 0.0 - v_m = LocalVar%HorWindV - v_h = LocalVar%HorWindV - LocalVar%WE_Vw = v_m + v_t + LocalVar%WE%om_r = max(LocalVar%WE%xh(1,1), EPSILON(1.0_DbKi)) + LocalVar%WE%v_t = LocalVar%WE%xh(2,1) + LocalVar%WE%v_m = LocalVar%WE%xh(3,1) + LocalVar%WE%v_h = LocalVar%WE%v_t + LocalVar%WE%v_m + LocalVar%WE_Vw = LocalVar%WE%v_m + LocalVar%WE%v_t + + IF (ieee_is_nan(LocalVar%WE%v_h)) THEN + LocalVar%WE%om_r = WE_Inp_Speed + LocalVar%WE%v_t = 0.0 + LocalVar%WE%v_m = LocalVar%HorWindV + LocalVar%WE%v_h = LocalVar%HorWindV + LocalVar%WE_Vw = LocalVar%WE%v_m + LocalVar%WE%v_t ENDIF ENDIF ! Debug Outputs DebugVar%WE_Cp = Cp_op - DebugVar%WE_Vm = v_m - DebugVar%WE_Vt = v_t + DebugVar%WE_Vm = LocalVar%WE%v_m + DebugVar%WE_Vt = LocalVar%WE%v_t DebugVar%WE_lambda = lambda ELSE ! Filter wind speed at hub height as directly passed from OpenFAST From ab4654564bb9ed059114d8770da0ebd51cb88fd2 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 10:37:06 -0700 Subject: [PATCH 02/40] Put restart flag in localvars --- ROSCO/src/ControllerBlocks.f90 | 6 +++--- ROSCO/src/Controllers.f90 | 32 +++++++++++++++---------------- ROSCO/src/Filters.f90 | 34 ++++++++++++++++----------------- ROSCO/src/ReadSetParameters.f90 | 6 ++++++ 4 files changed, 42 insertions(+), 36 deletions(-) diff --git a/ROSCO/src/ControllerBlocks.f90 b/ROSCO/src/ControllerBlocks.f90 index aca14ae0..04dc3ebb 100644 --- a/ROSCO/src/ControllerBlocks.f90 +++ b/ROSCO/src/ControllerBlocks.f90 @@ -321,7 +321,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er DebugVar%WE_lambda = lambda ELSE ! Filter wind speed at hub height as directly passed from OpenFAST - LocalVar%WE_Vw = LPFilter(LocalVar%HorWindV, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF) + LocalVar%WE_Vw = LPFilter(LocalVar%HorWindV, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ENDIF ! Add RoutineName to error message @@ -351,7 +351,7 @@ SUBROUTINE SetpointSmoother(LocalVar, CntrPar, objInst) DelOmega = ((LocalVar%PC_PitComT - LocalVar%PC_MinPit)/0.524) * CntrPar%SS_VSGain - ((CntrPar%VS_RtPwr - LocalVar%VS_LastGenPwr))/CntrPar%VS_RtPwr * 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) + LocalVar%SS_DelOmegaF = LPFilter(DelOmega, LocalVar%DT, CntrPar%F_SSCornerFreq, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ELSE LocalVar%SS_DelOmegaF = 0 ! No setpoint smoothing ENDIF @@ -404,7 +404,7 @@ REAL(DbKi) FUNCTION Shutdown(LocalVar, CntrPar, objInst) ! See if we should shutdown IF (.NOT. LocalVar%SD ) THEN ! Filter pitch signal - SD_BlPitchF = LPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%SD_CornerFreq, LocalVar%iStatus, .FALSE., objInst%instLPF) + SD_BlPitchF = LPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%SD_CornerFreq, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ! Go into shutdown if above max pit IF (SD_BlPitchF > CntrPar%SD_MaxPit) THEN diff --git a/ROSCO/src/Controllers.f90 b/ROSCO/src/Controllers.f90 index ac918868..7ceaa909 100644 --- a/ROSCO/src/Controllers.f90 +++ b/ROSCO/src/Controllers.f90 @@ -59,9 +59,9 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar, ErrVar) ! 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) + LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_FinePit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%PitCom(1), LocalVar%restart, objInst%instPI) ELSE - LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, LocalVar%PC_MinPit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%BlPitch(1), .FALSE., objInst%instPI) + LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, LocalVar%PC_MinPit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%BlPitch(1), LocalVar%restart, objInst%instPI) END IF DebugVar%PC_PICommand = LocalVar%PC_PitComT ! Find individual pitch control contribution @@ -171,14 +171,14 @@ SUBROUTINE VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar) END IF ! PI controller - LocalVar%GenTq = PIController(LocalVar%VS_SpdErr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, LocalVar%VS_MaxTq, LocalVar%DT, LocalVar%VS_LastGenTrq, .FALSE., objInst%instPI) + LocalVar%GenTq = PIController(LocalVar%VS_SpdErr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, LocalVar%VS_MaxTq, LocalVar%DT, LocalVar%VS_LastGenTrq, LocalVar%restart, objInst%instPI) LocalVar%GenTq = saturate(LocalVar%GenTq, CntrPar%VS_MinTq, LocalVar%VS_MaxTq) ! 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_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) + 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, LocalVar%restart, 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, LocalVar%restart, objInst%instPI) ! The action IF (LocalVar%VS_State == 1) THEN ! Region 1.5 @@ -315,8 +315,8 @@ SUBROUTINE IPC(CntrPar, LocalVar, objInst) ! High-pass filter the MBC yaw component and filter yaw alignment error, and compute the yaw-by-IPC contribution IF (CntrPar%Y_ControlMode == 2) THEN - Y_MErrF = SecLPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_IPC_omegaLP, CntrPar%Y_IPC_zetaLP, LocalVar%iStatus, .FALSE., objInst%instSecLPF) - Y_MErrF_IPC = PIController(Y_MErrF, CntrPar%Y_IPC_KP(1), CntrPar%Y_IPC_KI(1), -CntrPar%Y_IPC_IntSat, CntrPar%Y_IPC_IntSat, LocalVar%DT, 0.0_DbKi, .FALSE., objInst%instPI) + Y_MErrF = SecLPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_IPC_omegaLP, CntrPar%Y_IPC_zetaLP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) + Y_MErrF_IPC = PIController(Y_MErrF, CntrPar%Y_IPC_KP(1), CntrPar%Y_IPC_KI(1), -CntrPar%Y_IPC_IntSat, CntrPar%Y_IPC_IntSat, LocalVar%DT, 0.0_DbKi, LocalVar%restart, objInst%instPI) ELSE axisYawF_1P = axisYaw_1P Y_MErrF = 0.0 @@ -356,7 +356,7 @@ SUBROUTINE IPC(CntrPar, LocalVar, objInst) ! Optionally filter the resulting signal to induce a phase delay IF (CntrPar%IPC_CornerFreqAct > 0.0) THEN - PitComIPCF(K) = LPFilter(PitComIPC(K), LocalVar%DT, CntrPar%IPC_CornerFreqAct, LocalVar%iStatus, .FALSE., objInst%instLPF) + PitComIPCF(K) = LPFilter(PitComIPC(K), LocalVar%DT, CntrPar%IPC_CornerFreqAct, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ELSE PitComIPCF(K) = PitComIPC(K) END IF @@ -378,7 +378,7 @@ SUBROUTINE ForeAftDamping(CntrPar, LocalVar, objInst) TYPE(ObjectInstances), INTENT(INOUT) :: objInst ! Body - LocalVar%FA_AccHPFI = PIController(LocalVar%FA_AccHPF, 0.0_DbKi, CntrPar%FA_KI, -CntrPar%FA_IntSat, CntrPar%FA_IntSat, LocalVar%DT, 0.0_DbKi, .FALSE., objInst%instPI) + LocalVar%FA_AccHPFI = PIController(LocalVar%FA_AccHPF, 0.0_DbKi, CntrPar%FA_KI, -CntrPar%FA_IntSat, CntrPar%FA_IntSat, LocalVar%DT, 0.0_DbKi, LocalVar%restart, objInst%instPI) ! Store the fore-aft pitch contribution to LocalVar data type DO K = 1,LocalVar%NumBl @@ -403,8 +403,8 @@ REAL(DbKi) FUNCTION FloatingFeedback(LocalVar, CntrPar, objInst) REAL(DbKi) :: NacIMU_FA_vel ! Tower fore-aft pitching velocity [rad/s] ! Calculate floating contribution to pitch command - FA_vel = PIController(LocalVar%FA_AccF, 0.0_DbKi, 1.0_DbKi, -100.0_DbKi , 100.0_DbKi ,LocalVar%DT, 0.0_DbKi, .FALSE., objInst%instPI) ! NJA: should never reach saturation limits.... - NacIMU_FA_vel = PIController(LocalVar%NacIMU_FA_AccF, 0.0_DbKi, 1.0_DbKi, -100.0_DbKi , 100.0_DbKi ,LocalVar%DT, 0.0_DbKi, .FALSE., objInst%instPI) ! NJA: should never reach saturation limits.... + FA_vel = PIController(LocalVar%FA_AccF, 0.0_DbKi, 1.0_DbKi, -100.0_DbKi , 100.0_DbKi ,LocalVar%DT, 0.0_DbKi, LocalVar%restart, objInst%instPI) ! NJA: should never reach saturation limits.... + NacIMU_FA_vel = PIController(LocalVar%NacIMU_FA_AccF, 0.0_DbKi, 1.0_DbKi, -100.0_DbKi , 100.0_DbKi ,LocalVar%DT, 0.0_DbKi, LocalVar%restart, objInst%instPI) ! NJA: should never reach saturation limits.... if (CntrPar%Fl_Mode == 1) THEN FloatingFeedback = (0.0_DbKi - FA_vel) * CntrPar%Fl_Kp !* LocalVar%PC_KP/maxval(CntrPar%PC_GS_KP) ELSEIF (CntrPar%Fl_Mode == 2) THEN @@ -443,9 +443,9 @@ SUBROUTINE FlapControl(avrSWAP, CntrPar, LocalVar, objInst) LocalVar%Flp_Angle(2) = CntrPar%Flp_Angle LocalVar%Flp_Angle(3) = CntrPar%Flp_Angle ! Initialize filter - RootMOOP_F(1) = SecLPFilter(LocalVar%rootMOOP(1),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, .FALSE.,objInst%instSecLPF) - RootMOOP_F(2) = SecLPFilter(LocalVar%rootMOOP(2),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, .FALSE.,objInst%instSecLPF) - RootMOOP_F(3) = SecLPFilter(LocalVar%rootMOOP(3),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, .FALSE.,objInst%instSecLPF) + RootMOOP_F(1) = SecLPFilter(LocalVar%rootMOOP(1),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) + RootMOOP_F(2) = SecLPFilter(LocalVar%rootMOOP(2),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) + RootMOOP_F(3) = SecLPFilter(LocalVar%rootMOOP(3),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) ! Initialize controller IF (CntrPar%Flp_Mode == 2) THEN LocalVar%Flp_Angle(K) = PIIController(RootMyb_VelErr(K), 0 - LocalVar%Flp_Angle(K), CntrPar%Flp_Kp, CntrPar%Flp_Ki, 0.05, -CntrPar%Flp_MaxPit , CntrPar%Flp_MaxPit , LocalVar%DT, 0.0, .TRUE., objInst%instPI) @@ -461,14 +461,14 @@ SUBROUTINE FlapControl(avrSWAP, CntrPar, LocalVar, objInst) ELSEIF (CntrPar%Flp_Mode == 2) THEN DO K = 1,LocalVar%NumBl ! LPF Blade root bending moment - RootMOOP_F(K) = SecLPFilter(LocalVar%rootMOOP(K),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, .FALSE.,objInst%instSecLPF) + RootMOOP_F(K) = SecLPFilter(LocalVar%rootMOOP(K),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) ! Find derivative and derivative error of blade root bending moment RootMyb_Vel(K) = (RootMOOP_F(K) - RootMyb_Last(K))/LocalVar%DT RootMyb_VelErr(K) = 0 - RootMyb_Vel(K) ! Find flap angle command - includes an integral term to encourage zero flap angle - LocalVar%Flp_Angle(K) = PIIController(RootMyb_VelErr(K), 0 - LocalVar%Flp_Angle(K), CntrPar%Flp_Kp, CntrPar%Flp_Ki, REAL(0.05,DbKi), -CntrPar%Flp_MaxPit , CntrPar%Flp_MaxPit , LocalVar%DT, 0.0, .FALSE., objInst%instPI) + LocalVar%Flp_Angle(K) = PIIController(RootMyb_VelErr(K), 0 - LocalVar%Flp_Angle(K), CntrPar%Flp_Kp, CntrPar%Flp_Ki, REAL(0.05,DbKi), -CntrPar%Flp_MaxPit , CntrPar%Flp_MaxPit , LocalVar%DT, 0.0, LocalVar%restart, objInst%instPI) ! Saturation Limits LocalVar%Flp_Angle(K) = saturate(LocalVar%Flp_Angle(K), -CntrPar%Flp_MaxPit, CntrPar%Flp_MaxPit) diff --git a/ROSCO/src/Filters.f90 b/ROSCO/src/Filters.f90 index 7cfc43e5..0589c7b5 100644 --- a/ROSCO/src/Filters.f90 +++ b/ROSCO/src/Filters.f90 @@ -267,11 +267,11 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst, ErrVar) ! 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) + LocalVar%GenSpeedF = LPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) + LocalVar%RotSpeedF = LPFilter(LocalVar%RotSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, LocalVar%iStatus, LocalVar%restart, 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 + LocalVar%GenSpeedF = SecLPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, CntrPar%F_LPFDamping, LocalVar%iStatus, LocalVar%restart, 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, LocalVar%restart, objInst%instSecLPF) ! Second-order low-pass filter on generator speed ELSE IF (LocalVar%iStatus ==0) THEN print *, 'No generator speed low-pass filter is selected in ROSCO (F_LPFType=0)' @@ -281,37 +281,37 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst, ErrVar) ENDIF ! Apply Notch Fitler IF (CntrPar%F_NotchType == 1 .OR. CntrPar%F_NotchType == 3) THEN - LocalVar%GenSpeedF = NotchFilter(LocalVar%GenSpeedF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%iStatus, .FALSE., objInst%instNotch) + LocalVar%GenSpeedF = NotchFilter(LocalVar%GenSpeedF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%iStatus, LocalVar%restart, objInst%instNotch) ENDIF ! Filtering the tower fore-aft acceleration signal IF (CntrPar%Fl_Mode > 0) THEN ! Force to start at 0 IF (LocalVar%iStatus == 0) THEN - LocalVar%NacIMU_FA_AccF = SecLPFilter(REAL(0.,DbKi), LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! Fixed Damping - LocalVar%FA_AccF = SecLPFilter(REAL(0.,DbKi), LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! Fixed Damping + LocalVar%NacIMU_FA_AccF = SecLPFilter(REAL(0.,DbKi), LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping + LocalVar%FA_AccF = SecLPFilter(REAL(0.,DbKi), LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping ELSE - LocalVar%NacIMU_FA_AccF = SecLPFilter(LocalVar%NacIMU_FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! Fixed Damping - LocalVar%FA_AccF = SecLPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, .FALSE., objInst%instSecLPF) ! Fixed Damping + LocalVar%NacIMU_FA_AccF = SecLPFilter(LocalVar%NacIMU_FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping + LocalVar%FA_AccF = SecLPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping ENDIF - LocalVar%NacIMU_FA_AccF = HPFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, CntrPar%F_FlHighPassFreq, LocalVar%iStatus, .FALSE., objInst%instHPF) - LocalVar%FA_AccF = HPFilter(LocalVar%FA_AccF, LocalVar%DT, CntrPar%F_FlHighPassFreq, LocalVar%iStatus, .FALSE., objInst%instHPF) + LocalVar%NacIMU_FA_AccF = HPFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, CntrPar%F_FlHighPassFreq, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) + LocalVar%FA_AccF = HPFilter(LocalVar%FA_AccF, LocalVar%DT, CntrPar%F_FlHighPassFreq, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) IF (CntrPar%F_NotchType >= 2) THEN - LocalVar%NACIMU_FA_AccF = NotchFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%iStatus, .FALSE., objInst%instNotch) ! Fixed Damping - LocalVar%FA_AccF = NotchFilter(LocalVar%FA_AccF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%iStatus, .FALSE., objInst%instNotch) ! Fixed Damping + LocalVar%NACIMU_FA_AccF = NotchFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%iStatus, LocalVar%restart, objInst%instNotch) ! Fixed Damping + LocalVar%FA_AccF = NotchFilter(LocalVar%FA_AccF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%iStatus, LocalVar%restart, objInst%instNotch) ! Fixed Damping ENDIF ENDIF - LocalVar%FA_AccHPF = HPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%FA_HPFCornerFreq, LocalVar%iStatus, .FALSE., objInst%instHPF) + LocalVar%FA_AccHPF = HPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%FA_HPFCornerFreq, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) ! Filter Wind Speed Estimator Signal - LocalVar%We_Vw_F = LPFilter(LocalVar%WE_Vw, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%iStatus,.FALSE.,objInst%instLPF) ! 30 second time constant + LocalVar%We_Vw_F = LPFilter(LocalVar%WE_Vw, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%iStatus,LocalVar%restart,objInst%instLPF) ! 30 second time constant ! Control commands (used by WSE, mostly) - LocalVar%VS_LastGenTrqF = SecLPFilter(LocalVar%VS_LastGenTrq, LocalVar%DT, CntrPar%F_LPFCornerFreq, 0.7_DbKi, LocalVar%iStatus, .FALSE., objInst%instSecLPF) - LocalVar%PC_PitComTF = SecLPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%F_LPFCornerFreq*0.25, 0.7_DbKi, LocalVar%iStatus, .FALSE., objInst%instSecLPF) + LocalVar%VS_LastGenTrqF = SecLPFilter(LocalVar%VS_LastGenTrq, LocalVar%DT, CntrPar%F_LPFCornerFreq, 0.7_DbKi, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) + LocalVar%PC_PitComTF = SecLPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%F_LPFCornerFreq*0.25, 0.7_DbKi, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) END SUBROUTINE PreFilterMeasuredSignals END MODULE Filters diff --git a/ROSCO/src/ReadSetParameters.f90 b/ROSCO/src/ReadSetParameters.f90 index 846bfc8d..1b03967a 100644 --- a/ROSCO/src/ReadSetParameters.f90 +++ b/ROSCO/src/ReadSetParameters.f90 @@ -77,6 +77,12 @@ SUBROUTINE ReadAvrSWAP(avrSWAP, LocalVar) LocalVar%BlPitch(3) = LocalVar%PitCom(3) ENDIF + IF (LocalVar%iStatus == 0) THEN + LocalVar%restart = .True. + ELSE + LocalVar%restart = .False. + ENDIF + END SUBROUTINE ReadAvrSWAP ! ----------------------------------------------------------------------------------- ! Define parameters for control actions From 9d675478704b9d7318910621f1a16014bf9958a1 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 10:48:00 -0700 Subject: [PATCH 03/40] Use saved filter params from LocalVar --- ROSCO/src/ControllerBlocks.f90 | 6 +- ROSCO/src/Controllers.f90 | 20 +-- ROSCO/src/Filters.f90 | 288 ++++++++++++++------------------- 3 files changed, 136 insertions(+), 178 deletions(-) diff --git a/ROSCO/src/ControllerBlocks.f90 b/ROSCO/src/ControllerBlocks.f90 index 04dc3ebb..b8031706 100644 --- a/ROSCO/src/ControllerBlocks.f90 +++ b/ROSCO/src/ControllerBlocks.f90 @@ -321,7 +321,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er DebugVar%WE_lambda = lambda ELSE ! Filter wind speed at hub height as directly passed from OpenFAST - LocalVar%WE_Vw = LPFilter(LocalVar%HorWindV, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) + LocalVar%WE_Vw = LPFilter(LocalVar%HorWindV, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ENDIF ! Add RoutineName to error message @@ -351,7 +351,7 @@ SUBROUTINE SetpointSmoother(LocalVar, CntrPar, objInst) DelOmega = ((LocalVar%PC_PitComT - LocalVar%PC_MinPit)/0.524) * CntrPar%SS_VSGain - ((CntrPar%VS_RtPwr - LocalVar%VS_LastGenPwr))/CntrPar%VS_RtPwr * 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, LocalVar%restart, objInst%instLPF) + LocalVar%SS_DelOmegaF = LPFilter(DelOmega, LocalVar%DT, CntrPar%F_SSCornerFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ELSE LocalVar%SS_DelOmegaF = 0 ! No setpoint smoothing ENDIF @@ -404,7 +404,7 @@ REAL(DbKi) FUNCTION Shutdown(LocalVar, CntrPar, objInst) ! See if we should shutdown IF (.NOT. LocalVar%SD ) THEN ! Filter pitch signal - SD_BlPitchF = LPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%SD_CornerFreq, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) + SD_BlPitchF = LPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%SD_CornerFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ! Go into shutdown if above max pit IF (SD_BlPitchF > CntrPar%SD_MaxPit) THEN diff --git a/ROSCO/src/Controllers.f90 b/ROSCO/src/Controllers.f90 index 7ceaa909..73b0f951 100644 --- a/ROSCO/src/Controllers.f90 +++ b/ROSCO/src/Controllers.f90 @@ -248,8 +248,8 @@ SUBROUTINE YawRateControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar) IF (LocalVar%Time >= LocalVar%Y_YawEndT) THEN ! Check if the turbine is currently yawing avrSWAP(48) = 0.0 ! Set yaw rate to zero - LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%iStatus, .FALSE., objInst%instLPF) ! Fast low pass filtered yaw error with a frequency of 1 - LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%iStatus, .FALSE., objInst%instLPF) ! Slow low pass filtered yaw error with a frequency of 1/60 + LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%FP, LocalVar%iStatus, .FALSE., objInst%instLPF) ! Fast low pass filtered yaw error with a frequency of 1 + LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%FP, LocalVar%iStatus, .FALSE., objInst%instLPF) ! Slow low pass filtered yaw error with a frequency of 1/60 LocalVar%Y_AccErr = LocalVar%Y_AccErr + LocalVar%DT*SIGN(LocalVar%Y_ErrLPFFast**2, LocalVar%Y_ErrLPFFast) ! Integral of the fast low pass filtered yaw error @@ -258,8 +258,8 @@ SUBROUTINE YawRateControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar) END IF ELSE avrSWAP(48) = SIGN(CntrPar%Y_Rate, LocalVar%Y_MErr) ! Set yaw rate to predefined yaw rate, the sign of the error is copied to the rate - LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%iStatus, .TRUE., objInst%instLPF) ! Fast low pass filtered yaw error with a frequency of 1 - LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%iStatus, .TRUE., objInst%instLPF) ! Slow low pass filtered yaw error with a frequency of 1/60 + LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%FP, LocalVar%iStatus, .TRUE., objInst%instLPF) ! Fast low pass filtered yaw error with a frequency of 1 + LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%FP, LocalVar%iStatus, .TRUE., objInst%instLPF) ! Slow low pass filtered yaw error with a frequency of 1/60 LocalVar%Y_AccErr = 0.0 ! " END IF END IF @@ -315,7 +315,7 @@ SUBROUTINE IPC(CntrPar, LocalVar, objInst) ! High-pass filter the MBC yaw component and filter yaw alignment error, and compute the yaw-by-IPC contribution IF (CntrPar%Y_ControlMode == 2) THEN - Y_MErrF = SecLPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_IPC_omegaLP, CntrPar%Y_IPC_zetaLP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) + Y_MErrF = SecLPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_IPC_omegaLP, CntrPar%Y_IPC_zetaLP, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) Y_MErrF_IPC = PIController(Y_MErrF, CntrPar%Y_IPC_KP(1), CntrPar%Y_IPC_KI(1), -CntrPar%Y_IPC_IntSat, CntrPar%Y_IPC_IntSat, LocalVar%DT, 0.0_DbKi, LocalVar%restart, objInst%instPI) ELSE axisYawF_1P = axisYaw_1P @@ -356,7 +356,7 @@ SUBROUTINE IPC(CntrPar, LocalVar, objInst) ! Optionally filter the resulting signal to induce a phase delay IF (CntrPar%IPC_CornerFreqAct > 0.0) THEN - PitComIPCF(K) = LPFilter(PitComIPC(K), LocalVar%DT, CntrPar%IPC_CornerFreqAct, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) + PitComIPCF(K) = LPFilter(PitComIPC(K), LocalVar%DT, CntrPar%IPC_CornerFreqAct, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ELSE PitComIPCF(K) = PitComIPC(K) END IF @@ -443,9 +443,9 @@ SUBROUTINE FlapControl(avrSWAP, CntrPar, LocalVar, objInst) LocalVar%Flp_Angle(2) = CntrPar%Flp_Angle LocalVar%Flp_Angle(3) = CntrPar%Flp_Angle ! Initialize filter - RootMOOP_F(1) = SecLPFilter(LocalVar%rootMOOP(1),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) - RootMOOP_F(2) = SecLPFilter(LocalVar%rootMOOP(2),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) - RootMOOP_F(3) = SecLPFilter(LocalVar%rootMOOP(3),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) + RootMOOP_F(1) = SecLPFilter(LocalVar%rootMOOP(1),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) + RootMOOP_F(2) = SecLPFilter(LocalVar%rootMOOP(2),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) + RootMOOP_F(3) = SecLPFilter(LocalVar%rootMOOP(3),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) ! Initialize controller IF (CntrPar%Flp_Mode == 2) THEN LocalVar%Flp_Angle(K) = PIIController(RootMyb_VelErr(K), 0 - LocalVar%Flp_Angle(K), CntrPar%Flp_Kp, CntrPar%Flp_Ki, 0.05, -CntrPar%Flp_MaxPit , CntrPar%Flp_MaxPit , LocalVar%DT, 0.0, .TRUE., objInst%instPI) @@ -461,7 +461,7 @@ SUBROUTINE FlapControl(avrSWAP, CntrPar, LocalVar, objInst) ELSEIF (CntrPar%Flp_Mode == 2) THEN DO K = 1,LocalVar%NumBl ! LPF Blade root bending moment - RootMOOP_F(K) = SecLPFilter(LocalVar%rootMOOP(K),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) + RootMOOP_F(K) = SecLPFilter(LocalVar%rootMOOP(K),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) ! Find derivative and derivative error of blade root bending moment RootMyb_Vel(K) = (RootMOOP_F(K) - RootMyb_Last(K))/LocalVar%DT diff --git a/ROSCO/src/Filters.f90 b/ROSCO/src/Filters.f90 index 0589c7b5..1987906f 100644 --- a/ROSCO/src/Filters.f90 +++ b/ROSCO/src/Filters.f90 @@ -26,225 +26,195 @@ MODULE Filters CONTAINS !------------------------------------------------------------------------------------------------------------------------------- - REAL(DbKi) FUNCTION LPFilter(InputSignal, DT, CornerFreq, iStatus, reset, inst) + REAL FUNCTION LPFilter(InputSignal, DT, CornerFreq, FP, iStatus, reset, inst) ! Discrete time Low-Pass Filter of the form: ! Continuous Time Form: H(s) = CornerFreq/(1 + CornerFreq) ! Discrete Time Form: H(z) = (b1z + b0) / (a1*z + a0) ! - REAL(DbKi), INTENT(IN) :: InputSignal - REAL(DbKi), INTENT(IN) :: DT ! time step [s] - REAL(DbKi), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] - INTEGER(IntKi), INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. - INTEGER(IntKi), INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. + USE ROSCO_Types, ONLY : FilterParameters + TYPE(FilterParameters), INTENT(INOUT) :: FP + + REAL(8), INTENT(IN) :: InputSignal + REAL(8), INTENT(IN) :: DT ! time step [s] + REAL(8), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] + INTEGER(4), INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. + INTEGER(4), INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. LOGICAL(4), INTENT(IN) :: reset ! Reset the filter to the input signal - ! Local - REAL(DbKi), DIMENSION(99), SAVE :: a1 ! Denominator coefficient 1 - REAL(DbKi), DIMENSION(99), SAVE :: a0 ! Denominator coefficient 0 - REAL(DbKi), DIMENSION(99), SAVE :: b1 ! Numerator coefficient 1 - REAL(DbKi), DIMENSION(99), SAVE :: b0 ! Numerator coefficient 0 - - REAL(DbKi), DIMENSION(99), SAVE :: InputSignalLast ! Input signal the last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: OutputSignalLast ! Output signal the last time this filter was called. Supports 99 separate instances. - ! Initialization IF ((iStatus == 0) .OR. reset) THEN - OutputSignalLast(inst) = InputSignal - InputSignalLast(inst) = InputSignal - a1(inst) = 2 + CornerFreq*DT - a0(inst) = CornerFreq*DT - 2 - b1(inst) = CornerFreq*DT - b0(inst) = CornerFreq*DT + FP%lpf1_OutputSignalLast(inst) = InputSignal + FP%lpf1_InputSignalLast(inst) = InputSignal + FP%lpf1_a1(inst) = 2 + CornerFreq*DT + FP%lpf1_a0(inst) = CornerFreq*DT - 2 + FP%lpf1_b1(inst) = CornerFreq*DT + FP%lpf1_b0(inst) = CornerFreq*DT ENDIF ! Define coefficients ! Filter - LPFilter = 1.0/a1(inst) * (-a0(inst)*OutputSignalLast(inst) + b1(inst)*InputSignal + b0(inst)*InputSignalLast(inst)) + LPFilter = 1.0/FP%lpf1_a1(inst) * (-FP%lpf1_a0(inst)*FP%lpf1_OutputSignalLast(inst) + FP%lpf1_b1(inst)*InputSignal + FP%lpf1_b0(inst)*FP%lpf1_InputSignalLast(inst)) ! Save signals for next time step - InputSignalLast(inst) = InputSignal - OutputSignalLast(inst) = LPFilter + FP%lpf1_InputSignalLast(inst) = InputSignal + FP%lpf1_OutputSignalLast(inst) = LPFilter inst = inst + 1 END FUNCTION LPFilter !------------------------------------------------------------------------------------------------------------------------------- - REAL(DbKi) FUNCTION SecLPFilter(InputSignal, DT, CornerFreq, Damp, iStatus, reset, inst) + REAL FUNCTION SecLPFilter(InputSignal, DT, CornerFreq, Damp, FP, iStatus, reset, inst) ! Discrete time Low-Pass Filter of the form: ! Continuous Time Form: H(s) = CornerFreq^2/(s^2 + 2*CornerFreq*Damp*s + CornerFreq^2) ! Discrete Time From: H(z) = (b2*z^2 + b1*z + b0) / (a2*z^2 + a1*z + a0) - REAL(DbKi), INTENT(IN) :: InputSignal - REAL(DbKi), INTENT(IN) :: DT ! time step [s] - REAL(DbKi), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] - REAL(DbKi), INTENT(IN) :: Damp ! Dampening constant - INTEGER(IntKi), INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. - INTEGER(IntKi), INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. + USE ROSCO_Types, ONLY : FilterParameters + TYPE(FilterParameters), INTENT(INOUT) :: FP + REAL(8), INTENT(IN) :: InputSignal + REAL(8), INTENT(IN) :: DT ! time step [s] + REAL(8), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] + REAL(8), INTENT(IN) :: Damp ! Dampening constant + INTEGER(4), INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. + INTEGER(4), INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. LOGICAL(4), INTENT(IN) :: reset ! Reset the filter to the input signal - ! Local - REAL(DbKi), DIMENSION(99), SAVE :: a2 ! Denominator coefficient 2 - REAL(DbKi), DIMENSION(99), SAVE :: a1 ! Denominator coefficient 1 - REAL(DbKi), DIMENSION(99), SAVE :: a0 ! Denominator coefficient 0 - REAL(DbKi), DIMENSION(99), SAVE :: b2 ! Numerator coefficient 2 - REAL(DbKi), DIMENSION(99), SAVE :: b1 ! Numerator coefficient 1 - REAL(DbKi), DIMENSION(99), SAVE :: b0 ! Numerator coefficient 0 - REAL(DbKi), DIMENSION(99), SAVE :: InputSignalLast1 ! Input signal the last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: InputSignalLast2 ! Input signal the next to last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: OutputSignalLast1 ! Output signal the last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: OutputSignalLast2 ! Output signal the next to last time this filter was called. Supports 99 separate instances. - ! Initialization IF ((iStatus == 0) .OR. reset ) THEN - OutputSignalLast1(inst) = InputSignal - OutputSignalLast2(inst) = InputSignal - InputSignalLast1(inst) = InputSignal - InputSignalLast2(inst) = InputSignal + FP%lpf2_OutputSignalLast1(inst) = InputSignal + FP%lpf2_OutputSignalLast2(inst) = InputSignal + FP%lpf2_InputSignalLast1(inst) = InputSignal + FP%lpf2_InputSignalLast2(inst) = InputSignal ! Coefficients - a2(inst) = DT**2.0*CornerFreq**2.0 + 4.0 + 4.0*Damp*CornerFreq*DT - a1(inst) = 2.0*DT**2.0*CornerFreq**2.0 - 8.0 - a0(inst) = DT**2.0*CornerFreq**2.0 + 4.0 - 4.0*Damp*CornerFreq*DT - b2(inst) = DT**2.0*CornerFreq**2.0 - b1(inst) = 2.0*DT**2.0*CornerFreq**2.0 - b0(inst) = DT**2.0*CornerFreq**2.0 + FP%lpf2_a2(inst) = DT**2.0*CornerFreq**2.0 + 4.0 + 4.0*Damp*CornerFreq*DT + FP%lpf2_a1(inst) = 2.0*DT**2.0*CornerFreq**2.0 - 8.0 + FP%lpf2_a0(inst) = DT**2.0*CornerFreq**2.0 + 4.0 - 4.0*Damp*CornerFreq*DT + FP%lpf2_b2(inst) = DT**2.0*CornerFreq**2.0 + FP%lpf2_b1(inst) = 2.0*DT**2.0*CornerFreq**2.0 + FP%lpf2_b0(inst) = DT**2.0*CornerFreq**2.0 ENDIF ! Filter - SecLPFilter = 1.0/a2(inst) * (b2(inst)*InputSignal + b1(inst)*InputSignalLast1(inst) + b0(inst)*InputSignalLast2(inst) - a1(inst)*OutputSignalLast1(inst) - a0(inst)*OutputSignalLast2(inst)) - - ! SecLPFilter = 1/(4+4*DT*Damp*CornerFreq+DT**2*CornerFreq**2) * ( (8-2*DT**2*CornerFreq**2)*OutputSignalLast1(inst) & - ! + (-4+4*DT*Damp*CornerFreq-DT**2*CornerFreq**2)*OutputSignalLast2(inst) + (DT**2*CornerFreq**2)*InputSignal & - ! + (2*DT**2*CornerFreq**2)*InputSignalLast1(inst) + (DT**2*CornerFreq**2)*InputSignalLast2(inst) ) + SecLPFilter = 1.0/FP%lpf2_a2(inst) * (FP%lpf2_b2(inst)*InputSignal + FP%lpf2_b1(inst)*FP%lpf2_InputSignalLast1(inst) + FP%lpf2_b0(inst)*FP%lpf2_InputSignalLast2(inst) - FP%lpf2_a1(inst)*FP%lpf2_OutputSignalLast1(inst) - FP%lpf2_a0(inst)*FP%lpf2_OutputSignalLast2(inst)) ! Save signals for next time step - InputSignalLast2(inst) = InputSignalLast1(inst) - InputSignalLast1(inst) = InputSignal - OutputSignalLast2(inst) = OutputSignalLast1(inst) - OutputSignalLast1(inst) = SecLPFilter + FP%lpf2_InputSignalLast2(inst) = FP%lpf2_InputSignalLast1(inst) + FP%lpf2_InputSignalLast1(inst) = InputSignal + FP%lpf2_OutputSignalLast2(inst) = FP%lpf2_OutputSignalLast1(inst) + FP%lpf2_OutputSignalLast1(inst) = SecLPFilter inst = inst + 1 END FUNCTION SecLPFilter !------------------------------------------------------------------------------------------------------------------------------- - REAL(DbKi) FUNCTION HPFilter( InputSignal, DT, CornerFreq, iStatus, reset, inst) + REAL FUNCTION HPFilter( InputSignal, DT, CornerFreq, FP, iStatus, reset, inst) ! Discrete time High-Pass Filter + USE ROSCO_Types, ONLY : FilterParameters + TYPE(FilterParameters), INTENT(INOUT) :: FP - REAL(DbKi), INTENT(IN) :: InputSignal - REAL(DbKi), INTENT(IN) :: DT ! time step [s] - REAL(DbKi), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] + REAL(8), INTENT(IN) :: InputSignal + REAL(8), INTENT(IN) :: DT ! time step [s] + REAL(8), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] INTEGER, INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. INTEGER, INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. LOGICAL(4), INTENT(IN) :: reset ! Reset the filter to the input signal ! Local - REAL(DbKi) :: K ! Constant gain - REAL(DbKi), DIMENSION(99), SAVE :: InputSignalLast ! Input signal the last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: OutputSignalLast ! Output signal the last time this filter was called. Supports 99 separate instances. + REAL(8) :: K ! Constant gain ! Initialization IF ((iStatus == 0) .OR. reset) THEN - OutputSignalLast(inst) = InputSignal - InputSignalLast(inst) = InputSignal + FP%hpf_OutputSignalLast(inst) = InputSignal + FP%hpf_InputSignalLast(inst) = InputSignal ENDIF K = 2.0 / DT ! Body - HPFilter = K/(CornerFreq + K)*InputSignal - K/(CornerFreq + K)*InputSignalLast(inst) - (CornerFreq - K)/(CornerFreq + K)*OutputSignalLast(inst) + HPFilter = K/(CornerFreq + K)*InputSignal - K/(CornerFreq + K)*FP%hpf_InputSignalLast(inst) - (CornerFreq - K)/(CornerFreq + K)*FP%hpf_OutputSignalLast(inst) ! Save signals for next time step - InputSignalLast(inst) = InputSignal - OutputSignalLast(inst) = HPFilter + FP%HPF_InputSignalLast(inst) = InputSignal + FP%HPF_OutputSignalLast(inst) = HPFilter inst = inst + 1 END FUNCTION HPFilter !------------------------------------------------------------------------------------------------------------------------------- - REAL(DbKi) FUNCTION NotchFilterSlopes(InputSignal, DT, CornerFreq, Damp, iStatus, reset, inst) + REAL FUNCTION NotchFilterSlopes(InputSignal, DT, CornerFreq, Damp, FP, iStatus, reset, inst) ! Discrete time inverted Notch Filter with descending slopes, G = CornerFreq*s/(Damp*s^2+CornerFreq*s+Damp*CornerFreq^2) + USE ROSCO_Types, ONLY : FilterParameters + TYPE(FilterParameters), INTENT(INOUT) :: FP - REAL(DbKi), INTENT(IN) :: InputSignal - REAL(DbKi), INTENT(IN) :: DT ! time step [s] - REAL(DbKi), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] - REAL(DbKi), INTENT(IN) :: Damp ! Dampening constant + REAL(8), INTENT(IN) :: InputSignal + REAL(8), INTENT(IN) :: DT ! time step [s] + REAL(8), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] + REAL(8), INTENT(IN) :: Damp ! Dampening constant INTEGER, INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. INTEGER, INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. LOGICAL(4), INTENT(IN) :: reset ! Reset the filter to the input signal - ! Local - REAL(DbKi), DIMENSION(99), SAVE :: b2, b0, a2, a1, a0 ! Input signal the last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: InputSignalLast1 ! Input signal the last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: InputSignalLast2 ! Input signal the next to last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: OutputSignalLast1 ! Output signal the last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: OutputSignalLast2 ! Output signal the next to last time this filter was called. Supports 99 separate instances. - + ! Initialization IF ((iStatus == 0) .OR. reset) THEN - OutputSignalLast1(inst) = InputSignal - OutputSignalLast2(inst) = InputSignal - InputSignalLast1(inst) = InputSignal - InputSignalLast2(inst) = InputSignal - b2(inst) = 2.0 * DT * CornerFreq - b0(inst) = -b2(inst) - a2(inst) = Damp*DT**2.0*CornerFreq**2.0 + 2.0*DT*CornerFreq + 4.0*Damp - a1(inst) = 2.0*Damp*DT**2.0*CornerFreq**2.0 - 8.0*Damp - a0(inst) = Damp*DT**2.0*CornerFreq**2.0 - 2*DT*CornerFreq + 4.0*Damp + FP%nfs_OutputSignalLast1(inst) = InputSignal + FP%nfs_OutputSignalLast2(inst) = InputSignal + FP%nfs_InputSignalLast1(inst) = InputSignal + FP%nfs_InputSignalLast2(inst) = InputSignal + FP%nfs_b2(inst) = 2.0 * DT * CornerFreq + FP%nfs_b0(inst) = -FP%nfs_b2(inst) + FP%nfs_a2(inst) = Damp*DT**2.0*CornerFreq**2.0 + 2.0*DT*CornerFreq + 4.0*Damp + FP%nfs_a1(inst) = 2.0*Damp*DT**2.0*CornerFreq**2.0 - 8.0*Damp + FP%nfs_a0(inst) = Damp*DT**2.0*CornerFreq**2.0 - 2*DT*CornerFreq + 4.0*Damp ENDIF - NotchFilterSlopes = 1.0/a2(inst) * (b2(inst)*InputSignal + b0(inst)*InputSignalLast2(inst) & - - a1(inst)*OutputSignalLast1(inst) - a0(inst)*OutputSignalLast2(inst)) - ! Body - ! NotchFilterSlopes = 1.0/(4.0+2.0*DT*Damp*CornerFreq+DT**2.0*CornerFreq**2.0) * ( (8.0-2.0*DT**2.0*CornerFreq**2.0)*OutputSignalLast1(inst) & - ! + (-4.0+2.0*DT*Damp*CornerFreq-DT**2.0*CornerFreq**2.0)*OutputSignalLast2(inst) + & - ! (2.0*DT*Damp*CornerFreq)*InputSignal + (-2.0*DT*Damp*CornerFreq)*InputSignalLast2(inst) ) + NotchFilterSlopes = 1.0/FP%nfs_a2(inst) * (FP%nfs_b2(inst)*InputSignal + FP%nfs_b0(inst)*FP%nfs_InputSignalLast2(inst) & + - FP%nfs_a1(inst)*FP%nfs_OutputSignalLast1(inst) - FP%nfs_a0(inst)*FP%nfs_OutputSignalLast2(inst)) ! Save signals for next time step - InputSignalLast2(inst) = InputSignalLast1(inst) - InputSignalLast1(inst) = InputSignal !Save input signal for next time step - OutputSignalLast2(inst) = OutputSignalLast1(inst) !Save input signal for next time step - OutputSignalLast1(inst) = NotchFilterSlopes + FP%nfs_InputSignalLast2(inst) = FP%nfs_InputSignalLast1(inst) + FP%nfs_InputSignalLast1(inst) = InputSignal !Save input signal for next time step + FP%nfs_OutputSignalLast2(inst) = FP%nfs_OutputSignalLast1(inst) !Save input signal for next time step + FP%nfs_OutputSignalLast1(inst) = NotchFilterSlopes inst = inst + 1 END FUNCTION NotchFilterSlopes !------------------------------------------------------------------------------------------------------------------------------- - REAL(DbKi) FUNCTION NotchFilter(InputSignal, DT, omega, betaNum, betaDen, iStatus, reset, inst) + REAL FUNCTION NotchFilter(InputSignal, DT, omega, betaNum, betaDen, FP, iStatus, reset, inst) ! Discrete time Notch Filter ! Continuous Time Form: G(s) = (s^2 + 2*omega*betaNum*s + omega^2)/(s^2 + 2*omega*betaDen*s + omega^2) ! Discrete Time Form: H(z) = (b2*z^2 +b1*z^2 + b0*z)/((z^2 +a1*z^2 + a0*z)) - - REAL(DbKi), INTENT(IN) :: InputSignal - REAL(DbKi), INTENT(IN) :: DT ! time step [s] - REAL(DbKi), INTENT(IN) :: omega ! corner frequency [rad/s] - REAL(DbKi), INTENT(IN) :: betaNum ! Dampening constant in numerator of filter transfer function - REAL(DbKi), INTENT(IN) :: betaDen ! Dampening constant in denominator of filter transfer function + USE ROSCO_Types, ONLY : FilterParameters + TYPE(FilterParameters), INTENT(INOUT) :: FP + + REAL(8), INTENT(IN) :: InputSignal + REAL(8), INTENT(IN) :: DT ! time step [s] + REAL(8), INTENT(IN) :: omega ! corner frequency [rad/s] + REAL(8), INTENT(IN) :: betaNum ! Dampening constant in numerator of filter transfer function + REAL(8), INTENT(IN) :: betaDen ! Dampening constant in denominator of filter transfer function INTEGER, INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. INTEGER, INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. LOGICAL(4), INTENT(IN) :: reset ! Reset the filter to the input signal ! Local - REAL(DbKi), DIMENSION(99), SAVE :: K, b2, b1, b0, a1, a0 ! Constant gain - REAL(DbKi), DIMENSION(99), SAVE :: InputSignalLast1 ! Input signal the last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: InputSignalLast2 ! Input signal the next to last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: OutputSignalLast1 ! Output signal the last time this filter was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: OutputSignalLast2 ! Output signal the next to last time this filter was called. Supports 99 separate instances. + REAL(8) :: K ! Constant gain ! Initialization + K = 2.0/DT IF ((iStatus == 0) .OR. reset) THEN - OutputSignalLast1(inst) = InputSignal - OutputSignalLast2(inst) = InputSignal - InputSignalLast1(inst) = InputSignal - InputSignalLast2(inst) = InputSignal - K(inst) = 2.0/DT - b2(inst) = (K(inst)**2.0 + 2.0*omega*BetaNum*K(inst) + omega**2.0)/(K(inst)**2.0 + 2.0*omega*BetaDen*K(inst) + omega**2.0) - b1(inst) = (2.0*omega**2.0 - 2.0*K(inst)**2.0) / (K(inst)**2.0 + 2.0*omega*BetaDen*K(inst) + omega**2.0); - b0(inst) = (K(inst)**2.0 - 2.0*omega*BetaNum*K(inst) + omega**2.0) / (K(inst)**2.0 + 2.0*omega*BetaDen*K(inst) + omega**2.0) - a1(inst) = (2.0*omega**2.0 - 2.0*K(inst)**2.0) / (K(inst)**2.0 + 2.0*omega*BetaDen*K(inst) + omega**2.0) - a0(inst) = (K(inst)**2.0 - 2.0*omega*BetaDen*K(inst) + omega**2.0)/ (K(inst)**2.0 + 2.0*omega*BetaDen*K(inst) + omega**2.0) + FP%nf_OutputSignalLast1(inst) = InputSignal + FP%nf_OutputSignalLast2(inst) = InputSignal + FP%nf_InputSignalLast1(inst) = InputSignal + FP%nf_InputSignalLast2(inst) = InputSignal + FP%nf_b2(inst) = (K**2.0 + 2.0*omega*BetaNum*K + omega**2.0)/(K**2.0 + 2.0*omega*BetaDen*K + omega**2.0) + FP%nf_b1(inst) = (2.0*omega**2.0 - 2.0*K**2.0) / (K**2.0 + 2.0*omega*BetaDen*K + omega**2.0); + FP%nf_b0(inst) = (K**2.0 - 2.0*omega*BetaNum*K + omega**2.0) / (K**2.0 + 2.0*omega*BetaDen*K + omega**2.0) + FP%nf_a1(inst) = (2.0*omega**2.0 - 2.0*K**2.0) / (K**2.0 + 2.0*omega*BetaDen*K + omega**2.0) + FP%nf_a0(inst) = (K**2.0 - 2.0*omega*BetaDen*K + omega**2.0)/ (K**2.0 + 2.0*omega*BetaDen*K + omega**2.0) ENDIF ! Body - NotchFilter = b2(inst)*InputSignal + b1(inst)*InputSignalLast1(inst) + b0(inst)*InputSignalLast2(inst) - a1(inst)*OutputSignalLast1(inst) - a0(inst)*OutputSignalLast2(inst) + NotchFilter = FP%nf_b2(inst)*InputSignal + FP%nf_b1(inst)*FP%nf_InputSignalLast1(inst) + FP%nf_b0(inst)*FP%nf_InputSignalLast2(inst) - FP%nf_a1(inst)*FP%nf_OutputSignalLast1(inst) - FP%nf_a0(inst)*FP%nf_OutputSignalLast2(inst) ! Save signals for next time step - InputSignalLast2(inst) = InputSignalLast1(inst) - InputSignalLast1(inst) = InputSignal ! Save input signal for next time step - OutputSignalLast2(inst) = OutputSignalLast1(inst) ! Save input signal for next time step - OutputSignalLast1(inst) = NotchFilter + FP%nf_InputSignalLast2(inst) = FP%nf_InputSignalLast1(inst) + FP%nf_InputSignalLast1(inst) = InputSignal ! Save input signal for next time step + FP%nf_OutputSignalLast2(inst) = FP%nf_OutputSignalLast1(inst) ! Save input signal for next time step + FP%nf_OutputSignalLast1(inst) = NotchFilter inst = inst + 1 END FUNCTION NotchFilter @@ -254,64 +224,52 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst, ErrVar) USE ROSCO_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances, ErrorVariables - TYPE(ControlParameters), INTENT(INOUT) :: CntrPar - TYPE(LocalVariables), INTENT(INOUT) :: LocalVar - TYPE(ObjectInstances), INTENT(INOUT) :: objInst - TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + TYPE(ObjectInstances), INTENT(INOUT) :: objInst + TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar ! If there's an error, don't even try to run IF (ErrVar%aviFAIL < 0) THEN RETURN ENDIF - ! 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, LocalVar%restart, objInst%instLPF) - LocalVar%RotSpeedF = LPFilter(LocalVar%RotSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) + LocalVar%GenSpeedF = LPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) + LocalVar%RotSpeedF = LPFilter(LocalVar%RotSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ELSEIF (CntrPar%F_LPFType == 2) THEN - LocalVar%GenSpeedF = SecLPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, CntrPar%F_LPFDamping, LocalVar%iStatus, LocalVar%restart, 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, LocalVar%restart, objInst%instSecLPF) ! Second-order low-pass filter on generator speed - ELSE - IF (LocalVar%iStatus ==0) THEN - print *, 'No generator speed low-pass filter is selected in ROSCO (F_LPFType=0)' - ENDIF - LocalVar%GenSpeedF = LocalVar%GenSpeed - LocalVar%RotSpeedF = LocalVar%RotSpeed + LocalVar%GenSpeedF = SecLPFilter(LocalVar%GenSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, CntrPar%F_LPFDamping, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Second-order low-pass filter on generator speed + LocalVar%RotSpeedF = SecLPFilter(LocalVar%RotSpeed, LocalVar%DT, CntrPar%F_LPFCornerFreq, CntrPar%F_LPFDamping, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, 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 - LocalVar%GenSpeedF = NotchFilter(LocalVar%GenSpeedF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%iStatus, LocalVar%restart, objInst%instNotch) + LocalVar%GenSpeedF = NotchFilter(LocalVar%GenSpeedF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instNotch) ENDIF ! Filtering the tower fore-aft acceleration signal - IF (CntrPar%Fl_Mode > 0) THEN + IF (CntrPar%Fl_Mode == 1) THEN ! Force to start at 0 - IF (LocalVar%iStatus == 0) THEN - LocalVar%NacIMU_FA_AccF = SecLPFilter(REAL(0.,DbKi), LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping - LocalVar%FA_AccF = SecLPFilter(REAL(0.,DbKi), LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping - ELSE - LocalVar%NacIMU_FA_AccF = SecLPFilter(LocalVar%NacIMU_FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping - LocalVar%FA_AccF = SecLPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping - ENDIF - LocalVar%NacIMU_FA_AccF = HPFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, CntrPar%F_FlHighPassFreq, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) - LocalVar%FA_AccF = HPFilter(LocalVar%FA_AccF, LocalVar%DT, CntrPar%F_FlHighPassFreq, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) + LocalVar%NacIMU_FA_AccF = SecLPFilter(LocalVar%NacIMU_FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping + LocalVar%FA_AccF = SecLPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping + LocalVar%NacIMU_FA_AccF = HPFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, 0.0167, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) + LocalVar%FA_AccF = HPFilter(LocalVar%FA_AccF, LocalVar%DT, 0.0167, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) IF (CntrPar%F_NotchType >= 2) THEN - LocalVar%NACIMU_FA_AccF = NotchFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%iStatus, LocalVar%restart, objInst%instNotch) ! Fixed Damping - LocalVar%FA_AccF = NotchFilter(LocalVar%FA_AccF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%iStatus, LocalVar%restart, objInst%instNotch) ! Fixed Damping + LocalVar%NACIMU_FA_AccF = NotchFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instNotch) ! Fixed Damping + LocalVar%FA_AccF = NotchFilter(LocalVar%FA_AccF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instNotch) ! Fixed Damping ENDIF ENDIF - LocalVar%FA_AccHPF = HPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%FA_HPFCornerFreq, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) + LocalVar%FA_AccHPF = HPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%FA_HPFCornerFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) ! Filter Wind Speed Estimator Signal - LocalVar%We_Vw_F = LPFilter(LocalVar%WE_Vw, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%iStatus,LocalVar%restart,objInst%instLPF) ! 30 second time constant + LocalVar%We_Vw_F = LPFilter(LocalVar%WE_Vw, LocalVar%DT, 0.209, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ! 30 second time constant ! Control commands (used by WSE, mostly) - LocalVar%VS_LastGenTrqF = SecLPFilter(LocalVar%VS_LastGenTrq, LocalVar%DT, CntrPar%F_LPFCornerFreq, 0.7_DbKi, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) - LocalVar%PC_PitComTF = SecLPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%F_LPFCornerFreq*0.25, 0.7_DbKi, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) + LocalVar%VS_LastGenTrqF = SecLPFilter(LocalVar%VS_LastGenTrq, LocalVar%DT, CntrPar%F_LPFCornerFreq, 0.7, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) + LocalVar%PC_PitComTF = SecLPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%F_LPFCornerFreq*0.25, 0.7, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) END SUBROUTINE PreFilterMeasuredSignals -END MODULE Filters + END MODULE Filters \ No newline at end of file From ed81a233671ae7c340d1a25f14c694369383d035 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 10:49:31 -0700 Subject: [PATCH 04/40] save pitcomt last --- ROSCO/src/Controllers.f90 | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ROSCO/src/Controllers.f90 b/ROSCO/src/Controllers.f90 index 73b0f951..29e11841 100644 --- a/ROSCO/src/Controllers.f90 +++ b/ROSCO/src/Controllers.f90 @@ -39,7 +39,6 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar, ErrVar) ! Allocate Variables: INTEGER(IntKi) :: K ! Index used for looping through blades. - REAL(DbKi), Save :: PitComT_Last CHARACTER(*), PARAMETER :: RoutineName = 'PitchControl' @@ -100,8 +99,8 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar, ErrVar) ! Saturate collective pitch commands: LocalVar%PC_PitComT = saturate(LocalVar%PC_PitComT, LocalVar%PC_MinPit, CntrPar%PC_MaxPit) ! Saturate the overall command using the pitch angle limits - LocalVar%PC_PitComT = ratelimit(LocalVar%PC_PitComT, PitComT_Last, CntrPar%PC_MinRat, CntrPar%PC_MaxRat, LocalVar%DT) ! Saturate the overall command of blade K using the pitch rate limit - PitComT_Last = LocalVar%PC_PitComT + LocalVar%PC_PitComT = ratelimit(LocalVar%PC_PitComT, LocalVar%PC_PitComT_Last, CntrPar%PC_MinRat, CntrPar%PC_MaxRat, LocalVar%DT) ! Saturate the overall command of blade K using the pitch rate limit + LocalVar%PC_PitComT_Last = LocalVar%PC_PitComT ! Combine and saturate all individual pitch commands: ! Filter to emulate pitch actuator From 3bbaf86a82646e5580de849f0df318357a13ad77 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 10:53:03 -0700 Subject: [PATCH 05/40] Move IPC saved variables to localvars --- ROSCO/src/Controllers.f90 | 40 +++++++++++++++++++-------------------- 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/ROSCO/src/Controllers.f90 b/ROSCO/src/Controllers.f90 index 29e11841..d6ded5d1 100644 --- a/ROSCO/src/Controllers.f90 +++ b/ROSCO/src/Controllers.f90 @@ -291,9 +291,7 @@ SUBROUTINE IPC(CntrPar, LocalVar, objInst) REAL(DbKi) :: PitComIPC(3), PitComIPCF(3), PitComIPC_1P(3), PitComIPC_2P(3) INTEGER(IntKi) :: K ! Integer used to loop through turbine blades REAL(DbKi) :: axisTilt_1P, axisYaw_1P, axisYawF_1P ! Direct axis and quadrature axis outputted by Coleman transform, 1P - REAL(DbKi), SAVE :: IntAxisTilt_1P, IntAxisYaw_1P ! Integral of the direct axis and quadrature axis, 1P REAL(DbKi) :: axisTilt_2P, axisYaw_2P, axisYawF_2P ! Direct axis and quadrature axis outputted by Coleman transform, 1P - REAL(DbKi), SAVE :: IntAxisTilt_2P, IntAxisYaw_2P ! Integral of the direct axis and quadrature axis, 1P REAL(DbKi) :: IntAxisYawIPC_1P ! IPC contribution with yaw-by-IPC component REAL(DbKi) :: Y_MErrF, Y_MErrF_IPC ! Unfiltered and filtered yaw alignment error [rad] @@ -302,10 +300,10 @@ SUBROUTINE IPC(CntrPar, LocalVar, objInst) ! Initialization ! Set integrals to be 0 in the first time step IF (LocalVar%iStatus==0) THEN - IntAxisTilt_1P = 0.0 - IntAxisYaw_1P = 0.0 - IntAxisTilt_2P = 0.0 - IntAxisYaw_2P = 0.0 + LocalVar%IPC_IntAxisTilt_1P = 0.0 + LocalVar%IPC_IntAxisYaw_1P = 0.0 + LocalVar%IPC_IntAxisTilt_2P = 0.0 + LocalVar%IPC_IntAxisYaw_2P = 0.0 END IF ! Pass rootMOOPs through the Coleman transform to get the tilt and yaw moment axis @@ -324,30 +322,30 @@ SUBROUTINE IPC(CntrPar, LocalVar, objInst) ! Integrate the signal and multiply with the IPC gain IF ((CntrPar%IPC_ControlMode >= 1) .AND. (CntrPar%Y_ControlMode /= 2)) THEN - IntAxisTilt_1P = IntAxisTilt_1P + LocalVar%DT * CntrPar%IPC_KI(1) * axisTilt_1P - IntAxisYaw_1P = IntAxisYaw_1P + LocalVar%DT * CntrPar%IPC_KI(1) * axisYawF_1P - IntAxisTilt_1P = saturate(IntAxisTilt_1P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat) - IntAxisYaw_1P = saturate(IntAxisYaw_1P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat) + LocalVar%IPC_IntAxisTilt_1P = LocalVar%IPC_IntAxisTilt_1P + LocalVar%DT * CntrPar%IPC_KI(1) * axisTilt_1P + LocalVar%IPC_IntAxisYaw_1P = LocalVar%IPC_IntAxisYaw_1P + LocalVar%DT * CntrPar%IPC_KI(1) * axisYawF_1P + LocalVar%IPC_IntAxisTilt_1P = saturate(LocalVar%IPC_IntAxisTilt_1P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat) + LocalVar%IPC_IntAxisYaw_1P = saturate(LocalVar%IPC_IntAxisYaw_1P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat) IF (CntrPar%IPC_ControlMode >= 2) THEN - IntAxisTilt_2P = IntAxisTilt_2P + LocalVar%DT * CntrPar%IPC_KI(2) * axisTilt_2P - IntAxisYaw_2P = IntAxisYaw_2P + LocalVar%DT * CntrPar%IPC_KI(2) * axisYawF_2P - IntAxisTilt_2P = saturate(IntAxisTilt_2P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat) - IntAxisYaw_2P = saturate(IntAxisYaw_2P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat) + LocalVar%IPC_IntAxisTilt_2P = LocalVar%IPC_IntAxisTilt_2P + LocalVar%DT * CntrPar%IPC_KI(2) * axisTilt_2P + LocalVar%IPC_IntAxisYaw_2P = LocalVar%IPC_IntAxisYaw_2P + LocalVar%DT * CntrPar%IPC_KI(2) * axisYawF_2P + LocalVar%IPC_IntAxisTilt_2P = saturate(LocalVar%IPC_IntAxisTilt_2P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat) + LocalVar%IPC_IntAxisYaw_2P = saturate(LocalVar%IPC_IntAxisYaw_2P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat) END IF ELSE - IntAxisTilt_1P = 0.0 - IntAxisYaw_1P = 0.0 - IntAxisTilt_2P = 0.0 - IntAxisYaw_2P = 0.0 + LocalVar%IPC_IntAxisTilt_1P = 0.0 + LocalVar%IPC_IntAxisYaw_1P = 0.0 + LocalVar%IPC_IntAxisTilt_2P = 0.0 + LocalVar%IPC_IntAxisYaw_2P = 0.0 END IF ! Add the yaw-by-IPC contribution - IntAxisYawIPC_1P = IntAxisYaw_1P + Y_MErrF_IPC + IntAxisYawIPC_1P = LocalVar%IPC_IntAxisYaw_1P + Y_MErrF_IPC ! Pass direct and quadrature axis through the inverse Coleman transform to get the commanded pitch angles - CALL ColemanTransformInverse(IntAxisTilt_1P, IntAxisYawIPC_1P, LocalVar%Azimuth, NP_1, CntrPar%IPC_aziOffset(1), PitComIPC_1P) - CALL ColemanTransformInverse(IntAxisTilt_2P, IntAxisYaw_2P, LocalVar%Azimuth, NP_2, CntrPar%IPC_aziOffset(2), PitComIPC_2P) + CALL ColemanTransformInverse(LocalVar%IPC_IntAxisTilt_1P, IntAxisYawIPC_1P, LocalVar%Azimuth, NP_1, CntrPar%IPC_aziOffset(1), PitComIPC_1P) + CALL ColemanTransformInverse(LocalVar%IPC_IntAxisTilt_2P, LocalVar%IPC_IntAxisYaw_2P, LocalVar%Azimuth, NP_2, CntrPar%IPC_aziOffset(2), PitComIPC_2P) ! Sum nP IPC contributions and store to LocalVar data type DO K = 1,LocalVar%NumBl From 30e98d6705bc950e624def9f5011950a27d5c02e Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 11:00:32 -0700 Subject: [PATCH 06/40] Saved pi controller variables to localvar --- ROSCO/src/Controllers.f90 | 24 ++++----- ROSCO/src/Functions.f90 | 107 ++++++++++++++++++-------------------- 2 files changed, 63 insertions(+), 68 deletions(-) diff --git a/ROSCO/src/Controllers.f90 b/ROSCO/src/Controllers.f90 index d6ded5d1..1d962b70 100644 --- a/ROSCO/src/Controllers.f90 +++ b/ROSCO/src/Controllers.f90 @@ -58,9 +58,9 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar, ErrVar) ! 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), LocalVar%restart, objInst%instPI) + LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_FinePit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%PitCom(1), LocalVar%piP, LocalVar%restart, objInst%instPI) ELSE - LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, LocalVar%PC_MinPit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%BlPitch(1), LocalVar%restart, objInst%instPI) + LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, LocalVar%PC_MinPit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%BlPitch(1), LocalVar%piP, LocalVar%restart, objInst%instPI) END IF DebugVar%PC_PICommand = LocalVar%PC_PitComT ! Find individual pitch control contribution @@ -170,14 +170,14 @@ SUBROUTINE VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar) END IF ! PI controller - LocalVar%GenTq = PIController(LocalVar%VS_SpdErr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, LocalVar%VS_MaxTq, LocalVar%DT, LocalVar%VS_LastGenTrq, LocalVar%restart, objInst%instPI) + LocalVar%GenTq = PIController(LocalVar%VS_SpdErr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, LocalVar%VS_MaxTq, LocalVar%DT, LocalVar%VS_LastGenTrq, LocalVar%piP, LocalVar%restart, objInst%instPI) LocalVar%GenTq = saturate(LocalVar%GenTq, CntrPar%VS_MinTq, LocalVar%VS_MaxTq) ! 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_MaxOMTq, LocalVar%restart, 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, LocalVar%restart, 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, LocalVar%piP, LocalVar%restart, 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, LocalVar%piP, LocalVar%restart, objInst%instPI) ! The action IF (LocalVar%VS_State == 1) THEN ! Region 1.5 @@ -313,7 +313,7 @@ SUBROUTINE IPC(CntrPar, LocalVar, objInst) ! High-pass filter the MBC yaw component and filter yaw alignment error, and compute the yaw-by-IPC contribution IF (CntrPar%Y_ControlMode == 2) THEN Y_MErrF = SecLPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_IPC_omegaLP, CntrPar%Y_IPC_zetaLP, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) - Y_MErrF_IPC = PIController(Y_MErrF, CntrPar%Y_IPC_KP(1), CntrPar%Y_IPC_KI(1), -CntrPar%Y_IPC_IntSat, CntrPar%Y_IPC_IntSat, LocalVar%DT, 0.0_DbKi, LocalVar%restart, objInst%instPI) + Y_MErrF_IPC = PIController(Y_MErrF, CntrPar%Y_IPC_KP(1), CntrPar%Y_IPC_KI(1), -CntrPar%Y_IPC_IntSat, CntrPar%Y_IPC_IntSat, LocalVar%DT, 0.0_DbKi, LocalVar%piP, LocalVar%restart, objInst%instPI) ELSE axisYawF_1P = axisYaw_1P Y_MErrF = 0.0 @@ -375,7 +375,7 @@ SUBROUTINE ForeAftDamping(CntrPar, LocalVar, objInst) TYPE(ObjectInstances), INTENT(INOUT) :: objInst ! Body - LocalVar%FA_AccHPFI = PIController(LocalVar%FA_AccHPF, 0.0_DbKi, CntrPar%FA_KI, -CntrPar%FA_IntSat, CntrPar%FA_IntSat, LocalVar%DT, 0.0_DbKi, LocalVar%restart, objInst%instPI) + LocalVar%FA_AccHPFI = PIController(LocalVar%FA_AccHPF, 0.0_DbKi, CntrPar%FA_KI, -CntrPar%FA_IntSat, CntrPar%FA_IntSat, LocalVar%DT, 0.0_DbKi, LocalVar%piP, LocalVar%restart, objInst%instPI) ! Store the fore-aft pitch contribution to LocalVar data type DO K = 1,LocalVar%NumBl @@ -393,15 +393,15 @@ REAL(DbKi) FUNCTION FloatingFeedback(LocalVar, CntrPar, objInst) IMPLICIT NONE ! Inputs TYPE(ControlParameters), INTENT(IN) :: CntrPar - TYPE(LocalVariables), INTENT(IN) :: LocalVar + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar TYPE(ObjectInstances), INTENT(INOUT) :: objInst ! Allocate Variables REAL(DbKi) :: FA_vel ! Tower fore-aft velocity [m/s] REAL(DbKi) :: NacIMU_FA_vel ! Tower fore-aft pitching velocity [rad/s] ! Calculate floating contribution to pitch command - FA_vel = PIController(LocalVar%FA_AccF, 0.0_DbKi, 1.0_DbKi, -100.0_DbKi , 100.0_DbKi ,LocalVar%DT, 0.0_DbKi, LocalVar%restart, objInst%instPI) ! NJA: should never reach saturation limits.... - NacIMU_FA_vel = PIController(LocalVar%NacIMU_FA_AccF, 0.0_DbKi, 1.0_DbKi, -100.0_DbKi , 100.0_DbKi ,LocalVar%DT, 0.0_DbKi, LocalVar%restart, objInst%instPI) ! NJA: should never reach saturation limits.... + FA_vel = PIController(LocalVar%FA_AccF, 0.0_DbKi, 1.0_DbKi, -100.0_DbKi , 100.0_DbKi ,LocalVar%DT, 0.0_DbKi, LocalVar%piP, LocalVar%restart, objInst%instPI) ! NJA: should never reach saturation limits.... + NacIMU_FA_vel = PIController(LocalVar%NacIMU_FA_AccF, 0.0_DbKi, 1.0_DbKi, -100.0_DbKi , 100.0_DbKi ,LocalVar%DT, 0.0_DbKi, LocalVar%piP, LocalVar%restart, objInst%instPI) ! NJA: should never reach saturation limits.... if (CntrPar%Fl_Mode == 1) THEN FloatingFeedback = (0.0_DbKi - FA_vel) * CntrPar%Fl_Kp !* LocalVar%PC_KP/maxval(CntrPar%PC_GS_KP) ELSEIF (CntrPar%Fl_Mode == 2) THEN @@ -445,7 +445,7 @@ SUBROUTINE FlapControl(avrSWAP, CntrPar, LocalVar, objInst) RootMOOP_F(3) = SecLPFilter(LocalVar%rootMOOP(3),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) ! Initialize controller IF (CntrPar%Flp_Mode == 2) THEN - LocalVar%Flp_Angle(K) = PIIController(RootMyb_VelErr(K), 0 - LocalVar%Flp_Angle(K), CntrPar%Flp_Kp, CntrPar%Flp_Ki, 0.05, -CntrPar%Flp_MaxPit , CntrPar%Flp_MaxPit , LocalVar%DT, 0.0, .TRUE., objInst%instPI) + LocalVar%Flp_Angle(K) = PIIController(RootMyb_VelErr(K), 0 - LocalVar%Flp_Angle(K), CntrPar%Flp_Kp, CntrPar%Flp_Ki, 0.05, -CntrPar%Flp_MaxPit , CntrPar%Flp_MaxPit , LocalVar%DT, 0.0, LocalVar%piP, LocalVar%restart, objInst%instPI) ENDIF ! Steady flap angle @@ -465,7 +465,7 @@ SUBROUTINE FlapControl(avrSWAP, CntrPar, LocalVar, objInst) RootMyb_VelErr(K) = 0 - RootMyb_Vel(K) ! Find flap angle command - includes an integral term to encourage zero flap angle - LocalVar%Flp_Angle(K) = PIIController(RootMyb_VelErr(K), 0 - LocalVar%Flp_Angle(K), CntrPar%Flp_Kp, CntrPar%Flp_Ki, REAL(0.05,DbKi), -CntrPar%Flp_MaxPit , CntrPar%Flp_MaxPit , LocalVar%DT, 0.0, LocalVar%restart, objInst%instPI) + LocalVar%Flp_Angle(K) = PIIController(RootMyb_VelErr(K), 0 - LocalVar%Flp_Angle(K), CntrPar%Flp_Kp, CntrPar%Flp_Ki, REAL(0.05,DbKi), -CntrPar%Flp_MaxPit , CntrPar%Flp_MaxPit , LocalVar%DT, 0.0, LocalVar%piP, LocalVar%restart, objInst%instPI) ! Saturation Limits LocalVar%Flp_Angle(K) = saturate(LocalVar%Flp_Angle(K), -CntrPar%Flp_MaxPit, CntrPar%Flp_MaxPit) diff --git a/ROSCO/src/Functions.f90 b/ROSCO/src/Functions.f90 index 6b133502..53338f46 100644 --- a/ROSCO/src/Functions.f90 +++ b/ROSCO/src/Functions.f90 @@ -67,92 +67,87 @@ REAL(DbKi) FUNCTION ratelimit(inputSignal, inputSignalPrev, minRate, maxRate, DT END FUNCTION ratelimit !------------------------------------------------------------------------------------------------------------------------------- - REAL(DbKi) FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, reset, inst) + REAL FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, piP, reset, inst) + USE ROSCO_Types, ONLY : piParams + ! PI controller, with output saturation IMPLICIT NONE ! Allocate Inputs - REAL(DbKi), INTENT(IN) :: error - REAL(DbKi), INTENT(IN) :: kp - REAL(DbKi), INTENT(IN) :: ki - REAL(DbKi), INTENT(IN) :: minValue - REAL(DbKi), INTENT(IN) :: maxValue - REAL(DbKi), INTENT(IN) :: DT - INTEGER(IntKi), INTENT(INOUT) :: inst - REAL(DbKi), INTENT(IN) :: I0 - LOGICAL, INTENT(IN) :: reset + REAL(8), INTENT(IN) :: error + REAL(8), INTENT(IN) :: kp + REAL(8), INTENT(IN) :: ki + REAL(8), INTENT(IN) :: minValue + REAL(8), INTENT(IN) :: maxValue + REAL(8), INTENT(IN) :: DT + INTEGER(4), INTENT(INOUT) :: inst + REAL(8), INTENT(IN) :: I0 + TYPE(piParams), INTENT(INOUT) :: piP + LOGICAL, INTENT(IN) :: reset ! Allocate local variables - INTEGER(IntKi) :: i ! Counter for making arrays - REAL(DbKi) :: PTerm ! Proportional term - REAL(DbKi), DIMENSION(99), SAVE :: ITerm = (/ (real(9999.9), i = 1,99) /) ! Integral term, current. - REAL(DbKi), DIMENSION(99), SAVE :: ITermLast = (/ (real(9999.9), i = 1,99) /) ! Integral term, the last time this controller was called. Supports 99 separate instances. - INTEGER(IntKi), DIMENSION(99), SAVE :: FirstCall = (/ (1, i=1,99) /) ! First call of this function? - + INTEGER(4) :: i ! Counter for making arrays + REAL(8) :: PTerm ! Proportional term + ! Initialize persistent variables/arrays, and set inital condition for integrator term - IF ((FirstCall(inst) == 1) .OR. reset) THEN - ITerm(inst) = I0 - ITermLast(inst) = I0 + IF (reset) THEN + piP%ITerm(inst) = I0 + piP%ITermLast(inst) = I0 - FirstCall(inst) = 0 PIController = I0 ELSE PTerm = kp*error - ITerm(inst) = ITerm(inst) + DT*ki*error - ITerm(inst) = saturate(ITerm(inst), minValue, maxValue) - PIController = saturate(PTerm + ITerm(inst), minValue, maxValue) + piP%ITerm(inst) = piP%ITerm(inst) + DT*ki*error + piP%ITerm(inst) = saturate(piP%ITerm(inst), minValue, maxValue) + PIController = saturate(PTerm + piP%ITerm(inst), minValue, maxValue) - ITermLast(inst) = ITerm(inst) + piP%ITermLast(inst) = piP%ITerm(inst) END IF inst = inst + 1 END FUNCTION PIController !------------------------------------------------------------------------------------------------------------------------------- - REAL(DbKi) FUNCTION PIIController(error, error2, kp, ki, ki2, minValue, maxValue, DT, I0, reset, inst) + REAL(8) FUNCTION PIIController(error, error2, kp, ki, ki2, minValue, maxValue, DT, I0, piP, reset, inst) ! PI controller, with output saturation. ! Added error2 term for additional integral control input - + USE ROSCO_Types, ONLY : piParams + IMPLICIT NONE ! Allocate Inputs - REAL(DbKi), INTENT(IN) :: error - REAL(DbKi), INTENT(IN) :: error2 - REAL(DbKi), INTENT(IN) :: kp - REAL(DbKi), INTENT(IN) :: ki2 - REAL(DbKi), INTENT(IN) :: ki - REAL(DbKi), INTENT(IN) :: minValue - REAL(DbKi), INTENT(IN) :: maxValue - REAL(DbKi), INTENT(IN) :: DT - INTEGER(IntKi), INTENT(INOUT) :: inst - REAL(DbKi), INTENT(IN) :: I0 + REAL(8), INTENT(IN) :: error + REAL(8), INTENT(IN) :: error2 + REAL(8), INTENT(IN) :: kp + REAL(8), INTENT(IN) :: ki2 + REAL(8), INTENT(IN) :: ki + REAL(8), INTENT(IN) :: minValue + REAL(8), INTENT(IN) :: maxValue + REAL(8), INTENT(IN) :: DT + INTEGER(4), INTENT(INOUT) :: inst + REAL(8), INTENT(IN) :: I0 + TYPE(piParams), INTENT(INOUT) :: piP LOGICAL, INTENT(IN) :: reset ! Allocate local variables - INTEGER(IntKi) :: i ! Counter for making arrays - REAL(DbKi) :: PTerm ! Proportional term - REAL(DbKi), DIMENSION(99), SAVE :: ITerm = (/ (real(9999.9), i = 1,99) /) ! Integral term, current. - REAL(DbKi), DIMENSION(99), SAVE :: ITermLast = (/ (real(9999.9), i = 1,99) /) ! Integral term, the last time this controller was called. Supports 99 separate instances. - REAL(DbKi), DIMENSION(99), SAVE :: ITerm2 = (/ (real(9999.9), i = 1,99) /) ! Second Integral term, current. - REAL(DbKi), DIMENSION(99), SAVE :: ITermLast2 = (/ (real(9999.9), i = 1,99) /) ! Second Integral term, the last time this controller was called. Supports 99 separate instances. - INTEGER(IntKi), DIMENSION(99), SAVE :: FirstCall = (/ (1, i=1,99) /) ! First call of this function? - + INTEGER(4) :: i ! Counter for making arrays + REAL(8) :: PTerm ! Proportional term + ! Initialize persistent variables/arrays, and set inital condition for integrator term - IF ((FirstCall(inst) == 1) .OR. reset) THEN - ITerm(inst) = I0 - ITermLast(inst) = I0 - ITerm2(inst) = I0 - ITermLast2(inst) = I0 + IF (reset) THEN + piP%ITerm(inst) = I0 + piP%ITermLast(inst) = I0 + piP%ITerm2(inst) = I0 + piP%ITermLast2(inst) = I0 - FirstCall(inst) = 0 PIIController = I0 ELSE PTerm = kp*error - ITerm(inst) = ITerm(inst) + DT*ki*error - ITerm2(inst) = ITerm2(inst) + DT*ki2*error2 - ITerm(inst) = saturate(ITerm(inst), minValue, maxValue) - ITerm2(inst) = saturate(ITerm2(inst), minValue, maxValue) - PIIController = PTerm + ITerm(inst) + ITerm2(inst) + piP%ITerm(inst) = piP%ITerm(inst) + DT*ki*error + piP%ITerm2(inst) = piP%ITerm2(inst) + DT*ki2*error2 + piP%ITerm(inst) = saturate(piP%ITerm(inst), minValue, maxValue) + piP%ITerm2(inst) = saturate(piP%ITerm2(inst), minValue, maxValue) + PIIController = PTerm + piP%ITerm(inst) + piP%ITerm2(inst) PIIController = saturate(PIIController, minValue, maxValue) - ITermLast(inst) = ITerm(inst) + piP%ITermLast(inst) = piP%ITerm(inst) END IF inst = inst + 1 From 38dff08aa1cec088efb7634ab377967a6236dba1 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 13:20:54 -0700 Subject: [PATCH 07/40] Save RootMyb_Last to localvar --- ROSCO/src/Controllers.f90 | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/ROSCO/src/Controllers.f90 b/ROSCO/src/Controllers.f90 index 1d962b70..629aadd1 100644 --- a/ROSCO/src/Controllers.f90 +++ b/ROSCO/src/Controllers.f90 @@ -426,15 +426,14 @@ SUBROUTINE FlapControl(avrSWAP, CntrPar, LocalVar, objInst) INTEGER(IntKi) :: K REAL(DbKi) :: rootMOOP_F(3) REAL(DbKi) :: RootMyb_Vel(3) - REAL(DbKi), SAVE :: RootMyb_Last(3) REAL(DbKi) :: RootMyb_VelErr(3) ! Flap control IF (CntrPar%Flp_Mode >= 1) THEN IF ((LocalVar%iStatus == 0) .AND. (CntrPar%Flp_Mode >= 1)) THEN - RootMyb_Last(1) = 0 - LocalVar%rootMOOP(1) - RootMyb_Last(2) = 0 - LocalVar%rootMOOP(2) - RootMyb_Last(3) = 0 - LocalVar%rootMOOP(3) + LocalVar%RootMyb_Last(1) = 0 - LocalVar%rootMOOP(1) + LocalVar%RootMyb_Last(2) = 0 - LocalVar%rootMOOP(2) + LocalVar%RootMyb_Last(3) = 0 - LocalVar%rootMOOP(3) ! Initial Flap angle LocalVar%Flp_Angle(1) = CntrPar%Flp_Angle LocalVar%Flp_Angle(2) = CntrPar%Flp_Angle @@ -461,7 +460,7 @@ SUBROUTINE FlapControl(avrSWAP, CntrPar, LocalVar, objInst) RootMOOP_F(K) = SecLPFilter(LocalVar%rootMOOP(K),LocalVar%DT, CntrPar%F_FlpCornerFreq(1), CntrPar%F_FlpCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart,objInst%instSecLPF) ! Find derivative and derivative error of blade root bending moment - RootMyb_Vel(K) = (RootMOOP_F(K) - RootMyb_Last(K))/LocalVar%DT + RootMyb_Vel(K) = (RootMOOP_F(K) - LocalVar%RootMyb_Last(K))/LocalVar%DT RootMyb_VelErr(K) = 0 - RootMyb_Vel(K) ! Find flap angle command - includes an integral term to encourage zero flap angle @@ -470,7 +469,7 @@ SUBROUTINE FlapControl(avrSWAP, CntrPar, LocalVar, objInst) LocalVar%Flp_Angle(K) = saturate(LocalVar%Flp_Angle(K), -CntrPar%Flp_MaxPit, CntrPar%Flp_MaxPit) ! Save some data for next iteration - RootMyb_Last(K) = RootMOOP_F(K) + LocalVar%RootMyb_Last(K) = RootMOOP_F(K) END DO ENDIF From fc1c9ecdcf1b0ef076a22576f4d5ecda3cbb7abf Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 13:21:34 -0700 Subject: [PATCH 08/40] ROSCO_IO - initial commit. Include restart and debug functions --- ROSCO/src/ROSCO_IO.f90 | 498 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 498 insertions(+) create mode 100644 ROSCO/src/ROSCO_IO.f90 diff --git a/ROSCO/src/ROSCO_IO.f90 b/ROSCO/src/ROSCO_IO.f90 new file mode 100644 index 00000000..8bc50901 --- /dev/null +++ b/ROSCO/src/ROSCO_IO.f90 @@ -0,0 +1,498 @@ +! Copyright 2019 NREL + +! Licensed under the Apache License, Version 2.0 (the "License"); you may not use +! this file except in compliance with the License. You may obtain a copy of the +! License at http://www.apache.org/licenses/LICENSE-2.0 + +! Unless required by applicable law or agreed to in writing, software distributed +! under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR +! CONDITIONS OF ANY KIND, either express or implied. See the License for the +! specific language governing permissions and limitations under the License. +! ------------------------------------------------------------------------------------------- +! This handles all of ROSCO's input/output files ~WITH THE EXCPETION~ of reading the primary +! input files. The DISCON.IN and Cp_Ct_Cq.txt files are handed in ReadSetParameters.f90 + +MODULE ROSCO_IO + USE, INTRINSIC :: ISO_C_Binding + USE ROSCO_Types + USE ReadSetParameters + IMPLICIT NONE +CONTAINS + SUBROUTINE WriteRestartFile(LocalVar, CntrPar, objInst, RootName, size_avcOUTNAME) + USE ROSCO_Types, ONLY : LocalVariables, ObjectInstances, ControlParameters + + TYPE(LocalVariables), INTENT(IN) :: LocalVar + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + TYPE(ObjectInstances), INTENT(INOUT) :: objInst + INTEGER(4), INTENT(IN) :: size_avcOUTNAME + CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName + + INTEGER(4), PARAMETER :: Un = 87 ! I/O unit for pack/unpack (checkpoint & restart) + INTEGER(4) :: I ! Generic index. + CHARACTER(128) :: InFile ! Input checkpoint file + INTEGER(4) :: ErrStat + CHARACTER(128) :: ErrMsg + CHARACTER(128) :: n_t_global ! timestep number as a string + + WRITE(n_t_global, '(I0.0)' ), NINT(LocalVar%Time/LocalVar%DT) + InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp' + OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='WRITE' ) + + IF ( ErrStat /= 0 ) THEN + ErrMsg = 'Cannot open file "'//TRIM( InFile )//'". Another program may have locked it for writing.' + + ELSE + ! From AVR SWAP + WRITE( Un, IOSTAT=ErrStat) LocalVar%iStatus + WRITE( Un, IOSTAT=ErrStat) LocalVar%Time + WRITE( Un, IOSTAT=ErrStat) LocalVar%DT + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_GenPwr + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenSpeed + WRITE( Un, IOSTAT=ErrStat) LocalVar%RotSpeed + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_M + WRITE( Un, IOSTAT=ErrStat) LocalVar%HorWindV + WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%BlPitch(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%BlPitch(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%BlPitch(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%Azimuth + WRITE( Un, IOSTAT=ErrStat) LocalVar%NumBl + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_Acc + WRITE( Un, IOSTAT=ErrStat) LocalVar%NacIMU_FA_Acc + ! Internal Control Variables + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPF + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPFI + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%RotSpeedF + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenSpeedF + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenTq + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenTqMeas + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenArTq + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenBrTq + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_1P + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_1P + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_2P + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_2P + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_KP + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_KI + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_KD + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_TF + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_MaxPit + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_MinPit + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_Last + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComTF + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PwrErr + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_SpdErr + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_State + WRITE( Un, IOSTAT=ErrStat) LocalVar%PitCom(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%PitCom(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%PitCom(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%SS_DelOmegaF + WRITE( Un, IOSTAT=ErrStat) LocalVar%TestType + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_MaxTq + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrq + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenPwr + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_MechGenPwr + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrAr + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrBr + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErr + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_State + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_Rgn3Pitch + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_Vw + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_Vw_F + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_VwI + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_VwIdot + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrqF + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_AccErr + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFFast + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFSlow + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_MErr + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_YawEndT + WRITE( Un, IOSTAT=ErrStat) LocalVar%SD + WRITE( Un, IOSTAT=ErrStat) LocalVar%Fl_PitCom + WRITE( Un, IOSTAT=ErrStat) LocalVar%NACIMU_FA_AccF + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_AccF + WRITE( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE_SIZE + WRITE( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE + WRITE( Un, IOSTAT=ErrStat) LocalVar%restart + + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%om_r + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%v_t + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%v_m + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%v_h + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%P + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%xh + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%K + + WRITE( Un, IOSTAT=ErrStat) objInst%instLPF + WRITE( Un, IOSTAT=ErrStat) objInst%instSecLPF + WRITE( Un, IOSTAT=ErrStat) objInst%instHPF + WRITE( Un, IOSTAT=ErrStat) objInst%instNotchSlopes + WRITE( Un, IOSTAT=ErrStat) objInst%instNotch + WRITE( Un, IOSTAT=ErrStat) objInst%instPI + + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_InputSignalLast + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_OutputSignalLast + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast1 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%hpf_InputSignalLast + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%hpf_OutputSignalLast + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast1 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast2 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast1 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast2 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_b2 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_b0 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a2 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a1 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a0 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast1 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast2 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast1 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast2 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_b2 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_b1 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_b0 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_a1 + WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_a0 + + WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm + WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast + WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast2 + + CLOSE ( Un ) + + ENDIF + + END SUBROUTINE WriteRestartFile + + SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootName, size_avcOUTNAME, ErrVar) + USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, ObjectInstances, PerformanceData, ErrorVariables + + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + TYPE(ObjectInstances), INTENT(INOUT) :: objInst + TYPE(PerformanceData), INTENT(INOUT) :: PerfData + TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar + + + REAL(C_FLOAT), INTENT(IN) :: avrSWAP(*) + INTEGER(4), INTENT(IN) :: size_avcOUTNAME + CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName + + INTEGER(4), PARAMETER :: Un = 87 ! I/O unit for pack/unpack (checkpoint & restart) + INTEGER(4) :: I ! Generic index. + INTEGER(4) :: ErrStat + CHARACTER(128) :: ErrMsg + CHARACTER(128) :: n_t_global ! timestep number as a string + CHARACTER(128) :: InFile ! Name of ROSCO checkpoint file + + WRITE(n_t_global, '(I0.0)' ), NINT(avrSWAP(2)/avrSWAP(3)) + InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp' + OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='READ' ) + + IF ( ErrStat /= 0 ) THEN + ErrMsg = 'Cannot open file "'//TRIM( InFile )//'". Another program may have locked it for writing.' + + ELSE + ! From AVR SWAP + READ( Un, IOSTAT=ErrStat) LocalVar%iStatus + READ( Un, IOSTAT=ErrStat) LocalVar%Time + READ( Un, IOSTAT=ErrStat) LocalVar%DT + READ( Un, IOSTAT=ErrStat) LocalVar%VS_GenPwr + READ( Un, IOSTAT=ErrStat) LocalVar%GenSpeed + READ( Un, IOSTAT=ErrStat) LocalVar%RotSpeed + READ( Un, IOSTAT=ErrStat) LocalVar%Y_M + READ( Un, IOSTAT=ErrStat) LocalVar%HorWindV + READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(1) + READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(2) + READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(3) + READ( Un, IOSTAT=ErrStat) LocalVar%BlPitch(1) + READ( Un, IOSTAT=ErrStat) LocalVar%BlPitch(2) + READ( Un, IOSTAT=ErrStat) LocalVar%BlPitch(3) + READ( Un, IOSTAT=ErrStat) LocalVar%Azimuth + READ( Un, IOSTAT=ErrStat) LocalVar%NumBl + READ( Un, IOSTAT=ErrStat) LocalVar%FA_Acc + READ( Un, IOSTAT=ErrStat) LocalVar%NacIMU_FA_Acc + ! Internal Control Variables + READ( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPF + READ( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPFI + READ( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(1) + READ( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(2) + READ( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(3) + READ( Un, IOSTAT=ErrStat) LocalVar%RotSpeedF + READ( Un, IOSTAT=ErrStat) LocalVar%GenSpeedF + READ( Un, IOSTAT=ErrStat) LocalVar%GenTq + READ( Un, IOSTAT=ErrStat) LocalVar%GenTqMeas + READ( Un, IOSTAT=ErrStat) LocalVar%GenArTq + READ( Un, IOSTAT=ErrStat) LocalVar%GenBrTq + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(1) + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(2) + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(3) + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_1P + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_1P + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_2P + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_2P + READ( Un, IOSTAT=ErrStat) LocalVar%PC_KP + READ( Un, IOSTAT=ErrStat) LocalVar%PC_KI + READ( Un, IOSTAT=ErrStat) LocalVar%PC_KD + READ( Un, IOSTAT=ErrStat) LocalVar%PC_TF + READ( Un, IOSTAT=ErrStat) LocalVar%PC_MaxPit + READ( Un, IOSTAT=ErrStat) LocalVar%PC_MinPit + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_Last + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComTF + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(1) + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(2) + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(3) + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PwrErr + READ( Un, IOSTAT=ErrStat) LocalVar%PC_SpdErr + READ( Un, IOSTAT=ErrStat) LocalVar%PC_State + READ( Un, IOSTAT=ErrStat) LocalVar%PitCom(1) + READ( Un, IOSTAT=ErrStat) LocalVar%PitCom(2) + READ( Un, IOSTAT=ErrStat) LocalVar%PitCom(3) + READ( Un, IOSTAT=ErrStat) LocalVar%SS_DelOmegaF + READ( Un, IOSTAT=ErrStat) LocalVar%TestType + READ( Un, IOSTAT=ErrStat) LocalVar%VS_MaxTq + READ( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrq + READ( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenPwr + READ( Un, IOSTAT=ErrStat) LocalVar%VS_MechGenPwr + READ( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrAr + READ( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrBr + READ( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErr + READ( Un, IOSTAT=ErrStat) LocalVar%VS_State + READ( Un, IOSTAT=ErrStat) LocalVar%VS_Rgn3Pitch + READ( Un, IOSTAT=ErrStat) LocalVar%WE_Vw + READ( Un, IOSTAT=ErrStat) LocalVar%WE_Vw_F + READ( Un, IOSTAT=ErrStat) LocalVar%WE_VwI + READ( Un, IOSTAT=ErrStat) LocalVar%WE_VwIdot + READ( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrqF + READ( Un, IOSTAT=ErrStat) LocalVar%Y_AccErr + READ( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFFast + READ( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFSlow + READ( Un, IOSTAT=ErrStat) LocalVar%Y_MErr + READ( Un, IOSTAT=ErrStat) LocalVar%Y_YawEndT + READ( Un, IOSTAT=ErrStat) LocalVar%SD + READ( Un, IOSTAT=ErrStat) LocalVar%Fl_PitCom + READ( Un, IOSTAT=ErrStat) LocalVar%NACIMU_FA_AccF + READ( Un, IOSTAT=ErrStat) LocalVar%FA_AccF + READ( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(1) + READ( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(2) + READ( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(3) + READ( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(1) + READ( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(2) + READ( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(3) + READ( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE_SIZE + ALLOCATE(LocalVar%ACC_INFILE(LocalVar%ACC_INFILE_SIZE)) + READ( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE + READ( Un, IOSTAT=ErrStat) LocalVar%restart + + READ( Un, IOSTAT=ErrStat) LocalVar%WE%om_r + READ( Un, IOSTAT=ErrStat) LocalVar%WE%v_t + READ( Un, IOSTAT=ErrStat) LocalVar%WE%v_m + READ( Un, IOSTAT=ErrStat) LocalVar%WE%v_h + READ( Un, IOSTAT=ErrStat) LocalVar%WE%P + READ( Un, IOSTAT=ErrStat) LocalVar%WE%xh + READ( Un, IOSTAT=ErrStat) LocalVar%WE%K + + READ( Un, IOSTAT=ErrStat) objInst%instLPF + READ( Un, IOSTAT=ErrStat) objInst%instSecLPF + READ( Un, IOSTAT=ErrStat) objInst%instHPF + READ( Un, IOSTAT=ErrStat) objInst%instNotchSlopes + READ( Un, IOSTAT=ErrStat) objInst%instNotch + READ( Un, IOSTAT=ErrStat) objInst%instPI + + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a0 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b0 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_InputSignalLast + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_OutputSignalLast + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a0 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b0 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast1 + READ( un, IOSTAT=ErrStat) LocalVar%FP%hpf_InputSignalLast + READ( un, IOSTAT=ErrStat) LocalVar%FP%hpf_OutputSignalLast + READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast1 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast2 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast1 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast2 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_b2 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_b0 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a2 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a1 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a0 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast1 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast2 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast1 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast2 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_b2 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_b1 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_b0 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_a1 + READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_a0 + + READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm + READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast + READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm2 + READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast2 + + CLOSE ( Un ) + ENDIF + + ! Read Parameter files + CALL ReadControlParameterFileSub(CntrPar, LocalVar%ACC_INFILE, LocalVar%ACC_INFILE_SIZE, ErrVar) + IF (CntrPar%WE_Mode > 0) THEN + CALL READCpFile(CntrPar, PerfData, ErrVar) + ENDIF + + END SUBROUTINE ReadRestartFile + +!------------------------------------------------------------------------------------------------------------------------------- + SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME) + ! Debug routine, defines what gets printed to DEBUG.dbg if LoggingLevel = 1 + + USE, INTRINSIC :: ISO_C_Binding + USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, DebugVariables + + IMPLICIT NONE + + TYPE(ControlParameters), INTENT(IN) :: CntrPar + TYPE(LocalVariables), INTENT(IN) :: LocalVar + TYPE(DebugVariables), INTENT(IN) :: DebugVar + + INTEGER(IntKi), INTENT(IN) :: size_avcOUTNAME + INTEGER(IntKi) :: I , nDebugOuts ! Generic index. + CHARACTER(1), PARAMETER :: Tab = CHAR(9) ! The tab character. + CHARACTER(29), PARAMETER :: FmtDat = "(F10.3,TR5,99(ES10.3E2,TR5:))" ! The format of the debugging data + INTEGER(IntKi), PARAMETER :: UnDb = 85 ! I/O unit for the debugging information + INTEGER(IntKi), PARAMETER :: UnDb2 = 86 ! I/O unit for the debugging information, avrSWAP + REAL(ReKi), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. + CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName ! a Fortran version of the input C string (not considered an array here) [subtract 1 for the C null-character] + CHARACTER(200) :: Version ! git version of ROSCO + CHARACTER(10) :: DebugOutStr1, DebugOutStr2, DebugOutStr3, DebugOutStr4, DebugOutStr5, & + DebugOutStr6, DebugOutStr7, DebugOutStr8, DebugOutStr9, DebugOutStr10, & + DebugOutStr11, DebugOutStr12, DebugOutStr13, DebugOutStr14, DebugOutStr15, & + DebugOutStr16, DebugOutStr17, DebugOutStr18, DebugOutStr19, DebugOutStr20 + CHARACTER(10) :: DebugOutUni1, DebugOutUni2, DebugOutUni3, DebugOutUni4, DebugOutUni5, & + DebugOutUni6, DebugOutUni7, DebugOutUni8, DebugOutUni9, DebugOutUni10, & + DebugOutUni11, DebugOutUni12, DebugOutUni13, DebugOutUni14, DebugOutUni15, & + DebugOutUni16, DebugOutUni17, DebugOutUni18, DebugOutUni19, DebugOutUni20 + CHARACTER(10), ALLOCATABLE :: DebugOutStrings(:), DebugOutUnits(:) + REAL(DbKi), ALLOCATABLE :: DebugOutData(:) + + ! Set up Debug Strings and Data + ! Note that Debug strings have 10 character limit + nDebugOuts = 18 + ALLOCATE(DebugOutData(nDebugOuts)) + ! Header Unit Variable + ! Filters + DebugOutStr1 = 'FA_AccF'; DebugOutUni1 = '(rad/s^2)'; DebugOutData(1) = LocalVar%NacIMU_FA_AccF + DebugOutStr2 = 'FA_AccR'; DebugOutUni2 = '(rad/s^2)'; DebugOutData(2) = LocalVar%NacIMU_FA_Acc + DebugOutStr3 = 'RotSpeed'; DebugOutUni3 = '(rad/s)'; DebugOutData(3) = LocalVar%RotSpeed + DebugOutStr4 = 'RotSpeedF'; DebugOutUni4 = '(rad/s)'; DebugOutData(4) = LocalVar%RotSpeedF + DebugOutStr5 = 'GenSpeed'; DebugOutUni5 = '(rad/s)'; DebugOutData(5) = LocalVar%GenSpeed + DebugOutStr6 = 'GenSpeedF'; DebugOutUni6 = '(rad/s)'; DebugOutData(6) = LocalVar%GenSpeedF + ! Floating + DebugOutStr7 = 'FA_Acc'; DebugOutUni7 = '(m/s^2)'; DebugOutData(7) = LocalVar%FA_Acc + DebugOutStr8 = 'Fl_Pitcom'; DebugOutUni8 = '(rad)'; DebugOutData(8) = LocalVar%Fl_Pitcom + DebugOutStr9 = 'PC_MinPit'; DebugOutUni9 = '(rad)'; DebugOutData(9) = LocalVar%PC_MinPit + DebugOutStr10 = 'SS_dOmF'; DebugOutUni10 = '(rad/s)'; DebugOutData(10) = LocalVar%SS_DelOmegaF + ! WSE + DebugOutStr11 = 'WE_Vw'; DebugOutUni11 = '(m/s)'; DebugOutData(11) = LocalVar%WE_Vw + DebugOutStr12 = 'WE_b'; DebugOutUni12 = '(deg)'; DebugOutData(12) = DebugVar%WE_b + DebugOutStr13 = 'WE_t'; DebugOutUni13 = '(Nm)'; DebugOutData(13) = DebugVar%WE_t + DebugOutStr14 = 'WE_w'; DebugOutUni14 = '(rad/s)'; DebugOutData(14) = DebugVar%WE_w + DebugOutStr15 = 'WE_Vm'; DebugOutUni15 = '(m/s)'; DebugOutData(15) = DebugVar%WE_Vm + DebugOutStr16 = 'WE_Vt'; DebugOutUni16 = '(m/s)'; DebugOutData(16) = DebugVar%WE_Vt + DebugOutStr17 = 'WE_lambda'; DebugOutUni17 = '(-)'; DebugOutData(17) = DebugVar%WE_lambda + DebugOutStr18 = 'WE_Cp'; DebugOutUni18 = '(-)'; DebugOutData(18) = DebugVar%WE_Cp + + Allocate(DebugOutStrings(nDebugOuts)) + Allocate(DebugOutUnits(nDebugOuts)) + DebugOutStrings = [CHARACTER(10) :: DebugOutStr1, DebugOutStr2, DebugOutStr3, DebugOutStr4, & + DebugOutStr5, DebugOutStr6, DebugOutStr7, DebugOutStr8, & + DebugOutStr9, DebugOutStr10, DebugOutStr11, DebugOutStr12, & + DebugOutStr13, DebugOutStr14, DebugOutStr15, DebugOutStr16, & + DebugOutStr17, DebugOutStr18] + DebugOutUnits = [CHARACTER(10) :: DebugOutUni1, DebugOutUni2, DebugOutUni3, DebugOutUni4, & + DebugOutUni5, DebugOutUni6, DebugOutUni7, DebugOutUni8, & + DebugOutUni9, DebugOutUni10, DebugOutUni11, DebugOutUni12, & + DebugOutUni13, DebugOutUni14, DebugOutUni15, DebugOutUni1, & + DebugOutUni17, DebugOutUni18] + + ! Initialize debug file + IF ((LocalVar%iStatus == 0) .OR. (LocalVar%iStatus == -8)) THEN ! .TRUE. if we're on the first call to the DLL + ! If we're debugging, open the debug file and write the header: + ! Note that the headers will be Truncated to 10 characters!! + IF (CntrPar%LoggingLevel > 0) THEN + OPEN(unit=UnDb, FILE=RootName(1:size_avcOUTNAME-5)//'RO.dbg') + WRITE (UnDb,*) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version) + WRITE (UnDb,'(99(a10,TR5:))') 'Time', DebugOutStrings + WRITE (UnDb,'(99(a10,TR5:))') '(sec)', DebugOutUnits + END IF + + IF (CntrPar%LoggingLevel > 1) THEN + OPEN(unit=UnDb2, FILE=RootName(1:size_avcOUTNAME-5)//'RO.dbg2') + WRITE(UnDb2,'(/////)') + WRITE(UnDb2,'(A,85("'//Tab//'AvrSWAP(",I2,")"))') 'LocalVar%Time ', (i,i=1,85) + WRITE(UnDb2,'(A,85("'//Tab//'(-)"))') '(s)' + END IF + ELSE + ! Print simulation status, every 10 seconds + IF (MODULO(LocalVar%Time, 10.0_DbKi) == 0) THEN + WRITE(*, 100) LocalVar%GenSpeedF*RPS2RPM, LocalVar%BlPitch(1)*R2D, avrSWAP(15)/1000.0, LocalVar%WE_Vw ! LocalVar%Time !/1000.0 + 100 FORMAT('Generator speed: ', f6.1, ' RPM, Pitch angle: ', f5.1, ' deg, Power: ', f7.1, ' kW, Est. wind Speed: ', f5.1, ' m/s') + END IF + + ENDIF + + ! Write debug files + IF (CntrPar%LoggingLevel > 0) THEN + WRITE (UnDb,FmtDat) LocalVar%Time, DebugOutData + END IF + + IF (CntrPar%LoggingLevel > 1) THEN + WRITE (UnDb2,FmtDat) LocalVar%Time, avrSWAP(1:85) + END IF + + END SUBROUTINE Debug + +END MODULE ROSCO_IO \ No newline at end of file From 2025565b368317ccf346dbdbea52254f50648c76 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 13:21:44 -0700 Subject: [PATCH 09/40] Use ROSCO IO and call restart functions --- ROSCO/src/DISCON.F90 | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/ROSCO/src/DISCON.F90 b/ROSCO/src/DISCON.F90 index 84c5820b..1f3b9550 100644 --- a/ROSCO/src/DISCON.F90 +++ b/ROSCO/src/DISCON.F90 @@ -25,6 +25,7 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME USE :: Constants USE :: Filters USE :: Functions +USE :: ROSCO_IO IMPLICIT NONE ! Enable .dll export @@ -62,6 +63,12 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME !------------------------------------------------------------------------------------------------------------------------------ ! Main control calculations !------------------------------------------------------------------------------------------------------------------------------ + +! Check for restart +IF ( (NINT(avrSWAP(1)) == -9) .AND. (aviFAIL >= 0)) THEN ! Read restart files + CALL ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootName, SIZE(avcOUTNAME), ErrVar) +END IF + ! Read avrSWAP array into derived types/variables CALL ReadAvrSWAP(avrSWAP, LocalVar) @@ -71,12 +78,11 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME ! Filter signals CALL PreFilterMeasuredSignals(CntrPar, LocalVar, objInst, ErrVar) -IF ((LocalVar%iStatus >= 0) .AND. (ErrVar%aviFAIL >= 0)) THEN ! Only compute control calculations if no error has occurred and we are not on the last time step +IF (((LocalVar%iStatus >= 0) .OR. (LocalVar%iStatus <= -9)) .AND. (ErrVar%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, 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, ErrVar) CALL PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar, ErrVar) @@ -91,7 +97,12 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME IF (CntrPar%LoggingLevel > 0) THEN CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) END IF -END IF + +ELSEIF ((LocalVar%iStatus == -8) .AND. (ErrVar%aviFAIL >= 0)) THEN ! Write restart files + CALL WriteRestartFile(LocalVar, CntrPar, objInst, RootName, SIZE(avcOUTNAME)) + CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) + +ENDIF ! Add RoutineName to error message IF (ErrVar%aviFAIL < 0) THEN From 915922aa7429fdb05a91f115479c0738a84c5ac6 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 13:21:56 -0700 Subject: [PATCH 10/40] Remove debug from function.f90 --- ROSCO/src/Functions.f90 | 110 ---------------------------------------- 1 file changed, 110 deletions(-) diff --git a/ROSCO/src/Functions.f90 b/ROSCO/src/Functions.f90 index 53338f46..18bb27df 100644 --- a/ROSCO/src/Functions.f90 +++ b/ROSCO/src/Functions.f90 @@ -540,116 +540,6 @@ REAL(DbKi) FUNCTION AeroDynTorque(RotSpeed, BldPitch, LocalVar, CntrPar, PerfDat END FUNCTION AeroDynTorque -!------------------------------------------------------------------------------------------------------------------------------- - SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME) - ! Debug routine, defines what gets printed to DEBUG.dbg if LoggingLevel = 1 - - USE, INTRINSIC :: ISO_C_Binding - USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, DebugVariables - - IMPLICIT NONE - - TYPE(ControlParameters), INTENT(IN) :: CntrPar - TYPE(LocalVariables), INTENT(IN) :: LocalVar - TYPE(DebugVariables), INTENT(IN) :: DebugVar - - INTEGER(IntKi), INTENT(IN) :: size_avcOUTNAME - INTEGER(IntKi) :: I , nDebugOuts ! Generic index. - CHARACTER(1), PARAMETER :: Tab = CHAR(9) ! The tab character. - CHARACTER(29), PARAMETER :: FmtDat = "(F10.3,TR5,99(ES10.3E2,TR5:))" ! The format of the debugging data - INTEGER(IntKi), PARAMETER :: UnDb = 85 ! I/O unit for the debugging information - INTEGER(IntKi), PARAMETER :: UnDb2 = 86 ! I/O unit for the debugging information, avrSWAP - REAL(ReKi), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. - CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName ! a Fortran version of the input C string (not considered an array here) [subtract 1 for the C null-character] - CHARACTER(200) :: Version ! git version of ROSCO - CHARACTER(10) :: DebugOutStr1, DebugOutStr2, DebugOutStr3, DebugOutStr4, DebugOutStr5, & - DebugOutStr6, DebugOutStr7, DebugOutStr8, DebugOutStr9, DebugOutStr10, & - DebugOutStr11, DebugOutStr12, DebugOutStr13, DebugOutStr14, DebugOutStr15, & - DebugOutStr16, DebugOutStr17, DebugOutStr18, DebugOutStr19, DebugOutStr20 - CHARACTER(10) :: DebugOutUni1, DebugOutUni2, DebugOutUni3, DebugOutUni4, DebugOutUni5, & - DebugOutUni6, DebugOutUni7, DebugOutUni8, DebugOutUni9, DebugOutUni10, & - DebugOutUni11, DebugOutUni12, DebugOutUni13, DebugOutUni14, DebugOutUni15, & - DebugOutUni16, DebugOutUni17, DebugOutUni18, DebugOutUni19, DebugOutUni20 - CHARACTER(10), ALLOCATABLE :: DebugOutStrings(:), DebugOutUnits(:) - REAL(DbKi), ALLOCATABLE :: DebugOutData(:) - - ! Set up Debug Strings and Data - ! Note that Debug strings have 10 character limit - nDebugOuts = 18 - ALLOCATE(DebugOutData(nDebugOuts)) - ! Header Unit Variable - ! Filters - DebugOutStr1 = 'FA_AccF'; DebugOutUni1 = '(rad/s^2)'; DebugOutData(1) = LocalVar%NacIMU_FA_AccF - DebugOutStr2 = 'FA_AccR'; DebugOutUni2 = '(rad/s^2)'; DebugOutData(2) = LocalVar%NacIMU_FA_Acc - DebugOutStr3 = 'RotSpeed'; DebugOutUni3 = '(rad/s)'; DebugOutData(3) = LocalVar%RotSpeed - DebugOutStr4 = 'RotSpeedF'; DebugOutUni4 = '(rad/s)'; DebugOutData(4) = LocalVar%RotSpeedF - DebugOutStr5 = 'GenSpeed'; DebugOutUni5 = '(rad/s)'; DebugOutData(5) = LocalVar%GenSpeed - DebugOutStr6 = 'GenSpeedF'; DebugOutUni6 = '(rad/s)'; DebugOutData(6) = LocalVar%GenSpeedF - ! Floating - DebugOutStr7 = 'FA_Acc'; DebugOutUni7 = '(m/s^2)'; DebugOutData(7) = LocalVar%FA_Acc - DebugOutStr8 = 'Fl_Pitcom'; DebugOutUni8 = '(rad)'; DebugOutData(8) = LocalVar%Fl_Pitcom - DebugOutStr9 = 'PC_MinPit'; DebugOutUni9 = '(rad)'; DebugOutData(9) = LocalVar%PC_MinPit - DebugOutStr10 = 'SS_dOmF'; DebugOutUni10 = '(rad/s)'; DebugOutData(10) = LocalVar%SS_DelOmegaF - ! WSE - DebugOutStr11 = 'WE_Vw'; DebugOutUni11 = '(m/s)'; DebugOutData(11) = LocalVar%WE_Vw - DebugOutStr12 = 'WE_b'; DebugOutUni12 = '(deg)'; DebugOutData(12) = DebugVar%WE_b - DebugOutStr13 = 'WE_t'; DebugOutUni13 = '(Nm)'; DebugOutData(13) = DebugVar%WE_t - DebugOutStr14 = 'WE_w'; DebugOutUni14 = '(rad/s)'; DebugOutData(14) = DebugVar%WE_w - DebugOutStr15 = 'WE_Vm'; DebugOutUni15 = '(m/s)'; DebugOutData(15) = DebugVar%WE_Vm - DebugOutStr16 = 'WE_Vt'; DebugOutUni16 = '(m/s)'; DebugOutData(16) = DebugVar%WE_Vt - DebugOutStr17 = 'WE_lambda'; DebugOutUni17 = '(-)'; DebugOutData(17) = DebugVar%WE_lambda - DebugOutStr18 = 'WE_Cp'; DebugOutUni18 = '(-)'; DebugOutData(18) = DebugVar%WE_Cp - - Allocate(DebugOutStrings(nDebugOuts)) - Allocate(DebugOutUnits(nDebugOuts)) - DebugOutStrings = [CHARACTER(10) :: DebugOutStr1, DebugOutStr2, DebugOutStr3, DebugOutStr4, & - DebugOutStr5, DebugOutStr6, DebugOutStr7, DebugOutStr8, & - DebugOutStr9, DebugOutStr10, DebugOutStr11, DebugOutStr12, & - DebugOutStr13, DebugOutStr14, DebugOutStr15, DebugOutStr16, & - DebugOutStr17, DebugOutStr18] - DebugOutUnits = [CHARACTER(10) :: DebugOutUni1, DebugOutUni2, DebugOutUni3, DebugOutUni4, & - DebugOutUni5, DebugOutUni6, DebugOutUni7, DebugOutUni8, & - DebugOutUni9, DebugOutUni10, DebugOutUni11, DebugOutUni12, & - DebugOutUni13, DebugOutUni14, DebugOutUni15, DebugOutUni1, & - DebugOutUni17, DebugOutUni18] - - ! Initialize debug file - IF (LocalVar%iStatus == 0) THEN ! .TRUE. if we're on the first call to the DLL - ! If we're debugging, open the debug file and write the header: - ! Note that the headers will be Truncated to 10 characters!! - IF (CntrPar%LoggingLevel > 0) THEN - OPEN(unit=UnDb, FILE=RootName(1:size_avcOUTNAME-5)//'RO.dbg') - WRITE (UnDb,*) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version) - WRITE (UnDb,'(99(a10,TR5:))') 'Time', DebugOutStrings - WRITE (UnDb,'(99(a10,TR5:))') '(sec)', DebugOutUnits - END IF - - IF (CntrPar%LoggingLevel > 1) THEN - OPEN(unit=UnDb2, FILE=RootName(1:size_avcOUTNAME-5)//'RO.dbg2') - WRITE(UnDb2,'(/////)') - WRITE(UnDb2,'(A,85("'//Tab//'AvrSWAP(",I2,")"))') 'LocalVar%Time ', (i,i=1,85) - WRITE(UnDb2,'(A,85("'//Tab//'(-)"))') '(s)' - END IF - ELSE - ! Print simulation status, every 10 seconds - IF (MODULO(LocalVar%Time, 10.0_DbKi) == 0) THEN - WRITE(*, 100) LocalVar%GenSpeedF*RPS2RPM, LocalVar%BlPitch(1)*R2D, avrSWAP(15)/1000.0, LocalVar%WE_Vw ! LocalVar%Time !/1000.0 - 100 FORMAT('Generator speed: ', f6.1, ' RPM, Pitch angle: ', f5.1, ' deg, Power: ', f7.1, ' kW, Est. wind Speed: ', f5.1, ' m/s') - END IF - - ENDIF - - ! Write debug files - IF (CntrPar%LoggingLevel > 0) THEN - WRITE (UnDb,FmtDat) LocalVar%Time, DebugOutData - END IF - - IF (CntrPar%LoggingLevel > 1) THEN - WRITE (UnDb2,FmtDat) LocalVar%Time, avrSWAP(1:85) - END IF - - END SUBROUTINE Debug - !------------------------------------------------------------------------------------------------------------------------------- ! Copied from NWTC_IO.f90 From 680034402805c6d89f7ad5a4c00c0ff25e9972aa Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 13:22:17 -0700 Subject: [PATCH 11/40] Save ACC Infile info --- ROSCO/src/ReadSetParameters.f90 | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ROSCO/src/ReadSetParameters.f90 b/ROSCO/src/ReadSetParameters.f90 index 1b03967a..af90de9e 100644 --- a/ROSCO/src/ReadSetParameters.f90 +++ b/ROSCO/src/ReadSetParameters.f90 @@ -82,7 +82,7 @@ SUBROUTINE ReadAvrSWAP(avrSWAP, LocalVar) ELSE LocalVar%restart = .False. ENDIF - + END SUBROUTINE ReadAvrSWAP ! ----------------------------------------------------------------------------------- ! Define parameters for control actions @@ -147,7 +147,12 @@ SUBROUTINE SetParameters(avrSWAP, accINFILE, size_avcMSG, CntrPar, LocalVar, obj 'Developed in collaboration: National Renewable Energy Laboratory '//NEW_LINE('A')// & ' Delft University of Technology, The Netherlands '//NEW_LINE('A')// & '------------------------------------------------------------------------------' + ! Specifically save accINFILE info (DISCON.IN) + LocalVar%ACC_INFILE_SIZE = NINT(avrSWAP(50)) + Allocate(LocalVar%ACC_INFILE(LocalVar%ACC_INFILE_SIZE)) + LocalVar%ACC_INFILE = accINFILE + ! Read Control Parameter File CALL ReadControlParameterFileSub(CntrPar, accINFILE, NINT(avrSWAP(50)),ErrVar) ! If there's been an file reading error, don't continue ! Add RoutineName to error message From 92e27bb929c0823d9805349eaabf855a2c02918a Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Fri, 17 Dec 2021 14:24:27 -0700 Subject: [PATCH 12/40] update for restart capabilities --- ROSCO/rosco_registry/rosco_types.yaml | 281 +++++++++++++++++++++++-- ROSCO/rosco_registry/write_registry.py | 33 ++- ROSCO/src/ROSCO_Types.f90 | 95 +++++++-- 3 files changed, 364 insertions(+), 45 deletions(-) diff --git a/ROSCO/rosco_registry/rosco_types.yaml b/ROSCO/rosco_registry/rosco_types.yaml index 418dee80..9257db0d 100644 --- a/ROSCO/rosco_registry/rosco_types.yaml +++ b/ROSCO/rosco_registry/rosco_types.yaml @@ -8,7 +8,7 @@ default_types: real: &real type: real description: - shape: 1 # Use this if a higher-dimensional allocatable array (shape:2 --> REAL(8), DIMESION(:,:), ALLOCATABLE) + dimension: # Use this if a higher-dimensional allocatable array (dimension:(:,:) --> REAL(8), DIMESION(:,:), ALLOCATABLE) size: 0 # Use this if the type IS an array (size:3 --> REAL(8), BldPitch(3)) allocatable: False equals: @@ -17,6 +17,8 @@ default_types: description: allocatable: False size: 0 + length: + dimension: equals: logical: &logical type: logical @@ -43,6 +45,12 @@ default_types: description: size: 0 equals: + type: &derived_type + type: derived_type + id: + equals: + description: + ControlParameters: @@ -256,6 +264,7 @@ ControlParameters: PerfFileName: <<: *character description: File containing rotor performance tables (Cp,Ct,Cq) + length: 1024 PerfTableSize: <<: *integer description: Size of rotor performance tables, first number refers to number of blade pitch angles, second number referse to number of tip-speed ratios @@ -368,6 +377,7 @@ ControlParameters: OL_Filename: <<: *character description: Input file with open loop timeseries + length: 1024 OL_Mode: <<: *integer description: Open loop control mode {0 - no open loop control, 1 - open loop control vs. time, 2 - open loop control vs. wind speed} @@ -402,7 +412,7 @@ ControlParameters: OL_Channels: <<: *real allocatable: True - shape: 2 + dimension: (:,:) description: Open loop channels in timeseries # Calculated @@ -416,46 +426,235 @@ ControlParameters: <<: *real description: Minimum torque at the beginning of the below-rated region 2, [Nm] +WE: + om_r: + <<: *real + description: Estimated rotor speed [rad/s] + v_t: + <<: *real + description: Estimated wind speed, turbulent component [m/s] + v_m: + <<: *real + description: Estimated wind speed, 10-minute averaged [m/s] + v_h: + <<: *real + description: Combined estimated wind speed [m/s] + P: + <<: *real + dimension: (3,3) + description: Covariance estiamte + xh: + <<: *real + dimension: (3,1) + description: Estimated state matrix + K: + <<: *real + dimension: (3,1) + description: Kalman gain matrix + +FilterParameters: + lpf1_a1: + <<: *real + dimension: (99) + description: First order filter - Denominator coefficient 1 + lpf1_a0: + <<: *real + dimension: (99) + description: First order filter - Denominator coefficient 0 + lpf1_b1: + <<: *real + dimension: (99) + description: First order filter - Numerator coefficient 1 + lpf1_b0: + <<: *real + dimension: (99) + description: First order filter - Numerator coefficient 0 + lpf1_InputSignalLast: + <<: *real + dimension: (99) + description: First order filter - Previous input + lpf1_OutputSignalLast: + <<: *real + dimension: (99) + description: First order filter - Previous output + lpf2_a2: + <<: *real + dimension: (99) + description: Second order filter - Denominator coefficient 2 + lpf2_a1: + <<: *real + dimension: (99) + description: Second order filter - Denominator coefficient 1 + lpf2_a0: + <<: *real + dimension: (99) + description: Second order filter - Denominator coefficient 0 + lpf2_b2: + <<: *real + dimension: (99) + description: Second order filter - Numerator coefficient 2 + lpf2_b1: + <<: *real + dimension: (99) + description: Second order filter - Numerator coefficient 1 + lpf2_b0: + <<: *real + dimension: (99) + description: Second order filter - Numerator coefficient 0 + lpf2_InputSignalLast2: + <<: *real + dimension: (99) + description: Second order filter - Previous input 2 + lpf2_OutputSignalLast2: + <<: *real + dimension: (99) + description: Second order filter - Previous output 2 + lpf2_InputSignalLast1: + <<: *real + dimension: (99) + description: Second order filter - Previous input 1 + lpf2_OutputSignalLast1: + <<: *real + dimension: (99) + description: Second order filter - Previous output 1 + hpf_InputSignalLast: + <<: *real + dimension: (99) + description: High pass filter - Previous output 1 + hpf_OutputSignalLast: + <<: *real + dimension: (99) + description: High pass filter - Previous output 1 + nfs_OutputSignalLast1: + <<: *real + dimension: (99) + description: Notch filter slopes previous output 1 + nfs_OutputSignalLast2: + <<: *real + dimension: (99) + description: Notch filter slopes previous output 2 + nfs_InputSignalLast1: + <<: *real + dimension: (99) + description: Notch filter slopes previous input 1 + nfs_InputSignalLast2: + <<: *real + dimension: (99) + description: Notch filter slopes previous input 1 + nfs_b2: + <<: *real + dimension: (99) + description: Notch filter slopes numerator coefficient 2 + nfs_b0: + <<: *real + dimension: (99) + description: Notch filter slopes numerator coefficient 0 + nfs_a2: + <<: *real + dimension: (99) + description: Notch filter slopes denominator coefficient 2 + nfs_a1: + <<: *real + dimension: (99) + description: Notch filter slopes denominator coefficient 1 + nfs_a0: + <<: *real + dimension: (99) + description: Notch filter slopes denominator coefficient 0 + nf_OutputSignalLast1: + <<: *real + dimension: (99) + description: Notch filter previous output 1 + nf_OutputSignalLast2: + <<: *real + dimension: (99) + description: Notch filter previous output 2 + nf_InputSignalLast1: + <<: *real + dimension: (99) + description: Notch filter previous input 1 + nf_InputSignalLast2: + <<: *real + dimension: (99) + description: Notch filter previous input 2 + nf_b2: + <<: *real + dimension: (99) + description: Notch filter numerator coefficient 2 + nf_b1: + <<: *real + dimension: (99) + description: Notch filter numerator coefficient 1 + nf_b0: + <<: *real + dimension: (99) + description: Notch filter numerator coefficient 0 + nf_a1: + <<: *real + dimension: (99) + description: Notch filter denominator coefficient 1 + nf_a0: + <<: *real + dimension: (99) + description: Notch filter denominator coefficient 0 + +piParams: + ITerm: + <<: *real + dimension: (99) + description: Integrator term + ITermLast: + <<: *real + dimension: (99) + description: Previous integrator term + ITerm2: + <<: *real + dimension: (99) + description: Integrator term - second integrator + ITermLast2: + <<: *real + dimension: (99) + description: Previous integrator term - second integrator LocalVariables: iStatus: <<: *integer - description: + description: Initialization status Time: <<: *real - description: + description: Time [s] DT: <<: *real - description: + description: Time step [s] VS_GenPwr: <<: *real - description: + description: Generator power [W] GenSpeed: <<: *real - description: + description: Generator speed (HSS) [rad/s] RotSpeed: <<: *real - description: + description: Rotor speed (LSS) [rad/s] Y_M: <<: *real - description: + description: Yaw direction [rad] HorWindV: <<: *real - description: + description: Hub height wind speed m/s rootMOOP: <<: *real - description: + description: Blade root bending moment [Nm] size: 3 BlPitch: <<: *real - description: + description: Blade pitch [rad] size: 3 Azimuth: <<: *real - description: + description: Rotor aziumuth angle [rad] NumBl: <<: *integer - description: + description: Number of blades [-] FA_Acc: <<: *real description: Tower fore-aft acceleration [m/s^2] @@ -515,6 +714,9 @@ LocalVariables: PC_PitComT: <<: *real description: Total command pitch based on the sum of the proportional and integral terms [rad]. + PC_PitComT_Last: + <<: *real + description: Last total command pitch based on the sum of the proportional and integral terms [rad]. PC_PitComTF: <<: *real description: Filtered Total command pitch based on the sum of the proportional and integral terms [rad]. @@ -525,12 +727,21 @@ LocalVariables: PC_PwrErr: <<: *real description: Power error with respect to rated power [W] - PC_SineExcitation: - <<: *real - description: Sine contribution to pitch signal PC_SpdErr: <<: *real description: Current speed error (pitch control) [rad/s]. + IPC_IntAxisTilt_1P: + <<: *real + description: Integral of the direct axis, 1P + IPC_IntAxisYaw_1P: + <<: *real + description: Integral of quadrature, 1P + IPC_IntAxisTilt_2P: + <<: *real + description: Integral of the direct axis, 2P + IPC_IntAxisYaw_2P: + <<: *real + description: Integral of quadrature, 2P PC_State: <<: *integer description: State of the pitch control system @@ -615,6 +826,33 @@ LocalVariables: <<: *real description: Flap Angle (rad) size: 3 + RootMyb_Last: + <<: *real + description: Last blade root bending moment (Nm) + size: 3 + ACC_INFILE_SIZE: + <<: *integer + description: Length of parameter input filename + ACC_INFILE: + <<: *character + description: Parameter input filename + dimension: (:) + allocatable: True + restart: + <<: *logical + description: Restart flag + WE: + <<: *derived_type + id: WE + description: Wind speed estimator parameters derived type + FP: + <<: *derived_type + id: FilterParameters + description: Filter parameters derived type + piP: + <<: *derived_type + id: piParams + description: PI parameters derived type ObjectInstances: instLPF: @@ -648,17 +886,17 @@ PerformanceData: Cp_mat: <<: *real allocatable: True - shape: 2 + dimension: (:,:) description: Power coefficient surface Ct_mat: <<: *real allocatable: True - shape: 2 + dimension: (:,:) description: Thrust coefficient surface Cq_mat: <<: *real allocatable: True - shape: 2 + dimension: (:,:) description: Torque coefficient surface DebugVariables: @@ -699,7 +937,6 @@ ErrorVariables: allocatable: True ExtDLL_Type: - FileAddr: <<: *c_intptr_t description: The address of file FileName. (RETURN value from LoadLibrary ) [Windows] @@ -714,9 +951,11 @@ ExtDLL_Type: description: The address of procedure ProcName. (RETURN value from GetProcAddress or dlsym) [initialized to Null for pack/unpack] FileName: <<: *character + length: 1024 description: The name of the DLL file including the full path to the current working directory. ProcName: <<: *character equals: '""' size: 3 + length: 1024 description: The name of the procedure in the DLL that will be called. \ No newline at end of file diff --git a/ROSCO/rosco_registry/write_registry.py b/ROSCO/rosco_registry/write_registry.py index bc3f0188..03fc8b03 100644 --- a/ROSCO/rosco_registry/write_registry.py +++ b/ROSCO/rosco_registry/write_registry.py @@ -29,11 +29,14 @@ def write_registry(yfile): file.close() def check_size(main_attribute, sub_attribute): - size = int(main_attribute[sub_attribute]['size']) - if size == 0: + if main_attribute[sub_attribute]['type'] == 'derived_type': atstr = sub_attribute else: - atstr = sub_attribute + '({})'.format(size) + size = int(main_attribute[sub_attribute]['size']) + if size == 0: + atstr = sub_attribute + else: + atstr = sub_attribute + '({})'.format(size) return atstr def read_type(param): @@ -43,16 +46,24 @@ def read_type(param): f90type += ', DIMENSION(:), ALLOCATABLE' elif param['type'] == 'real': f90type = 'REAL(8)' - if param['allocatable'] and param['shape'] == 1: - f90type += ', DIMENSION(:), ALLOCATABLE' - if param['allocatable'] and param['shape'] == 2: - f90type += ', DIMENSION(:,:), ALLOCATABLE' + if param['allocatable']: + if param['dimension']: + f90type += ', DIMENSION{}, ALLOCATABLE'.format(param['dimension']) + else: + f90type += ', DIMENSION(:), ALLOCATABLE' + elif param['dimension']: + f90type += ', DIMENSION{}'.format(param['dimension']) elif param['type'] == 'character': - f90type = 'CHARACTER(1024)' + f90type = 'CHARACTER' + if param['length']: + f90type += '({})'.format(param['length']) if param['allocatable']: - f90type = 'CHARACTER(:), ALLOCATABLE' + if param['dimension']: + f90type += ', DIMENSION{}, ALLOCATABLE'.format(param['dimension']) + else: + f90type = 'CHARACTER(:), ALLOCATABLE' elif param['type'] == 'logical': - f90type = 'LOGICAL(1)' + f90type = 'LOGICAL' elif param['type'] == 'c_integer': f90type = 'INTEGER(C_INT)' elif param['type'] == 'c_pointer': @@ -61,6 +72,8 @@ def read_type(param): f90type = 'INTEGER(C_INTPTR_T)' elif param['type'] == 'c_funptr': f90type = 'TYPE(C_FUNPTR)' + elif param['type'] == 'derived_type': + f90type = 'TYPE({})'.format(param['id']) else: raise AttributeError('{} does not have a recognizable type'.format(param['type'])) diff --git a/ROSCO/src/ROSCO_Types.f90 b/ROSCO/src/ROSCO_Types.f90 index 94620ffe..fa505a35 100644 --- a/ROSCO/src/ROSCO_Types.f90 +++ b/ROSCO/src/ROSCO_Types.f90 @@ -114,19 +114,75 @@ MODULE ROSCO_Types REAL(8) :: VS_MinOMTq ! Minimum torque at the beginning of the below-rated region 2, [Nm] END TYPE ControlParameters +TYPE, PUBLIC :: WE + REAL(8) :: om_r ! Estimated rotor speed [rad/s] + REAL(8) :: v_t ! Estimated wind speed, turbulent component [m/s] + REAL(8) :: v_m ! Estimated wind speed, 10-minute averaged [m/s] + REAL(8) :: v_h ! Combined estimated wind speed [m/s] + REAL(8), DIMENSION(3,3) :: P ! Covariance estiamte + REAL(8), DIMENSION(3,1) :: xh ! Estimated state matrix + REAL(8), DIMENSION(3,1) :: K ! Kalman gain matrix +END TYPE WE + +TYPE, PUBLIC :: FilterParameters + REAL(8), DIMENSION(99) :: lpf1_a1 ! First order filter - Denominator coefficient 1 + REAL(8), DIMENSION(99) :: lpf1_a0 ! First order filter - Denominator coefficient 0 + REAL(8), DIMENSION(99) :: lpf1_b1 ! First order filter - Numerator coefficient 1 + REAL(8), DIMENSION(99) :: lpf1_b0 ! First order filter - Numerator coefficient 0 + REAL(8), DIMENSION(99) :: lpf1_InputSignalLast ! First order filter - Previous input + REAL(8), DIMENSION(99) :: lpf1_OutputSignalLast ! First order filter - Previous output + REAL(8), DIMENSION(99) :: lpf2_a2 ! Second order filter - Denominator coefficient 2 + REAL(8), DIMENSION(99) :: lpf2_a1 ! Second order filter - Denominator coefficient 1 + REAL(8), DIMENSION(99) :: lpf2_a0 ! Second order filter - Denominator coefficient 0 + REAL(8), DIMENSION(99) :: lpf2_b2 ! Second order filter - Numerator coefficient 2 + REAL(8), DIMENSION(99) :: lpf2_b1 ! Second order filter - Numerator coefficient 1 + REAL(8), DIMENSION(99) :: lpf2_b0 ! Second order filter - Numerator coefficient 0 + REAL(8), DIMENSION(99) :: lpf2_InputSignalLast2 ! Second order filter - Previous input 2 + REAL(8), DIMENSION(99) :: lpf2_OutputSignalLast2 ! Second order filter - Previous output 2 + REAL(8), DIMENSION(99) :: lpf2_InputSignalLast1 ! Second order filter - Previous input 1 + REAL(8), DIMENSION(99) :: lpf2_OutputSignalLast1 ! Second order filter - Previous output 1 + REAL(8), DIMENSION(99) :: hpf_InputSignalLast ! High pass filter - Previous output 1 + REAL(8), DIMENSION(99) :: hpf_OutputSignalLast ! High pass filter - Previous output 1 + REAL(8), DIMENSION(99) :: nfs_OutputSignalLast1 ! Notch filter slopes previous output 1 + REAL(8), DIMENSION(99) :: nfs_OutputSignalLast2 ! Notch filter slopes previous output 2 + REAL(8), DIMENSION(99) :: nfs_InputSignalLast1 ! Notch filter slopes previous input 1 + REAL(8), DIMENSION(99) :: nfs_InputSignalLast2 ! Notch filter slopes previous input 1 + REAL(8), DIMENSION(99) :: nfs_b2 ! Notch filter slopes numerator coefficient 2 + REAL(8), DIMENSION(99) :: nfs_b0 ! Notch filter slopes numerator coefficient 0 + REAL(8), DIMENSION(99) :: nfs_a2 ! Notch filter slopes denominator coefficient 2 + REAL(8), DIMENSION(99) :: nfs_a1 ! Notch filter slopes denominator coefficient 1 + REAL(8), DIMENSION(99) :: nfs_a0 ! Notch filter slopes denominator coefficient 0 + REAL(8), DIMENSION(99) :: nf_OutputSignalLast1 ! Notch filter previous output 1 + REAL(8), DIMENSION(99) :: nf_OutputSignalLast2 ! Notch filter previous output 2 + REAL(8), DIMENSION(99) :: nf_InputSignalLast1 ! Notch filter previous input 1 + REAL(8), DIMENSION(99) :: nf_InputSignalLast2 ! Notch filter previous input 2 + REAL(8), DIMENSION(99) :: nf_b2 ! Notch filter numerator coefficient 2 + REAL(8), DIMENSION(99) :: nf_b1 ! Notch filter numerator coefficient 1 + REAL(8), DIMENSION(99) :: nf_b0 ! Notch filter numerator coefficient 0 + REAL(8), DIMENSION(99) :: nf_a1 ! Notch filter denominator coefficient 1 + REAL(8), DIMENSION(99) :: nf_a0 ! Notch filter denominator coefficient 0 +END TYPE FilterParameters + +TYPE, PUBLIC :: piParams + REAL(8), DIMENSION(99) :: ITerm ! Integrator term + REAL(8), DIMENSION(99) :: ITermLast ! Previous integrator term + REAL(8), DIMENSION(99) :: ITerm2 ! Integrator term - second integrator + REAL(8), DIMENSION(99) :: ITermLast2 ! Previous integrator term - second integrator +END TYPE piParams + TYPE, PUBLIC :: LocalVariables - INTEGER(4) :: iStatus ! None - REAL(8) :: Time ! None - REAL(8) :: DT ! None - REAL(8) :: VS_GenPwr ! None - REAL(8) :: GenSpeed ! None - REAL(8) :: RotSpeed ! None - REAL(8) :: Y_M ! None - REAL(8) :: HorWindV ! None - REAL(8) :: rootMOOP(3) ! None - REAL(8) :: BlPitch(3) ! None - REAL(8) :: Azimuth ! None - INTEGER(4) :: NumBl ! None + INTEGER(4) :: iStatus ! Initialization status + REAL(8) :: Time ! Time [s] + REAL(8) :: DT ! Time step [s] + REAL(8) :: VS_GenPwr ! Generator power [W] + REAL(8) :: GenSpeed ! Generator speed (HSS) [rad/s] + REAL(8) :: RotSpeed ! Rotor speed (LSS) [rad/s] + REAL(8) :: Y_M ! Yaw direction [rad] + REAL(8) :: HorWindV ! Hub height wind speed m/s + REAL(8) :: rootMOOP(3) ! Blade root bending moment [Nm] + REAL(8) :: BlPitch(3) ! Blade pitch [rad] + REAL(8) :: Azimuth ! Rotor aziumuth angle [rad] + INTEGER(4) :: NumBl ! Number of blades [-] REAL(8) :: FA_Acc ! Tower fore-aft acceleration [m/s^2] REAL(8) :: NacIMU_FA_Acc ! Tower fore-aft acceleration [rad/s^2] REAL(8) :: FA_AccHPF ! High-pass filtered fore-aft acceleration [m/s^2] @@ -146,11 +202,15 @@ MODULE ROSCO_Types REAL(8) :: PC_MaxPit ! Maximum pitch setting in pitch controller (variable) [rad]. REAL(8) :: PC_MinPit ! Minimum pitch setting in pitch controller (variable) [rad]. REAL(8) :: PC_PitComT ! Total command pitch based on the sum of the proportional and integral terms [rad]. + REAL(8) :: PC_PitComT_Last ! Last total command pitch based on the sum of the proportional and integral terms [rad]. REAL(8) :: PC_PitComTF ! Filtered Total command pitch based on the sum of the proportional and integral terms [rad]. REAL(8) :: PC_PitComT_IPC(3) ! Total command pitch based on the sum of the proportional and integral terms, including IPC term [rad]. REAL(8) :: PC_PwrErr ! Power error with respect to rated power [W] - REAL(8) :: PC_SineExcitation ! Sine contribution to pitch signal REAL(8) :: PC_SpdErr ! Current speed error (pitch control) [rad/s]. + REAL(8) :: IPC_IntAxisTilt_1P ! Integral of the direct axis, 1P + REAL(8) :: IPC_IntAxisYaw_1P ! Integral of quadrature, 1P + REAL(8) :: IPC_IntAxisTilt_2P ! Integral of the direct axis, 2P + REAL(8) :: IPC_IntAxisYaw_2P ! Integral of quadrature, 2P INTEGER(4) :: PC_State ! State of the pitch control system REAL(8) :: PitCom(3) ! Commanded pitch of each blade the last time the controller was called [rad]. REAL(8) :: SS_DelOmegaF ! Filtered setpoint shifting term defined in setpoint smoother [rad/s]. @@ -174,11 +234,18 @@ MODULE ROSCO_Types REAL(8) :: Y_ErrLPFSlow ! Filtered yaw error by slow low pass filter [rad]. REAL(8) :: Y_MErr ! Measured yaw error, measured + setpoint [rad]. REAL(8) :: Y_YawEndT ! Yaw end time [s]. Indicates the time up until which yaw is active with a fixed rate - LOGICAL(1) :: SD ! Shutdown, .FALSE. if inactive, .TRUE. if active + LOGICAL :: SD ! Shutdown, .FALSE. if inactive, .TRUE. if active REAL(8) :: Fl_PitCom ! Shutdown, .FALSE. if inactive, .TRUE. if active REAL(8) :: NACIMU_FA_AccF ! None REAL(8) :: FA_AccF ! None REAL(8) :: Flp_Angle(3) ! Flap Angle (rad) + REAL(8) :: RootMyb_Last(3) ! Last blade root bending moment (Nm) + INTEGER(4) :: ACC_INFILE_SIZE ! Length of parameter input filename + CHARACTER, DIMENSION(:), ALLOCATABLE :: ACC_INFILE ! Parameter input filename + LOGICAL :: restart ! Restart flag + TYPE(WE) :: WE ! Wind speed estimator parameters derived type + TYPE(FilterParameters) :: FP ! Filter parameters derived type + TYPE(piParams) :: piP ! PI parameters derived type END TYPE LocalVariables TYPE, PUBLIC :: ObjectInstances From 3ff38007f0c2a96c739a1ecfba0295cd92881383 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 08:43:36 -0700 Subject: [PATCH 13/40] add rosco_io with restart and debug functions --- ROSCO/CMakeLists.txt | 1 + ROSCO/rosco_registry/rosco_types.yaml | 8 +- ROSCO/rosco_registry/write_registry.py | 242 ++++++++++++++++++++++++- 3 files changed, 243 insertions(+), 8 deletions(-) diff --git a/ROSCO/CMakeLists.txt b/ROSCO/CMakeLists.txt index 262d232b..c9ad7aa3 100644 --- a/ROSCO/CMakeLists.txt +++ b/ROSCO/CMakeLists.txt @@ -36,6 +36,7 @@ set(SOURCES src/Filters.f90 src/Functions.f90 src/ReadSetParameters.f90 + src/ROSCO_IO.f90 ) if (${CMAKE_Fortran_COMPILER_ID} STREQUAL "GNU") diff --git a/ROSCO/rosco_registry/rosco_types.yaml b/ROSCO/rosco_registry/rosco_types.yaml index 9257db0d..13ebd35b 100644 --- a/ROSCO/rosco_registry/rosco_types.yaml +++ b/ROSCO/rosco_registry/rosco_types.yaml @@ -902,16 +902,16 @@ PerformanceData: DebugVariables: WE_Cp: <<: *real - description: Cp that WSE uses to determine aerodynamic torque[-] + description: Cp that WSE uses to determine aerodynamic torque [-] WE_b: <<: *real - description: Pitch that WSE uses to determine aerodynamic torque[-] + description: Pitch that WSE uses to determine aerodynamic torque [-] WE_w: <<: *real - description: Rotor Speed that WSE uses to determine aerodynamic torque[-] + description: Rotor Speed that WSE uses to determine aerodynamic torque [-] WE_t: <<: *real - description: Torque that WSE uses[-] + description: Torque that WSE uses [-] WE_Vm: <<: *real description: Mean wind speed component in WSE [m/s] diff --git a/ROSCO/rosco_registry/write_registry.py b/ROSCO/rosco_registry/write_registry.py index 03fc8b03..380a26be 100644 --- a/ROSCO/rosco_registry/write_registry.py +++ b/ROSCO/rosco_registry/write_registry.py @@ -3,7 +3,11 @@ import os from ROSCO_toolbox.ofTools.util.FileTools import load_yaml -def write_registry(yfile): +def generate(yfile): + write_types(yfile) + write_roscoio(yfile) + +def write_types(yfile): reg = load_yaml(yfile) reg.pop('default_types') registry_fname = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))),'src','ROSCO_Types.f90') @@ -13,6 +17,7 @@ def write_registry(yfile): file.write('! For any modification to the registry, please edit the rosco_types.yaml accordingly\n \n') file.write('MODULE ROSCO_Types\n') file.write('USE, INTRINSIC :: ISO_C_Binding\n') + file.write('USE Constants\n') file.write('IMPLICIT NONE\n') file.write('\n') for toptype in reg.keys(): @@ -28,6 +33,235 @@ def write_registry(yfile): file.write('END MODULE ROSCO_Types') file.close() +def write_roscoio(yfile): + reg = load_yaml(yfile) + reg.pop('default_types') + registry_fname = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))),'src','ROSCO_IO.f90') + file = open(registry_fname, 'w') + file.write('! ROSCO IO\n') + file.write('! This file is automatically generated by write_registry.py using ROSCO v{}\n'.format(ROSCO_toolbox.__version__)) + file.write('! For any modification to the registry, please edit the rosco_types.yaml accordingly\n \n') + file.write('MODULE ROSCO_IO\n') + file.write(' USE, INTRINSIC :: ISO_C_Binding\n') + file.write(' USE ROSCO_Types\n') + file.write(' USE ReadSetParameters\n') + file.write(' USE Constants\n') + file.write('IMPLICIT NONE\n\n') + file.write('CONTAINS\n\n') + file.write('SUBROUTINE WriteRestartFile(LocalVar, CntrPar, objInst, RootName, size_avcOUTNAME)\n') + file.write(" TYPE(LocalVariables), INTENT(IN) :: LocalVar\n") + file.write(" TYPE(ControlParameters), INTENT(INOUT) :: CntrPar\n") + file.write(" TYPE(ObjectInstances), INTENT(INOUT) :: objInst\n") + file.write(" INTEGER(IntKi), INTENT(IN) :: size_avcOUTNAME\n") + file.write(" CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName \n") + file.write(" \n") + file.write(" INTEGER(IntKi), PARAMETER :: Un = 87 ! I/O unit for pack/unpack (checkpoint & restart)\n") + file.write(" INTEGER(IntKi) :: I ! Generic index.\n") + file.write(" CHARACTER(128) :: InFile ! Input checkpoint file\n") + file.write(" INTEGER(IntKi) :: ErrStat\n") + file.write(" CHARACTER(128) :: ErrMsg \n") + file.write(" CHARACTER(128) :: n_t_global ! timestep number as a string\n") + file.write("\n") + file.write(" WRITE(n_t_global, '(I0.0)' ), NINT(LocalVar%Time/LocalVar%DT)\n") + file.write(" InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp'\n") + file.write(" OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='WRITE' )\n") + file.write("\n") + file.write(" IF ( ErrStat /= 0 ) THEN\n") + file.write(" ErrMsg = 'Cannot open file "'//TRIM( InFile )//'". Another program may have locked it for writing.'\n") + file.write("\n") + file.write(" ELSE\n") + for var in reg['LocalVariables']: + if reg['LocalVariables'][var]['type'] == 'derived_type': + for dvar in reg[reg['LocalVariables'][var]['id']]: + file.write(' WRITE( Un, IOSTAT=ErrStat) LocalVar%{}%{}\n'.format(var,dvar)) + elif reg['LocalVariables'][var]['size'] > 0: + for i in range(reg['LocalVariables'][var]['size']): + file.write(' WRITE( Un, IOSTAT=ErrStat) LocalVar%{}({})\n'.format(var, i+1)) + else: + file.write(' WRITE( Un, IOSTAT=ErrStat) LocalVar%{}\n'.format(var)) + for var in reg['ObjectInstances']: + file.write(' WRITE( Un, IOSTAT=ErrStat) objInst%{}\n'.format(var)) + file.write(' Close ( Un )\n') + file.write(' ENDIF\n') + file.write('END SUBROUTINE WriteRestartFile\n') + file.write('\n \n') + file.write('SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootName, size_avcOUTNAME, ErrVar)\n') + # file.write(' USE ROSCO_Types, ONLY: LocalVariables, ControlParameters, ObjectInstances, PerformanceData, ErrorVariables\n') + file.write(" TYPE(LocalVariables), INTENT(INOUT) :: LocalVar\n") + file.write(" TYPE(ControlParameters), INTENT(INOUT) :: CntrPar\n") + file.write(" TYPE(ObjectInstances), INTENT(INOUT) :: objInst\n") + file.write(" TYPE(PerformanceData), INTENT(INOUT) :: PerfData\n") + file.write(" TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar\n") + file.write(" REAL(C_FLOAT), INTENT(IN) :: avrSWAP(*)\n") + file.write(" INTEGER(IntKi), INTENT(IN) :: size_avcOUTNAME\n") + file.write(" CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName \n") + file.write(" \n") + file.write(" INTEGER(IntKi), PARAMETER :: Un = 87 ! I/O unit for pack/unpack (checkpoint & restart)\n") + file.write(" INTEGER(IntKi) :: I ! Generic index.\n") + file.write(" CHARACTER(128) :: InFile ! Input checkpoint file\n") + file.write(" INTEGER(IntKi) :: ErrStat\n") + file.write(" CHARACTER(128) :: ErrMsg \n") + file.write(" CHARACTER(128) :: n_t_global ! timestep number as a string\n") + file.write("\n") + file.write(" WRITE(n_t_global, '(I0.0)' ), NINT(avrSWAP(2)/avrSWAP(3))\n") + file.write(" InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp'\n") + file.write(" OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='READ' )\n") + file.write("\n") + file.write(" IF ( ErrStat /= 0 ) THEN\n") + file.write(" ErrMsg = 'Cannot open file "'//TRIM( InFile )//'". Another program may have locked it for writing.'\n") + file.write("\n") + file.write(" ELSE\n") + for var in reg['LocalVariables']: + if reg['LocalVariables'][var]['type'] == 'derived_type': + for dvar in reg[reg['LocalVariables'][var]['id']]: + file.write(' READ( Un, IOSTAT=ErrStat) LocalVar%{}%{}\n'.format(var, dvar)) + elif reg['LocalVariables'][var]['size'] > 0: + for i in range(reg['LocalVariables'][var]['size']): + file.write(' READ( Un, IOSTAT=ErrStat) LocalVar%{}({})\n'.format(var, i+1)) + else: + file.write(' READ( Un, IOSTAT=ErrStat) LocalVar%{}\n'.format(var)) + if var == 'ACC_INFILE_SIZE': + file.write(' ALLOCATE(LocalVar%ACC_INFILE(LocalVar%ACC_INFILE_SIZE))\n') + for var in reg['ObjectInstances']: + file.write(' READ( Un, IOSTAT=ErrStat) objInst%{}\n'.format(var)) + file.write(' Close ( Un )\n') + file.write(' ENDIF\n') + file.write(' ! Read Parameter files\n') + file.write(' CALL ReadControlParameterFileSub(CntrPar, LocalVar%ACC_INFILE, LocalVar%ACC_INFILE_SIZE, ErrVar)\n') + file.write(' IF (CntrPar%WE_Mode > 0) THEN\n') + file.write(' CALL READCpFile(CntrPar, PerfData, ErrVar)\n') + file.write(' ENDIF\n') + file.write('END SUBROUTINE ReadRestartFile\n') + file.write('\n \n') + file.write('SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME)\n') + file.write('! Debug routine, defines what gets printed to DEBUG.dbg if LoggingLevel = 1\n') + file.write('\n') + file.write(' TYPE(ControlParameters), INTENT(IN) :: CntrPar\n') + file.write(' TYPE(LocalVariables), INTENT(IN) :: LocalVar\n') + file.write(' TYPE(DebugVariables), INTENT(IN) :: DebugVar\n') + file.write('\n') + file.write(' INTEGER(IntKi), INTENT(IN) :: size_avcOUTNAME\n') + file.write(' INTEGER(IntKi) :: I , nDebugOuts, nLocalVars ! Generic index.\n') + file.write(' CHARACTER(1), PARAMETER :: Tab = CHAR(9) ! The tab character.\n') + file.write(' CHARACTER(29), PARAMETER :: FmtDat = "(F20.5,TR5,99(ES20.5E2,TR5:))" ! The format of the debugging data\n') + file.write(' INTEGER(IntKi), PARAMETER :: UnDb = 85 ! I/O unit for the debugging information\n') + file.write(' INTEGER(IntKi), PARAMETER :: UnDb2 = 86 ! I/O unit for the debugging information, avrSWAP\n') + file.write(' INTEGER(IntKi), PARAMETER :: UnDb3 = 87 ! I/O unit for the debugging information, avrSWAP\n') + file.write(' REAL(ReKi), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller.\n') + file.write(' CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName ! a Fortran version of the input C string (not considered an array here) [subtract 1 for the C null-character]\n') + file.write(' CHARACTER(200) :: Version ! git version of ROSCO\n') + file.write(' CHARACTER(15), ALLOCATABLE :: DebugOutStrings(:), DebugOutUnits(:)\n') + file.write(' REAL(DbKi), ALLOCATABLE :: DebugOutData(:)\n \n') + file.write(' CHARACTER(15), ALLOCATABLE :: LocalVarOutStrings(:)\n') + file.write(' REAL(DbKi), ALLOCATABLE :: LocalVarOutData(:)\n \n') + file.write(' nDebugOuts = {}\n'.format(len(reg['DebugVariables'].keys()))) + file.write(' Allocate(DebugOutData(nDebugOuts))\n') + file.write(' Allocate(DebugOutStrings(nDebugOuts))\n') + file.write(' Allocate(DebugOutUnits(nDebugOuts))\n') + dbg_strings = [] + dbg_units = [] + for dbg_idx, dbgvar in enumerate(reg['DebugVariables']): + dbg_strings.append(dbgvar) + desc = reg['DebugVariables'][dbgvar]['description'] + dbg_units.append(desc[desc.find('['):desc.find(']')+1]) + file.write(' DebugOutData({}) = DebugVar%{}\n'.format(dbg_idx+1,dbgvar)) + file.write(' DebugOutStrings = [CHARACTER(15) :: ') + counter = 0 + for string in dbg_strings: + counter += 1 + if counter == len(dbg_strings): + file.write(" '{}'".format(string)) + else: + file.write(" '{}',".format(string)) + if (counter % 5 == 0): + file.write(' & \n ') + file.write(']\n') + file.write(' DebugOutUnits = [CHARACTER(15) :: ') + counter = 0 + for unit in dbg_units: + counter += 1 + if counter == len(dbg_units): + file.write(" '{}'".format(unit)) + else: + file.write(" '{}',".format(unit)) + if (counter % 5 == 0): + file.write(' & \n ') + file.write(']\n') + lv_strings = [] + for lv_idx, localvar in enumerate(reg['LocalVariables']): + if reg['LocalVariables'][localvar]['type'] in ['integer', 'real']: + lv_strings.append(localvar) + file.write(' nLocalVars = {}\n'.format(len(lv_strings))) + file.write(' Allocate(LocalVarOutData(nLocalVars))\n') + file.write(' Allocate(LocalVarOutStrings(nLocalVars))\n') + for lv_idx, localvar in enumerate(reg['LocalVariables']): + if reg['LocalVariables'][localvar]['type'] in ['integer', 'real']: + if reg['LocalVariables'][localvar]['size'] > 0: + file.write(' LocalVarOutData({}) = LocalVar%{}(1)\n'.format(lv_idx+1, localvar)) + else: + file.write(' LocalVarOutData({}) = LocalVar%{}\n'.format(lv_idx+1, localvar)) + file.write(' LocalVarOutStrings = [CHARACTER(15) :: ') + counter = 0 + for string in lv_strings: + counter += 1 + if counter == len(lv_strings): + file.write(" '{}'".format(string)) + else: + file.write(" '{}',".format(string)) + if (counter % 5 == 0): + file.write(' & \n ') + file.write(']\n') + + file.write(" ! Initialize debug file\n") + file.write(" IF ((LocalVar%iStatus == 0) .OR. (LocalVar%iStatus == -9)) THEN ! .TRUE. if we're on the first call to the DLL\n") + file.write(" ! If we're debugging, open the debug file and write the header:\n") + file.write(" ! Note that the headers will be Truncated to 10 characters!!\n") + file.write(" IF (CntrPar%LoggingLevel > 0) THEN\n") + file.write(" OPEN(unit=UnDb, FILE=RootName(1: size_avcOUTNAME-5)//'RO.dbg')\n") + file.write(" WRITE(UnDb, *) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version)\n") + file.write(" WRITE(UnDb, '(99(a20,TR5:))') 'Time', DebugOutStrings\n") + file.write(" WRITE(UnDb, '(99(a20,TR5:))') '(sec)', DebugOutUnits\n") + file.write(" END IF\n") + file.write("\n") + file.write(" IF (CntrPar%LoggingLevel > 1) THEN\n") + file.write(" OPEN(unit=UnDb2, FILE=RootName(1: size_avcOUTNAME-5)//'RO.dbg2')\n") + file.write(" WRITE(UnDb2, *) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version)\n") + file.write(" WRITE(UnDb2, '(99(a20,TR5:))') 'Time', LocalVarOutStrings\n") + file.write(" WRITE(UnDb2, '(99(a20,TR5:))')\n") + file.write(" END IF\n") + file.write("\n") + file.write(" IF (CntrPar%LoggingLevel > 2) THEN\n") + file.write(" OPEN(unit=UnDb3, FILE=RootName(1: size_avcOUTNAME-5)//'RO.dbg3')\n") + file.write(" WRITE(UnDb3,'(/////)')\n") + file.write(" WRITE(UnDb3,'"+'(A,85("'+"'//Tab//'"+'AvrSWAP("'+',I2,")"'+"))') 'LocalVar%Time ', (i,i=1, 85)\n") + file.write(" WRITE(UnDb3,'"+'(A,85("'+"'//Tab//'"+'(-)"'+"))') '(s)'"+'\n') + file.write(" END IF\n") + file.write(" ELSE\n") + file.write(" ! Print simulation status, every 10 seconds\n") + file.write(" IF (MODULO(LocalVar%Time, 10.0_DbKi) == 0) THEN\n") + file.write(" WRITE(*, 100) LocalVar%GenSpeedF*RPS2RPM, LocalVar%BlPitch(1)*R2D, avrSWAP(15)/1000.0, LocalVar%WE_Vw\n") + file.write(" 100 FORMAT('Generator speed: ', f6.1, ' RPM, Pitch angle: ', f5.1, ' deg, Power: ', f7.1, ' kW, Est. wind Speed: ', f5.1, ' m/s')\n") + file.write(" END IF\n") + file.write("\n") + file.write(" ! Write debug files\n") + file.write(" IF(CntrPar%LoggingLevel > 0) THEN\n") + file.write(" WRITE (UnDb, FmtDat) LocalVar%Time, DebugOutData\n") + file.write(" END IF\n") + file.write("\n") + file.write(" IF(CntrPar%LoggingLevel > 1) THEN\n") + file.write(" WRITE (UnDb2, FmtDat) LocalVar%Time, LocalVarOutData\n") + file.write(" END IF\n") + file.write("\n") + file.write(" IF(CntrPar%LoggingLevel > 2) THEN\n") + file.write(" WRITE (UnDb3, FmtDat) LocalVar%Time, avrSWAP(1: 85)\n") + file.write(" END IF\n") + file.write(" END IF\n") + file.write("\n") + file.write("END SUBROUTINE Debug\n") + file.write("\n") + file.write("END MODULE ROSCO_IO") + file.close() + def check_size(main_attribute, sub_attribute): if main_attribute[sub_attribute]['type'] == 'derived_type': atstr = sub_attribute @@ -41,11 +275,11 @@ def check_size(main_attribute, sub_attribute): def read_type(param): if param['type'] == 'integer': - f90type = 'INTEGER(4)' + f90type = 'INTEGER(IntKi)' if param['allocatable']: f90type += ', DIMENSION(:), ALLOCATABLE' elif param['type'] == 'real': - f90type = 'REAL(8)' + f90type = 'REAL(DbKi)' if param['allocatable']: if param['dimension']: f90type += ', DIMENSION{}, ALLOCATABLE'.format(param['dimension']) @@ -82,5 +316,5 @@ def read_type(param): if __name__ == '__main__': fname = os.path.join(os.path.dirname(os.path.abspath(__file__)),'rosco_types.yaml') - write_registry(fname) + generate(fname) From 15eb73d80566976bff35450d91a8b868acf86f27 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 08:44:41 -0700 Subject: [PATCH 14/40] cleanup debug call --- ROSCO/src/DISCON.F90 | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/ROSCO/src/DISCON.F90 b/ROSCO/src/DISCON.F90 index 1f3b9550..cf9b2c41 100644 --- a/ROSCO/src/DISCON.F90 +++ b/ROSCO/src/DISCON.F90 @@ -93,17 +93,13 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME IF (CntrPar%Flp_Mode > 0) THEN CALL FlapControl(avrSWAP, CntrPar, LocalVar, objInst) END IF - - IF (CntrPar%LoggingLevel > 0) THEN - CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) - END IF ELSEIF ((LocalVar%iStatus == -8) .AND. (ErrVar%aviFAIL >= 0)) THEN ! Write restart files - CALL WriteRestartFile(LocalVar, CntrPar, objInst, RootName, SIZE(avcOUTNAME)) - CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) - + CALL WriteRestartFile(LocalVar, CntrPar, objInst, RootName, SIZE(avcOUTNAME)) ENDIF +CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) + ! Add RoutineName to error message IF (ErrVar%aviFAIL < 0) THEN ErrVar%ErrMsg = RoutineName//':'//TRIM(ErrVar%ErrMsg) @@ -113,7 +109,6 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME avcMSG = TRANSFER(ErrMsg//C_NULL_CHAR, avcMSG, LEN(ErrMsg)+1) avcMSG = TRANSFER(ErrMsg//C_NULL_CHAR, avcMSG, SIZE(avcMSG)) aviFAIL = ErrVar%aviFAIL - ErrVar%ErrMsg = '' RETURN From 5e2eb07b6b6647213e003f16e63cfdcdebd8dcd5 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 08:45:05 -0700 Subject: [PATCH 15/40] use registry generate types and IO --- ROSCO/src/ROSCO_IO.f90 | 977 ++++++++++++++++++++------------------ ROSCO/src/ROSCO_Types.f90 | 479 +++++++++---------- 2 files changed, 745 insertions(+), 711 deletions(-) diff --git a/ROSCO/src/ROSCO_IO.f90 b/ROSCO/src/ROSCO_IO.f90 index 8bc50901..98510c21 100644 --- a/ROSCO/src/ROSCO_IO.f90 +++ b/ROSCO/src/ROSCO_IO.f90 @@ -1,498 +1,531 @@ -! Copyright 2019 NREL - -! Licensed under the Apache License, Version 2.0 (the "License"); you may not use -! this file except in compliance with the License. You may obtain a copy of the -! License at http://www.apache.org/licenses/LICENSE-2.0 - -! Unless required by applicable law or agreed to in writing, software distributed -! under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR -! CONDITIONS OF ANY KIND, either express or implied. See the License for the -! specific language governing permissions and limitations under the License. -! ------------------------------------------------------------------------------------------- -! This handles all of ROSCO's input/output files ~WITH THE EXCPETION~ of reading the primary -! input files. The DISCON.IN and Cp_Ct_Cq.txt files are handed in ReadSetParameters.f90 - +! ROSCO IO +! This file is automatically generated by write_registry.py using ROSCO v2.4.1 +! For any modification to the registry, please edit the rosco_types.yaml accordingly + MODULE ROSCO_IO USE, INTRINSIC :: ISO_C_Binding USE ROSCO_Types USE ReadSetParameters - IMPLICIT NONE -CONTAINS - SUBROUTINE WriteRestartFile(LocalVar, CntrPar, objInst, RootName, size_avcOUTNAME) - USE ROSCO_Types, ONLY : LocalVariables, ObjectInstances, ControlParameters - - TYPE(LocalVariables), INTENT(IN) :: LocalVar - TYPE(ControlParameters), INTENT(INOUT) :: CntrPar - TYPE(ObjectInstances), INTENT(INOUT) :: objInst - INTEGER(4), INTENT(IN) :: size_avcOUTNAME - CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName - - INTEGER(4), PARAMETER :: Un = 87 ! I/O unit for pack/unpack (checkpoint & restart) - INTEGER(4) :: I ! Generic index. - CHARACTER(128) :: InFile ! Input checkpoint file - INTEGER(4) :: ErrStat - CHARACTER(128) :: ErrMsg - CHARACTER(128) :: n_t_global ! timestep number as a string - - WRITE(n_t_global, '(I0.0)' ), NINT(LocalVar%Time/LocalVar%DT) - InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp' - OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='WRITE' ) - - IF ( ErrStat /= 0 ) THEN - ErrMsg = 'Cannot open file "'//TRIM( InFile )//'". Another program may have locked it for writing.' - - ELSE - ! From AVR SWAP - WRITE( Un, IOSTAT=ErrStat) LocalVar%iStatus - WRITE( Un, IOSTAT=ErrStat) LocalVar%Time - WRITE( Un, IOSTAT=ErrStat) LocalVar%DT - WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_GenPwr - WRITE( Un, IOSTAT=ErrStat) LocalVar%GenSpeed - WRITE( Un, IOSTAT=ErrStat) LocalVar%RotSpeed - WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_M - WRITE( Un, IOSTAT=ErrStat) LocalVar%HorWindV - WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(1) - WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(2) - WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(3) - WRITE( Un, IOSTAT=ErrStat) LocalVar%BlPitch(1) - WRITE( Un, IOSTAT=ErrStat) LocalVar%BlPitch(2) - WRITE( Un, IOSTAT=ErrStat) LocalVar%BlPitch(3) - WRITE( Un, IOSTAT=ErrStat) LocalVar%Azimuth - WRITE( Un, IOSTAT=ErrStat) LocalVar%NumBl - WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_Acc - WRITE( Un, IOSTAT=ErrStat) LocalVar%NacIMU_FA_Acc - ! Internal Control Variables - WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPF - WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPFI - WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(1) - WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(2) - WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(3) - WRITE( Un, IOSTAT=ErrStat) LocalVar%RotSpeedF - WRITE( Un, IOSTAT=ErrStat) LocalVar%GenSpeedF - WRITE( Un, IOSTAT=ErrStat) LocalVar%GenTq - WRITE( Un, IOSTAT=ErrStat) LocalVar%GenTqMeas - WRITE( Un, IOSTAT=ErrStat) LocalVar%GenArTq - WRITE( Un, IOSTAT=ErrStat) LocalVar%GenBrTq - WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(1) - WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(2) - WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(3) - WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_1P - WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_1P - WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_2P - WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_2P - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_KP - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_KI - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_KD - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_TF - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_MaxPit - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_MinPit - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_Last - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComTF - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(1) - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(2) - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(3) - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PwrErr - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_SpdErr - WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_State - WRITE( Un, IOSTAT=ErrStat) LocalVar%PitCom(1) - WRITE( Un, IOSTAT=ErrStat) LocalVar%PitCom(2) - WRITE( Un, IOSTAT=ErrStat) LocalVar%PitCom(3) - WRITE( Un, IOSTAT=ErrStat) LocalVar%SS_DelOmegaF - WRITE( Un, IOSTAT=ErrStat) LocalVar%TestType - WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_MaxTq - WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrq - WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenPwr - WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_MechGenPwr - WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrAr - WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrBr - WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErr - WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_State - WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_Rgn3Pitch - WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_Vw - WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_Vw_F - WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_VwI - WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_VwIdot - WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrqF - WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_AccErr - WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFFast - WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFSlow - WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_MErr - WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_YawEndT - WRITE( Un, IOSTAT=ErrStat) LocalVar%SD - WRITE( Un, IOSTAT=ErrStat) LocalVar%Fl_PitCom - WRITE( Un, IOSTAT=ErrStat) LocalVar%NACIMU_FA_AccF - WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_AccF - WRITE( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(1) - WRITE( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(2) - WRITE( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(3) - WRITE( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(1) - WRITE( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(2) - WRITE( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(3) - WRITE( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE_SIZE - WRITE( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE - WRITE( Un, IOSTAT=ErrStat) LocalVar%restart - - WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%om_r - WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%v_t - WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%v_m - WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%v_h - WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%P - WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%xh - WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%K + USE Constants +IMPLICIT NONE - WRITE( Un, IOSTAT=ErrStat) objInst%instLPF - WRITE( Un, IOSTAT=ErrStat) objInst%instSecLPF - WRITE( Un, IOSTAT=ErrStat) objInst%instHPF - WRITE( Un, IOSTAT=ErrStat) objInst%instNotchSlopes - WRITE( Un, IOSTAT=ErrStat) objInst%instNotch - WRITE( Un, IOSTAT=ErrStat) objInst%instPI - - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a1 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a0 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b1 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b0 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_InputSignalLast - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_OutputSignalLast - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a2 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a1 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a0 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b2 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b1 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b0 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast2 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast2 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast1 - WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast1 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%hpf_InputSignalLast - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%hpf_OutputSignalLast - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast1 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast2 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast1 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast2 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_b2 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_b0 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a2 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a1 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a0 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast1 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast2 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast1 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast2 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_b2 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_b1 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_b0 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_a1 - WRITE( un, IOSTAT=ErrStat) LocalVar%FP%nf_a0 - - WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm - WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast - WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm2 - WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast2 - - CLOSE ( Un ) - - ENDIF - - END SUBROUTINE WriteRestartFile - - SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootName, size_avcOUTNAME, ErrVar) - USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, ObjectInstances, PerformanceData, ErrorVariables - - TYPE(LocalVariables), INTENT(INOUT) :: LocalVar - TYPE(ControlParameters), INTENT(INOUT) :: CntrPar - TYPE(ObjectInstances), INTENT(INOUT) :: objInst - TYPE(PerformanceData), INTENT(INOUT) :: PerfData - TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar +CONTAINS +SUBROUTINE WriteRestartFile(LocalVar, CntrPar, objInst, RootName, size_avcOUTNAME) + TYPE(LocalVariables), INTENT(IN) :: LocalVar + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + TYPE(ObjectInstances), INTENT(INOUT) :: objInst + INTEGER(IntKi), INTENT(IN) :: size_avcOUTNAME + CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName + + INTEGER(IntKi), PARAMETER :: Un = 87 ! I/O unit for pack/unpack (checkpoint & restart) + INTEGER(IntKi) :: I ! Generic index. + CHARACTER(128) :: InFile ! Input checkpoint file + INTEGER(IntKi) :: ErrStat + CHARACTER(128) :: ErrMsg + CHARACTER(128) :: n_t_global ! timestep number as a string - REAL(C_FLOAT), INTENT(IN) :: avrSWAP(*) - INTEGER(4), INTENT(IN) :: size_avcOUTNAME - CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName - - INTEGER(4), PARAMETER :: Un = 87 ! I/O unit for pack/unpack (checkpoint & restart) - INTEGER(4) :: I ! Generic index. - INTEGER(4) :: ErrStat - CHARACTER(128) :: ErrMsg - CHARACTER(128) :: n_t_global ! timestep number as a string - CHARACTER(128) :: InFile ! Name of ROSCO checkpoint file + WRITE(n_t_global, '(I0.0)' ), NINT(LocalVar%Time/LocalVar%DT) + InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp' + OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='WRITE' ) - WRITE(n_t_global, '(I0.0)' ), NINT(avrSWAP(2)/avrSWAP(3)) - InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp' - OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='READ' ) + IF ( ErrStat /= 0 ) THEN + ErrMsg = 'Cannot open file //TRIM( InFile )//. Another program may have locked it for writing.' - IF ( ErrStat /= 0 ) THEN - ErrMsg = 'Cannot open file "'//TRIM( InFile )//'". Another program may have locked it for writing.' - - ELSE - ! From AVR SWAP - READ( Un, IOSTAT=ErrStat) LocalVar%iStatus - READ( Un, IOSTAT=ErrStat) LocalVar%Time - READ( Un, IOSTAT=ErrStat) LocalVar%DT - READ( Un, IOSTAT=ErrStat) LocalVar%VS_GenPwr - READ( Un, IOSTAT=ErrStat) LocalVar%GenSpeed - READ( Un, IOSTAT=ErrStat) LocalVar%RotSpeed - READ( Un, IOSTAT=ErrStat) LocalVar%Y_M - READ( Un, IOSTAT=ErrStat) LocalVar%HorWindV - READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(1) - READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(2) - READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(3) - READ( Un, IOSTAT=ErrStat) LocalVar%BlPitch(1) - READ( Un, IOSTAT=ErrStat) LocalVar%BlPitch(2) - READ( Un, IOSTAT=ErrStat) LocalVar%BlPitch(3) - READ( Un, IOSTAT=ErrStat) LocalVar%Azimuth - READ( Un, IOSTAT=ErrStat) LocalVar%NumBl - READ( Un, IOSTAT=ErrStat) LocalVar%FA_Acc - READ( Un, IOSTAT=ErrStat) LocalVar%NacIMU_FA_Acc - ! Internal Control Variables - READ( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPF - READ( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPFI - READ( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(1) - READ( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(2) - READ( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(3) - READ( Un, IOSTAT=ErrStat) LocalVar%RotSpeedF - READ( Un, IOSTAT=ErrStat) LocalVar%GenSpeedF - READ( Un, IOSTAT=ErrStat) LocalVar%GenTq - READ( Un, IOSTAT=ErrStat) LocalVar%GenTqMeas - READ( Un, IOSTAT=ErrStat) LocalVar%GenArTq - READ( Un, IOSTAT=ErrStat) LocalVar%GenBrTq - READ( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(1) - READ( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(2) - READ( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(3) - READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_1P - READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_1P - READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_2P - READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_2P - READ( Un, IOSTAT=ErrStat) LocalVar%PC_KP - READ( Un, IOSTAT=ErrStat) LocalVar%PC_KI - READ( Un, IOSTAT=ErrStat) LocalVar%PC_KD - READ( Un, IOSTAT=ErrStat) LocalVar%PC_TF - READ( Un, IOSTAT=ErrStat) LocalVar%PC_MaxPit - READ( Un, IOSTAT=ErrStat) LocalVar%PC_MinPit - READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT - READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_Last - READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComTF - READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(1) - READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(2) - READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(3) - READ( Un, IOSTAT=ErrStat) LocalVar%PC_PwrErr - READ( Un, IOSTAT=ErrStat) LocalVar%PC_SpdErr - READ( Un, IOSTAT=ErrStat) LocalVar%PC_State - READ( Un, IOSTAT=ErrStat) LocalVar%PitCom(1) - READ( Un, IOSTAT=ErrStat) LocalVar%PitCom(2) - READ( Un, IOSTAT=ErrStat) LocalVar%PitCom(3) - READ( Un, IOSTAT=ErrStat) LocalVar%SS_DelOmegaF - READ( Un, IOSTAT=ErrStat) LocalVar%TestType - READ( Un, IOSTAT=ErrStat) LocalVar%VS_MaxTq - READ( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrq - READ( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenPwr - READ( Un, IOSTAT=ErrStat) LocalVar%VS_MechGenPwr - READ( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrAr - READ( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrBr - READ( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErr - READ( Un, IOSTAT=ErrStat) LocalVar%VS_State - READ( Un, IOSTAT=ErrStat) LocalVar%VS_Rgn3Pitch - READ( Un, IOSTAT=ErrStat) LocalVar%WE_Vw - READ( Un, IOSTAT=ErrStat) LocalVar%WE_Vw_F - READ( Un, IOSTAT=ErrStat) LocalVar%WE_VwI - READ( Un, IOSTAT=ErrStat) LocalVar%WE_VwIdot - READ( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrqF - READ( Un, IOSTAT=ErrStat) LocalVar%Y_AccErr - READ( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFFast - READ( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFSlow - READ( Un, IOSTAT=ErrStat) LocalVar%Y_MErr - READ( Un, IOSTAT=ErrStat) LocalVar%Y_YawEndT - READ( Un, IOSTAT=ErrStat) LocalVar%SD - READ( Un, IOSTAT=ErrStat) LocalVar%Fl_PitCom - READ( Un, IOSTAT=ErrStat) LocalVar%NACIMU_FA_AccF - READ( Un, IOSTAT=ErrStat) LocalVar%FA_AccF - READ( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(1) - READ( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(2) - READ( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(3) - READ( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(1) - READ( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(2) - READ( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(3) - READ( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE_SIZE - ALLOCATE(LocalVar%ACC_INFILE(LocalVar%ACC_INFILE_SIZE)) - READ( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE - READ( Un, IOSTAT=ErrStat) LocalVar%restart + ELSE + WRITE( Un, IOSTAT=ErrStat) LocalVar%iStatus + WRITE( Un, IOSTAT=ErrStat) LocalVar%Time + WRITE( Un, IOSTAT=ErrStat) LocalVar%DT + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_GenPwr + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenSpeed + WRITE( Un, IOSTAT=ErrStat) LocalVar%RotSpeed + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_M + WRITE( Un, IOSTAT=ErrStat) LocalVar%HorWindV + WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%BlPitch(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%BlPitch(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%BlPitch(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%Azimuth + WRITE( Un, IOSTAT=ErrStat) LocalVar%NumBl + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_Acc + WRITE( Un, IOSTAT=ErrStat) LocalVar%NacIMU_FA_Acc + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPF + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPFI + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%RotSpeedF + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenSpeedF + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenTq + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenTqMeas + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenArTq + WRITE( Un, IOSTAT=ErrStat) LocalVar%GenBrTq + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_KP + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_KI + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_KD + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_TF + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_MaxPit + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_MinPit + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_Last + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComTF + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_PwrErr + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_SpdErr + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_1P + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_1P + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_2P + WRITE( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_2P + WRITE( Un, IOSTAT=ErrStat) LocalVar%PC_State + WRITE( Un, IOSTAT=ErrStat) LocalVar%PitCom(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%PitCom(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%PitCom(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%SS_DelOmegaF + WRITE( Un, IOSTAT=ErrStat) LocalVar%TestType + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_MaxTq + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrq + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenPwr + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_MechGenPwr + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrAr + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrBr + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErr + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_State + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_Rgn3Pitch + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_Vw + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_Vw_F + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_VwI + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE_VwIdot + WRITE( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrqF + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_AccErr + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFFast + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFSlow + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_MErr + WRITE( Un, IOSTAT=ErrStat) LocalVar%Y_YawEndT + WRITE( Un, IOSTAT=ErrStat) LocalVar%SD + WRITE( Un, IOSTAT=ErrStat) LocalVar%Fl_PitCom + WRITE( Un, IOSTAT=ErrStat) LocalVar%NACIMU_FA_AccF + WRITE( Un, IOSTAT=ErrStat) LocalVar%FA_AccF + WRITE( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(1) + WRITE( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(2) + WRITE( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(3) + WRITE( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE_SIZE + WRITE( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE + WRITE( Un, IOSTAT=ErrStat) LocalVar%restart + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%om_r + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%v_t + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%v_m + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%v_h + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%P + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%xh + WRITE( Un, IOSTAT=ErrStat) LocalVar%WE%K + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_InputSignalLast + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_OutputSignalLast + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%hpf_InputSignalLast + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%hpf_OutputSignalLast + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_b2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_b0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_a2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_a1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_a0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nf_b2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nf_b1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nf_b0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nf_a1 + WRITE( Un, IOSTAT=ErrStat) LocalVar%FP%nf_a0 + WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm + WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast + WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm2 + WRITE( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast2 + WRITE( Un, IOSTAT=ErrStat) objInst%instLPF + WRITE( Un, IOSTAT=ErrStat) objInst%instSecLPF + WRITE( Un, IOSTAT=ErrStat) objInst%instHPF + WRITE( Un, IOSTAT=ErrStat) objInst%instNotchSlopes + WRITE( Un, IOSTAT=ErrStat) objInst%instNotch + WRITE( Un, IOSTAT=ErrStat) objInst%instPI + Close ( Un ) + ENDIF +END SUBROUTINE WriteRestartFile - READ( Un, IOSTAT=ErrStat) LocalVar%WE%om_r - READ( Un, IOSTAT=ErrStat) LocalVar%WE%v_t - READ( Un, IOSTAT=ErrStat) LocalVar%WE%v_m - READ( Un, IOSTAT=ErrStat) LocalVar%WE%v_h - READ( Un, IOSTAT=ErrStat) LocalVar%WE%P - READ( Un, IOSTAT=ErrStat) LocalVar%WE%xh - READ( Un, IOSTAT=ErrStat) LocalVar%WE%K + +SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootName, size_avcOUTNAME, ErrVar) + TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + TYPE(ControlParameters), INTENT(INOUT) :: CntrPar + TYPE(ObjectInstances), INTENT(INOUT) :: objInst + TYPE(PerformanceData), INTENT(INOUT) :: PerfData + TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar + REAL(C_FLOAT), INTENT(IN) :: avrSWAP(*) + INTEGER(IntKi), INTENT(IN) :: size_avcOUTNAME + CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName + + INTEGER(IntKi), PARAMETER :: Un = 87 ! I/O unit for pack/unpack (checkpoint & restart) + INTEGER(IntKi) :: I ! Generic index. + CHARACTER(128) :: InFile ! Input checkpoint file + INTEGER(IntKi) :: ErrStat + CHARACTER(128) :: ErrMsg + CHARACTER(128) :: n_t_global ! timestep number as a string - READ( Un, IOSTAT=ErrStat) objInst%instLPF - READ( Un, IOSTAT=ErrStat) objInst%instSecLPF - READ( Un, IOSTAT=ErrStat) objInst%instHPF - READ( Un, IOSTAT=ErrStat) objInst%instNotchSlopes - READ( Un, IOSTAT=ErrStat) objInst%instNotch - READ( Un, IOSTAT=ErrStat) objInst%instPI + WRITE(n_t_global, '(I0.0)' ), NINT(avrSWAP(2)/avrSWAP(3)) + InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp' + OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='READ' ) - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a1 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a0 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b1 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b0 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_InputSignalLast - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_OutputSignalLast - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a2 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a1 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a0 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b2 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b1 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b0 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast2 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast2 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast1 - READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast1 - READ( un, IOSTAT=ErrStat) LocalVar%FP%hpf_InputSignalLast - READ( un, IOSTAT=ErrStat) LocalVar%FP%hpf_OutputSignalLast - READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast1 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast2 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast1 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast2 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_b2 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_b0 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a2 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a1 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nfs_a0 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast1 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast2 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast1 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast2 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_b2 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_b1 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_b0 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_a1 - READ( un, IOSTAT=ErrStat) LocalVar%FP%nf_a0 + IF ( ErrStat /= 0 ) THEN + ErrMsg = 'Cannot open file //TRIM( InFile )//. Another program may have locked it for writing.' - READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm - READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast - READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm2 - READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast2 + ELSE + READ( Un, IOSTAT=ErrStat) LocalVar%iStatus + READ( Un, IOSTAT=ErrStat) LocalVar%Time + READ( Un, IOSTAT=ErrStat) LocalVar%DT + READ( Un, IOSTAT=ErrStat) LocalVar%VS_GenPwr + READ( Un, IOSTAT=ErrStat) LocalVar%GenSpeed + READ( Un, IOSTAT=ErrStat) LocalVar%RotSpeed + READ( Un, IOSTAT=ErrStat) LocalVar%Y_M + READ( Un, IOSTAT=ErrStat) LocalVar%HorWindV + READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(1) + READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(2) + READ( Un, IOSTAT=ErrStat) LocalVar%rootMOOP(3) + READ( Un, IOSTAT=ErrStat) LocalVar%BlPitch(1) + READ( Un, IOSTAT=ErrStat) LocalVar%BlPitch(2) + READ( Un, IOSTAT=ErrStat) LocalVar%BlPitch(3) + READ( Un, IOSTAT=ErrStat) LocalVar%Azimuth + READ( Un, IOSTAT=ErrStat) LocalVar%NumBl + READ( Un, IOSTAT=ErrStat) LocalVar%FA_Acc + READ( Un, IOSTAT=ErrStat) LocalVar%NacIMU_FA_Acc + READ( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPF + READ( Un, IOSTAT=ErrStat) LocalVar%FA_AccHPFI + READ( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(1) + READ( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(2) + READ( Un, IOSTAT=ErrStat) LocalVar%FA_PitCom(3) + READ( Un, IOSTAT=ErrStat) LocalVar%RotSpeedF + READ( Un, IOSTAT=ErrStat) LocalVar%GenSpeedF + READ( Un, IOSTAT=ErrStat) LocalVar%GenTq + READ( Un, IOSTAT=ErrStat) LocalVar%GenTqMeas + READ( Un, IOSTAT=ErrStat) LocalVar%GenArTq + READ( Un, IOSTAT=ErrStat) LocalVar%GenBrTq + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(1) + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(2) + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_PitComF(3) + READ( Un, IOSTAT=ErrStat) LocalVar%PC_KP + READ( Un, IOSTAT=ErrStat) LocalVar%PC_KI + READ( Un, IOSTAT=ErrStat) LocalVar%PC_KD + READ( Un, IOSTAT=ErrStat) LocalVar%PC_TF + READ( Un, IOSTAT=ErrStat) LocalVar%PC_MaxPit + READ( Un, IOSTAT=ErrStat) LocalVar%PC_MinPit + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_Last + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComTF + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(1) + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(2) + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PitComT_IPC(3) + READ( Un, IOSTAT=ErrStat) LocalVar%PC_PwrErr + READ( Un, IOSTAT=ErrStat) LocalVar%PC_SpdErr + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_1P + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_1P + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisTilt_2P + READ( Un, IOSTAT=ErrStat) LocalVar%IPC_IntAxisYaw_2P + READ( Un, IOSTAT=ErrStat) LocalVar%PC_State + READ( Un, IOSTAT=ErrStat) LocalVar%PitCom(1) + READ( Un, IOSTAT=ErrStat) LocalVar%PitCom(2) + READ( Un, IOSTAT=ErrStat) LocalVar%PitCom(3) + READ( Un, IOSTAT=ErrStat) LocalVar%SS_DelOmegaF + READ( Un, IOSTAT=ErrStat) LocalVar%TestType + READ( Un, IOSTAT=ErrStat) LocalVar%VS_MaxTq + READ( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrq + READ( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenPwr + READ( Un, IOSTAT=ErrStat) LocalVar%VS_MechGenPwr + READ( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrAr + READ( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErrBr + READ( Un, IOSTAT=ErrStat) LocalVar%VS_SpdErr + READ( Un, IOSTAT=ErrStat) LocalVar%VS_State + READ( Un, IOSTAT=ErrStat) LocalVar%VS_Rgn3Pitch + READ( Un, IOSTAT=ErrStat) LocalVar%WE_Vw + READ( Un, IOSTAT=ErrStat) LocalVar%WE_Vw_F + READ( Un, IOSTAT=ErrStat) LocalVar%WE_VwI + READ( Un, IOSTAT=ErrStat) LocalVar%WE_VwIdot + READ( Un, IOSTAT=ErrStat) LocalVar%VS_LastGenTrqF + READ( Un, IOSTAT=ErrStat) LocalVar%Y_AccErr + READ( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFFast + READ( Un, IOSTAT=ErrStat) LocalVar%Y_ErrLPFSlow + READ( Un, IOSTAT=ErrStat) LocalVar%Y_MErr + READ( Un, IOSTAT=ErrStat) LocalVar%Y_YawEndT + READ( Un, IOSTAT=ErrStat) LocalVar%SD + READ( Un, IOSTAT=ErrStat) LocalVar%Fl_PitCom + READ( Un, IOSTAT=ErrStat) LocalVar%NACIMU_FA_AccF + READ( Un, IOSTAT=ErrStat) LocalVar%FA_AccF + READ( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(1) + READ( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(2) + READ( Un, IOSTAT=ErrStat) LocalVar%Flp_Angle(3) + READ( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(1) + READ( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(2) + READ( Un, IOSTAT=ErrStat) LocalVar%RootMyb_Last(3) + READ( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE_SIZE + ALLOCATE(LocalVar%ACC_INFILE(LocalVar%ACC_INFILE_SIZE)) + READ( Un, IOSTAT=ErrStat) LocalVar%ACC_INFILE + READ( Un, IOSTAT=ErrStat) LocalVar%restart + READ( Un, IOSTAT=ErrStat) LocalVar%WE%om_r + READ( Un, IOSTAT=ErrStat) LocalVar%WE%v_t + READ( Un, IOSTAT=ErrStat) LocalVar%WE%v_m + READ( Un, IOSTAT=ErrStat) LocalVar%WE%v_h + READ( Un, IOSTAT=ErrStat) LocalVar%WE%P + READ( Un, IOSTAT=ErrStat) LocalVar%WE%xh + READ( Un, IOSTAT=ErrStat) LocalVar%WE%K + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_a0 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_b0 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_InputSignalLast + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf1_OutputSignalLast + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_a0 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_b0 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_InputSignalLast1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%lpf2_OutputSignalLast1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%hpf_InputSignalLast + READ( Un, IOSTAT=ErrStat) LocalVar%FP%hpf_OutputSignalLast + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_OutputSignalLast2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_InputSignalLast2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_b2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_b0 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_a2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_a1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nfs_a0 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nf_OutputSignalLast2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nf_InputSignalLast2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nf_b2 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nf_b1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nf_b0 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nf_a1 + READ( Un, IOSTAT=ErrStat) LocalVar%FP%nf_a0 + READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm + READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast + READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITerm2 + READ( Un, IOSTAT=ErrStat) LocalVar%piP%ITermLast2 + READ( Un, IOSTAT=ErrStat) objInst%instLPF + READ( Un, IOSTAT=ErrStat) objInst%instSecLPF + READ( Un, IOSTAT=ErrStat) objInst%instHPF + READ( Un, IOSTAT=ErrStat) objInst%instNotchSlopes + READ( Un, IOSTAT=ErrStat) objInst%instNotch + READ( Un, IOSTAT=ErrStat) objInst%instPI + Close ( Un ) + ENDIF + ! Read Parameter files + CALL ReadControlParameterFileSub(CntrPar, LocalVar%ACC_INFILE, LocalVar%ACC_INFILE_SIZE, ErrVar) + IF (CntrPar%WE_Mode > 0) THEN + CALL READCpFile(CntrPar, PerfData, ErrVar) + ENDIF +END SUBROUTINE ReadRestartFile - CLOSE ( Un ) - ENDIF + +SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME) +! Debug routine, defines what gets printed to DEBUG.dbg if LoggingLevel = 1 - ! Read Parameter files - CALL ReadControlParameterFileSub(CntrPar, LocalVar%ACC_INFILE, LocalVar%ACC_INFILE_SIZE, ErrVar) - IF (CntrPar%WE_Mode > 0) THEN - CALL READCpFile(CntrPar, PerfData, ErrVar) - ENDIF - - END SUBROUTINE ReadRestartFile + TYPE(ControlParameters), INTENT(IN) :: CntrPar + TYPE(LocalVariables), INTENT(IN) :: LocalVar + TYPE(DebugVariables), INTENT(IN) :: DebugVar -!------------------------------------------------------------------------------------------------------------------------------- - SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME) - ! Debug routine, defines what gets printed to DEBUG.dbg if LoggingLevel = 1 - - USE, INTRINSIC :: ISO_C_Binding - USE ROSCO_Types, ONLY : LocalVariables, ControlParameters, DebugVariables - - IMPLICIT NONE - - TYPE(ControlParameters), INTENT(IN) :: CntrPar - TYPE(LocalVariables), INTENT(IN) :: LocalVar - TYPE(DebugVariables), INTENT(IN) :: DebugVar - - INTEGER(IntKi), INTENT(IN) :: size_avcOUTNAME - INTEGER(IntKi) :: I , nDebugOuts ! Generic index. - CHARACTER(1), PARAMETER :: Tab = CHAR(9) ! The tab character. - CHARACTER(29), PARAMETER :: FmtDat = "(F10.3,TR5,99(ES10.3E2,TR5:))" ! The format of the debugging data - INTEGER(IntKi), PARAMETER :: UnDb = 85 ! I/O unit for the debugging information - INTEGER(IntKi), PARAMETER :: UnDb2 = 86 ! I/O unit for the debugging information, avrSWAP - REAL(ReKi), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. - CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName ! a Fortran version of the input C string (not considered an array here) [subtract 1 for the C null-character] - CHARACTER(200) :: Version ! git version of ROSCO - CHARACTER(10) :: DebugOutStr1, DebugOutStr2, DebugOutStr3, DebugOutStr4, DebugOutStr5, & - DebugOutStr6, DebugOutStr7, DebugOutStr8, DebugOutStr9, DebugOutStr10, & - DebugOutStr11, DebugOutStr12, DebugOutStr13, DebugOutStr14, DebugOutStr15, & - DebugOutStr16, DebugOutStr17, DebugOutStr18, DebugOutStr19, DebugOutStr20 - CHARACTER(10) :: DebugOutUni1, DebugOutUni2, DebugOutUni3, DebugOutUni4, DebugOutUni5, & - DebugOutUni6, DebugOutUni7, DebugOutUni8, DebugOutUni9, DebugOutUni10, & - DebugOutUni11, DebugOutUni12, DebugOutUni13, DebugOutUni14, DebugOutUni15, & - DebugOutUni16, DebugOutUni17, DebugOutUni18, DebugOutUni19, DebugOutUni20 - CHARACTER(10), ALLOCATABLE :: DebugOutStrings(:), DebugOutUnits(:) - REAL(DbKi), ALLOCATABLE :: DebugOutData(:) + INTEGER(IntKi), INTENT(IN) :: size_avcOUTNAME + INTEGER(IntKi) :: I , nDebugOuts, nLocalVars ! Generic index. + CHARACTER(1), PARAMETER :: Tab = CHAR(9) ! The tab character. + CHARACTER(29), PARAMETER :: FmtDat = "(F20.5,TR5,99(ES20.5E2,TR5:))" ! The format of the debugging data + INTEGER(IntKi), PARAMETER :: UnDb = 85 ! I/O unit for the debugging information + INTEGER(IntKi), PARAMETER :: UnDb2 = 86 ! I/O unit for the debugging information, avrSWAP + INTEGER(IntKi), PARAMETER :: UnDb3 = 87 ! I/O unit for the debugging information, avrSWAP + REAL(ReKi), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller. + CHARACTER(size_avcOUTNAME-1), INTENT(IN) :: RootName ! a Fortran version of the input C string (not considered an array here) [subtract 1 for the C null-character] + CHARACTER(200) :: Version ! git version of ROSCO + CHARACTER(15), ALLOCATABLE :: DebugOutStrings(:), DebugOutUnits(:) + REAL(DbKi), ALLOCATABLE :: DebugOutData(:) + + CHARACTER(15), ALLOCATABLE :: LocalVarOutStrings(:) + REAL(DbKi), ALLOCATABLE :: LocalVarOutData(:) + + nDebugOuts = 8 + Allocate(DebugOutData(nDebugOuts)) + Allocate(DebugOutStrings(nDebugOuts)) + Allocate(DebugOutUnits(nDebugOuts)) + DebugOutData(1) = DebugVar%WE_Cp + DebugOutData(2) = DebugVar%WE_b + DebugOutData(3) = DebugVar%WE_w + DebugOutData(4) = DebugVar%WE_t + DebugOutData(5) = DebugVar%WE_Vm + DebugOutData(6) = DebugVar%WE_Vt + DebugOutData(7) = DebugVar%WE_lambda + DebugOutData(8) = DebugVar%PC_PICommand + DebugOutStrings = [CHARACTER(15) :: 'WE_Cp', 'WE_b', 'WE_w', 'WE_t', 'WE_Vm', & + 'WE_Vt', 'WE_lambda', 'PC_PICommand'] + DebugOutUnits = [CHARACTER(15) :: '[-]', '[-]', '[-]', '[-]', '[m/s]', & + '[m/s]', '[rad]', '[rad]'] + nLocalVars = 69 + Allocate(LocalVarOutData(nLocalVars)) + Allocate(LocalVarOutStrings(nLocalVars)) + LocalVarOutData(1) = LocalVar%iStatus + LocalVarOutData(2) = LocalVar%Time + LocalVarOutData(3) = LocalVar%DT + LocalVarOutData(4) = LocalVar%VS_GenPwr + LocalVarOutData(5) = LocalVar%GenSpeed + LocalVarOutData(6) = LocalVar%RotSpeed + LocalVarOutData(7) = LocalVar%Y_M + LocalVarOutData(8) = LocalVar%HorWindV + LocalVarOutData(9) = LocalVar%rootMOOP(1) + LocalVarOutData(10) = LocalVar%BlPitch(1) + LocalVarOutData(11) = LocalVar%Azimuth + LocalVarOutData(12) = LocalVar%NumBl + LocalVarOutData(13) = LocalVar%FA_Acc + LocalVarOutData(14) = LocalVar%NacIMU_FA_Acc + LocalVarOutData(15) = LocalVar%FA_AccHPF + LocalVarOutData(16) = LocalVar%FA_AccHPFI + LocalVarOutData(17) = LocalVar%FA_PitCom(1) + LocalVarOutData(18) = LocalVar%RotSpeedF + LocalVarOutData(19) = LocalVar%GenSpeedF + LocalVarOutData(20) = LocalVar%GenTq + LocalVarOutData(21) = LocalVar%GenTqMeas + LocalVarOutData(22) = LocalVar%GenArTq + LocalVarOutData(23) = LocalVar%GenBrTq + LocalVarOutData(24) = LocalVar%IPC_PitComF(1) + LocalVarOutData(25) = LocalVar%PC_KP + LocalVarOutData(26) = LocalVar%PC_KI + LocalVarOutData(27) = LocalVar%PC_KD + LocalVarOutData(28) = LocalVar%PC_TF + LocalVarOutData(29) = LocalVar%PC_MaxPit + LocalVarOutData(30) = LocalVar%PC_MinPit + LocalVarOutData(31) = LocalVar%PC_PitComT + LocalVarOutData(32) = LocalVar%PC_PitComT_Last + LocalVarOutData(33) = LocalVar%PC_PitComTF + LocalVarOutData(34) = LocalVar%PC_PitComT_IPC(1) + LocalVarOutData(35) = LocalVar%PC_PwrErr + LocalVarOutData(36) = LocalVar%PC_SpdErr + LocalVarOutData(37) = LocalVar%IPC_IntAxisTilt_1P + LocalVarOutData(38) = LocalVar%IPC_IntAxisYaw_1P + LocalVarOutData(39) = LocalVar%IPC_IntAxisTilt_2P + LocalVarOutData(40) = LocalVar%IPC_IntAxisYaw_2P + LocalVarOutData(41) = LocalVar%PC_State + LocalVarOutData(42) = LocalVar%PitCom(1) + LocalVarOutData(43) = LocalVar%SS_DelOmegaF + LocalVarOutData(44) = LocalVar%TestType + LocalVarOutData(45) = LocalVar%VS_MaxTq + LocalVarOutData(46) = LocalVar%VS_LastGenTrq + LocalVarOutData(47) = LocalVar%VS_LastGenPwr + LocalVarOutData(48) = LocalVar%VS_MechGenPwr + LocalVarOutData(49) = LocalVar%VS_SpdErrAr + LocalVarOutData(50) = LocalVar%VS_SpdErrBr + LocalVarOutData(51) = LocalVar%VS_SpdErr + LocalVarOutData(52) = LocalVar%VS_State + LocalVarOutData(53) = LocalVar%VS_Rgn3Pitch + LocalVarOutData(54) = LocalVar%WE_Vw + LocalVarOutData(55) = LocalVar%WE_Vw_F + LocalVarOutData(56) = LocalVar%WE_VwI + LocalVarOutData(57) = LocalVar%WE_VwIdot + LocalVarOutData(58) = LocalVar%VS_LastGenTrqF + LocalVarOutData(59) = LocalVar%Y_AccErr + LocalVarOutData(60) = LocalVar%Y_ErrLPFFast + LocalVarOutData(61) = LocalVar%Y_ErrLPFSlow + LocalVarOutData(62) = LocalVar%Y_MErr + LocalVarOutData(63) = LocalVar%Y_YawEndT + LocalVarOutData(65) = LocalVar%Fl_PitCom + LocalVarOutData(66) = LocalVar%NACIMU_FA_AccF + LocalVarOutData(67) = LocalVar%FA_AccF + LocalVarOutData(68) = LocalVar%Flp_Angle(1) + LocalVarOutData(69) = LocalVar%RootMyb_Last(1) + LocalVarOutData(70) = LocalVar%ACC_INFILE_SIZE + LocalVarOutStrings = [CHARACTER(15) :: 'iStatus', 'Time', 'DT', 'VS_GenPwr', 'GenSpeed', & + 'RotSpeed', 'Y_M', 'HorWindV', 'rootMOOP', 'BlPitch', & + 'Azimuth', 'NumBl', 'FA_Acc', 'NacIMU_FA_Acc', 'FA_AccHPF', & + 'FA_AccHPFI', 'FA_PitCom', 'RotSpeedF', 'GenSpeedF', 'GenTq', & + 'GenTqMeas', 'GenArTq', 'GenBrTq', 'IPC_PitComF', 'PC_KP', & + 'PC_KI', 'PC_KD', 'PC_TF', 'PC_MaxPit', 'PC_MinPit', & + 'PC_PitComT', 'PC_PitComT_Last', 'PC_PitComTF', 'PC_PitComT_IPC', 'PC_PwrErr', & + 'PC_SpdErr', 'IPC_IntAxisTilt_1P', 'IPC_IntAxisYaw_1P', 'IPC_IntAxisTilt_2P', 'IPC_IntAxisYaw_2P', & + 'PC_State', 'PitCom', 'SS_DelOmegaF', 'TestType', 'VS_MaxTq', & + 'VS_LastGenTrq', 'VS_LastGenPwr', 'VS_MechGenPwr', 'VS_SpdErrAr', 'VS_SpdErrBr', & + 'VS_SpdErr', 'VS_State', 'VS_Rgn3Pitch', 'WE_Vw', 'WE_Vw_F', & + 'WE_VwI', 'WE_VwIdot', 'VS_LastGenTrqF', 'Y_AccErr', 'Y_ErrLPFFast', & + 'Y_ErrLPFSlow', 'Y_MErr', 'Y_YawEndT', 'Fl_PitCom', 'NACIMU_FA_AccF', & + 'FA_AccF', 'Flp_Angle', 'RootMyb_Last', 'ACC_INFILE_SIZE'] + ! Initialize debug file + IF ((LocalVar%iStatus == 0) .OR. (LocalVar%iStatus == -9)) THEN ! .TRUE. if we're on the first call to the DLL + ! If we're debugging, open the debug file and write the header: + ! Note that the headers will be Truncated to 10 characters!! + IF (CntrPar%LoggingLevel > 0) THEN + OPEN(unit=UnDb, FILE=RootName(1: size_avcOUTNAME-5)//'RO.dbg') + WRITE(UnDb, *) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version) + WRITE(UnDb, '(99(a20,TR5:))') 'Time', DebugOutStrings + WRITE(UnDb, '(99(a20,TR5:))') '(sec)', DebugOutUnits + END IF - ! Set up Debug Strings and Data - ! Note that Debug strings have 10 character limit - nDebugOuts = 18 - ALLOCATE(DebugOutData(nDebugOuts)) - ! Header Unit Variable - ! Filters - DebugOutStr1 = 'FA_AccF'; DebugOutUni1 = '(rad/s^2)'; DebugOutData(1) = LocalVar%NacIMU_FA_AccF - DebugOutStr2 = 'FA_AccR'; DebugOutUni2 = '(rad/s^2)'; DebugOutData(2) = LocalVar%NacIMU_FA_Acc - DebugOutStr3 = 'RotSpeed'; DebugOutUni3 = '(rad/s)'; DebugOutData(3) = LocalVar%RotSpeed - DebugOutStr4 = 'RotSpeedF'; DebugOutUni4 = '(rad/s)'; DebugOutData(4) = LocalVar%RotSpeedF - DebugOutStr5 = 'GenSpeed'; DebugOutUni5 = '(rad/s)'; DebugOutData(5) = LocalVar%GenSpeed - DebugOutStr6 = 'GenSpeedF'; DebugOutUni6 = '(rad/s)'; DebugOutData(6) = LocalVar%GenSpeedF - ! Floating - DebugOutStr7 = 'FA_Acc'; DebugOutUni7 = '(m/s^2)'; DebugOutData(7) = LocalVar%FA_Acc - DebugOutStr8 = 'Fl_Pitcom'; DebugOutUni8 = '(rad)'; DebugOutData(8) = LocalVar%Fl_Pitcom - DebugOutStr9 = 'PC_MinPit'; DebugOutUni9 = '(rad)'; DebugOutData(9) = LocalVar%PC_MinPit - DebugOutStr10 = 'SS_dOmF'; DebugOutUni10 = '(rad/s)'; DebugOutData(10) = LocalVar%SS_DelOmegaF - ! WSE - DebugOutStr11 = 'WE_Vw'; DebugOutUni11 = '(m/s)'; DebugOutData(11) = LocalVar%WE_Vw - DebugOutStr12 = 'WE_b'; DebugOutUni12 = '(deg)'; DebugOutData(12) = DebugVar%WE_b - DebugOutStr13 = 'WE_t'; DebugOutUni13 = '(Nm)'; DebugOutData(13) = DebugVar%WE_t - DebugOutStr14 = 'WE_w'; DebugOutUni14 = '(rad/s)'; DebugOutData(14) = DebugVar%WE_w - DebugOutStr15 = 'WE_Vm'; DebugOutUni15 = '(m/s)'; DebugOutData(15) = DebugVar%WE_Vm - DebugOutStr16 = 'WE_Vt'; DebugOutUni16 = '(m/s)'; DebugOutData(16) = DebugVar%WE_Vt - DebugOutStr17 = 'WE_lambda'; DebugOutUni17 = '(-)'; DebugOutData(17) = DebugVar%WE_lambda - DebugOutStr18 = 'WE_Cp'; DebugOutUni18 = '(-)'; DebugOutData(18) = DebugVar%WE_Cp + IF (CntrPar%LoggingLevel > 1) THEN + OPEN(unit=UnDb2, FILE=RootName(1: size_avcOUTNAME-5)//'RO.dbg2') + WRITE(UnDb2, *) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version) + WRITE(UnDb2, '(99(a20,TR5:))') 'Time', LocalVarOutStrings + WRITE(UnDb2, '(99(a20,TR5:))') + END IF - Allocate(DebugOutStrings(nDebugOuts)) - Allocate(DebugOutUnits(nDebugOuts)) - DebugOutStrings = [CHARACTER(10) :: DebugOutStr1, DebugOutStr2, DebugOutStr3, DebugOutStr4, & - DebugOutStr5, DebugOutStr6, DebugOutStr7, DebugOutStr8, & - DebugOutStr9, DebugOutStr10, DebugOutStr11, DebugOutStr12, & - DebugOutStr13, DebugOutStr14, DebugOutStr15, DebugOutStr16, & - DebugOutStr17, DebugOutStr18] - DebugOutUnits = [CHARACTER(10) :: DebugOutUni1, DebugOutUni2, DebugOutUni3, DebugOutUni4, & - DebugOutUni5, DebugOutUni6, DebugOutUni7, DebugOutUni8, & - DebugOutUni9, DebugOutUni10, DebugOutUni11, DebugOutUni12, & - DebugOutUni13, DebugOutUni14, DebugOutUni15, DebugOutUni1, & - DebugOutUni17, DebugOutUni18] - - ! Initialize debug file - IF ((LocalVar%iStatus == 0) .OR. (LocalVar%iStatus == -8)) THEN ! .TRUE. if we're on the first call to the DLL - ! If we're debugging, open the debug file and write the header: - ! Note that the headers will be Truncated to 10 characters!! - IF (CntrPar%LoggingLevel > 0) THEN - OPEN(unit=UnDb, FILE=RootName(1:size_avcOUTNAME-5)//'RO.dbg') - WRITE (UnDb,*) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version) - WRITE (UnDb,'(99(a10,TR5:))') 'Time', DebugOutStrings - WRITE (UnDb,'(99(a10,TR5:))') '(sec)', DebugOutUnits - END IF - - IF (CntrPar%LoggingLevel > 1) THEN - OPEN(unit=UnDb2, FILE=RootName(1:size_avcOUTNAME-5)//'RO.dbg2') - WRITE(UnDb2,'(/////)') - WRITE(UnDb2,'(A,85("'//Tab//'AvrSWAP(",I2,")"))') 'LocalVar%Time ', (i,i=1,85) - WRITE(UnDb2,'(A,85("'//Tab//'(-)"))') '(s)' - END IF - ELSE - ! Print simulation status, every 10 seconds - IF (MODULO(LocalVar%Time, 10.0_DbKi) == 0) THEN - WRITE(*, 100) LocalVar%GenSpeedF*RPS2RPM, LocalVar%BlPitch(1)*R2D, avrSWAP(15)/1000.0, LocalVar%WE_Vw ! LocalVar%Time !/1000.0 - 100 FORMAT('Generator speed: ', f6.1, ' RPM, Pitch angle: ', f5.1, ' deg, Power: ', f7.1, ' kW, Est. wind Speed: ', f5.1, ' m/s') - END IF - - ENDIF + IF (CntrPar%LoggingLevel > 2) THEN + OPEN(unit=UnDb3, FILE=RootName(1: size_avcOUTNAME-5)//'RO.dbg3') + WRITE(UnDb3,'(/////)') + WRITE(UnDb3,'(A,85("'//Tab//'AvrSWAP(",I2,")"))') 'LocalVar%Time ', (i,i=1, 85) + WRITE(UnDb3,'(A,85("'//Tab//'(-)"))') '(s)' + END IF + ELSE + ! Print simulation status, every 10 seconds + IF (MODULO(LocalVar%Time, 10.0_DbKi) == 0) THEN + WRITE(*, 100) LocalVar%GenSpeedF*RPS2RPM, LocalVar%BlPitch(1)*R2D, avrSWAP(15)/1000.0, LocalVar%WE_Vw + 100 FORMAT('Generator speed: ', f6.1, ' RPM, Pitch angle: ', f5.1, ' deg, Power: ', f7.1, ' kW, Est. wind Speed: ', f5.1, ' m/s') + END IF ! Write debug files - IF (CntrPar%LoggingLevel > 0) THEN - WRITE (UnDb,FmtDat) LocalVar%Time, DebugOutData + IF(CntrPar%LoggingLevel > 0) THEN + WRITE (UnDb, FmtDat) LocalVar%Time, DebugOutData END IF - IF (CntrPar%LoggingLevel > 1) THEN - WRITE (UnDb2,FmtDat) LocalVar%Time, avrSWAP(1:85) + IF(CntrPar%LoggingLevel > 1) THEN + WRITE (UnDb2, FmtDat) LocalVar%Time, LocalVarOutData + END IF + + IF(CntrPar%LoggingLevel > 2) THEN + WRITE (UnDb3, FmtDat) LocalVar%Time, avrSWAP(1: 85) END IF + END IF - END SUBROUTINE Debug +END SUBROUTINE Debug END MODULE ROSCO_IO \ No newline at end of file diff --git a/ROSCO/src/ROSCO_Types.f90 b/ROSCO/src/ROSCO_Types.f90 index fa505a35..80b0c50f 100644 --- a/ROSCO/src/ROSCO_Types.f90 +++ b/ROSCO/src/ROSCO_Types.f90 @@ -4,243 +4,244 @@ MODULE ROSCO_Types USE, INTRINSIC :: ISO_C_Binding +USE Constants 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 - INTEGER(4) :: F_LPFType ! Low pass filter on the rotor and generator speed {1 - first-order low-pass filter, 2 - second-order low-pass filter}, [rad/s] - INTEGER(4) :: F_NotchType ! Notch on the measured generator speed {0 - disable, 1 - enable} - REAL(8) :: F_LPFCornerFreq ! Corner frequency (-3dB point) in the first-order low-pass filter, [rad/s] - REAL(8) :: F_LPFDamping ! Damping coefficient [used only when F_FilterType = 2] - REAL(8) :: F_NotchCornerFreq ! Natural frequency of the notch filter, [rad/s] - REAL(8), DIMENSION(:), ALLOCATABLE :: F_NotchBetaNumDen ! These two notch damping values (numerator and denominator) determines the width and depth of the notch - REAL(8) :: F_SSCornerFreq ! Corner frequency (-3dB point) in the first order low pass filter for the setpoint smoother [rad/s] - REAL(8) :: F_WECornerFreq ! Corner frequency (-3dB point) in the first order low pass filter for the wind speed estimate [rad/s] - REAL(8), DIMENSION(:), ALLOCATABLE :: F_FlCornerFreq ! Corner frequency (-3dB point) in the second order low pass filter of the tower-top fore-aft motion for floating feedback control [rad/s]. - REAL(8) :: F_FlHighPassFreq ! Natural frequency of first-roder high-pass filter for nacelle fore-aft motion [rad/s]. - REAL(8), DIMENSION(:), ALLOCATABLE :: F_FlpCornerFreq ! Corner frequency (-3dB point) in the second order low pass filter of the blade root bending moment for flap control [rad/s]. - REAL(8) :: FA_HPFCornerFreq ! Corner frequency (-3dB point) in the high-pass filter on the fore-aft acceleration signal [rad/s] - REAL(8) :: FA_IntSat ! Integrator saturation (maximum signal amplitude contrbution to pitch from FA damper), [rad] - REAL(8) :: FA_KI ! Integral gain for the fore-aft tower damper controller, -1 = off / >0 = on [rad s/m] - INTEGER(4) :: IPC_ControlMode ! Turn Individual Pitch Control (IPC) for fatigue load reductions (pitch contribution) {0 - off, 1 - 1P reductions, 2 - 1P+2P reductions} - REAL(8) :: IPC_IntSat ! Integrator saturation (maximum signal amplitude contrbution to pitch from IPC) - REAL(8), DIMENSION(:), ALLOCATABLE :: IPC_KI ! Integral gain for the individual pitch controller, [-]. 8E-10 - REAL(8), DIMENSION(:), ALLOCATABLE :: IPC_aziOffset ! Phase offset added to the azimuth angle for the individual pitch controller, [rad]. - REAL(8) :: IPC_CornerFreqAct ! Corner frequency of the first-order actuators model, to induce a phase lag in the IPC signal {0 - Disable}, [rad/s] - INTEGER(4) :: PC_ControlMode ! Blade pitch control mode {0 - No pitch, fix to fine pitch, 1 - active PI blade pitch control} - INTEGER(4) :: PC_GS_n ! Amount of gain-scheduling table entries - REAL(8), DIMENSION(:), ALLOCATABLE :: PC_GS_angles ! Gain-schedule table - pitch angles - REAL(8), DIMENSION(:), ALLOCATABLE :: PC_GS_KP ! Gain-schedule table - pitch controller kp gains - REAL(8), DIMENSION(:), ALLOCATABLE :: PC_GS_KI ! Gain-schedule table - pitch controller ki gains - REAL(8), DIMENSION(:), ALLOCATABLE :: PC_GS_KD ! Gain-schedule table - pitch controller kd gains - REAL(8), DIMENSION(:), ALLOCATABLE :: PC_GS_TF ! Gain-schedule table - pitch controller tf gains (derivative filter) - REAL(8) :: PC_MaxPit ! Maximum physical pitch limit, [rad]. - REAL(8) :: PC_MinPit ! Minimum physical pitch limit, [rad]. - REAL(8) :: PC_MaxRat ! Maximum pitch rate (in absolute value) in pitch controller, [rad/s]. - REAL(8) :: PC_MinRat ! Minimum pitch rate (in absolute value) in pitch controller, [rad/s]. - REAL(8) :: PC_RefSpd ! Desired (reference) HSS speed for pitch controller, [rad/s]. - REAL(8) :: PC_FinePit ! Record 5 - Below-rated pitch angle set-point (deg) [used only with Bladed Interface] - REAL(8) :: PC_Switch ! Angle above lowest minimum pitch angle for switch [rad] - INTEGER(4) :: VS_ControlMode ! Generator torque control mode in above rated conditions {0 - constant torque, 1 - constant power, 2 - TSR Tracking, 3 - TSR Tracking w/ const power} - REAL(8) :: VS_GenEff ! Generator efficiency mechanical power -> electrical power [-] - REAL(8) :: VS_ArSatTq ! Above rated generator torque PI control saturation, [Nm] -- 212900 - REAL(8) :: VS_MaxRat ! Maximum torque rate (in absolute value) in torque controller, [Nm/s]. - REAL(8) :: VS_MaxTq ! Maximum generator torque in Region 3 (HSS side), [Nm]. -- chosen to be 10% above VS_RtTq - REAL(8) :: VS_MinTq ! Minimum generator (HSS side), [Nm]. - REAL(8) :: VS_MinOMSpd ! Optimal mode minimum speed, [rad/s] - REAL(8) :: VS_Rgn2K ! Generator torque constant in Region 2 (HSS side), N-m/(rad/s)^2 - REAL(8) :: VS_RtPwr ! Wind turbine rated power [W] - REAL(8) :: VS_RtTq ! Rated torque, [Nm]. - REAL(8) :: VS_RefSpd ! Rated generator speed [rad/s] - INTEGER(4) :: VS_n ! Number of controller gains - REAL(8), DIMENSION(:), ALLOCATABLE :: VS_KP ! Proportional gain for generator PI torque controller, used in the transitional 2.5 region - REAL(8), DIMENSION(:), ALLOCATABLE :: VS_KI ! Integral gain for generator PI torque controller, used in the transitional 2.5 region - REAL(8) :: VS_TSRopt ! Power-maximizing region 2 tip-speed ratio [rad] - INTEGER(4) :: SS_Mode ! Setpoint Smoother mode {0 - no setpoint smoothing, 1 - introduce setpoint smoothing} - REAL(8) :: SS_VSGain ! Variable speed torque controller setpoint smoother gain, [-]. - REAL(8) :: SS_PCGain ! Collective pitch controller setpoint smoother gain, [-]. - 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(8) :: WE_BladeRadius ! Blade length [m] - INTEGER(4) :: WE_CP_n ! Amount of parameters in the Cp array - REAL(8), DIMENSION(:), ALLOCATABLE :: WE_CP ! Parameters that define the parameterized CP(\lambda) function - REAL(8) :: WE_Gamma ! Adaption gain of the wind speed estimator algorithm [m/rad] - REAL(8) :: WE_GearboxRatio ! Gearbox ratio, >=1 [-] - REAL(8) :: WE_Jtot ! Total drivetrain inertia, including blades, hub and casted generator inertia to LSS [kg m^2] - REAL(8) :: WE_RhoAir ! Air density [kg m^-3] + INTEGER(IntKi) :: LoggingLevel ! 0 - write no debug files, 1 - write standard output .dbg-file, 2 - write standard output .dbg-file and complete avrSWAP-array .dbg2-file + INTEGER(IntKi) :: F_LPFType ! Low pass filter on the rotor and generator speed {1 - first-order low-pass filter, 2 - second-order low-pass filter}, [rad/s] + INTEGER(IntKi) :: F_NotchType ! Notch on the measured generator speed {0 - disable, 1 - enable} + REAL(DbKi) :: F_LPFCornerFreq ! Corner frequency (-3dB point) in the first-order low-pass filter, [rad/s] + REAL(DbKi) :: F_LPFDamping ! Damping coefficient [used only when F_FilterType = 2] + REAL(DbKi) :: F_NotchCornerFreq ! Natural frequency of the notch filter, [rad/s] + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: F_NotchBetaNumDen ! These two notch damping values (numerator and denominator) determines the width and depth of the notch + REAL(DbKi) :: F_SSCornerFreq ! Corner frequency (-3dB point) in the first order low pass filter for the setpoint smoother [rad/s] + REAL(DbKi) :: F_WECornerFreq ! Corner frequency (-3dB point) in the first order low pass filter for the wind speed estimate [rad/s] + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: F_FlCornerFreq ! Corner frequency (-3dB point) in the second order low pass filter of the tower-top fore-aft motion for floating feedback control [rad/s]. + REAL(DbKi) :: F_FlHighPassFreq ! Natural frequency of first-roder high-pass filter for nacelle fore-aft motion [rad/s]. + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: F_FlpCornerFreq ! Corner frequency (-3dB point) in the second order low pass filter of the blade root bending moment for flap control [rad/s]. + REAL(DbKi) :: FA_HPFCornerFreq ! Corner frequency (-3dB point) in the high-pass filter on the fore-aft acceleration signal [rad/s] + REAL(DbKi) :: FA_IntSat ! Integrator saturation (maximum signal amplitude contrbution to pitch from FA damper), [rad] + REAL(DbKi) :: FA_KI ! Integral gain for the fore-aft tower damper controller, -1 = off / >0 = on [rad s/m] + INTEGER(IntKi) :: IPC_ControlMode ! Turn Individual Pitch Control (IPC) for fatigue load reductions (pitch contribution) {0 - off, 1 - 1P reductions, 2 - 1P+2P reductions} + REAL(DbKi) :: IPC_IntSat ! Integrator saturation (maximum signal amplitude contrbution to pitch from IPC) + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: IPC_KI ! Integral gain for the individual pitch controller, [-]. 8E-10 + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: IPC_aziOffset ! Phase offset added to the azimuth angle for the individual pitch controller, [rad]. + REAL(DbKi) :: IPC_CornerFreqAct ! Corner frequency of the first-order actuators model, to induce a phase lag in the IPC signal {0 - Disable}, [rad/s] + INTEGER(IntKi) :: PC_ControlMode ! Blade pitch control mode {0 - No pitch, fix to fine pitch, 1 - active PI blade pitch control} + INTEGER(IntKi) :: PC_GS_n ! Amount of gain-scheduling table entries + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: PC_GS_angles ! Gain-schedule table - pitch angles + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: PC_GS_KP ! Gain-schedule table - pitch controller kp gains + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: PC_GS_KI ! Gain-schedule table - pitch controller ki gains + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: PC_GS_KD ! Gain-schedule table - pitch controller kd gains + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: PC_GS_TF ! Gain-schedule table - pitch controller tf gains (derivative filter) + REAL(DbKi) :: PC_MaxPit ! Maximum physical pitch limit, [rad]. + REAL(DbKi) :: PC_MinPit ! Minimum physical pitch limit, [rad]. + REAL(DbKi) :: PC_MaxRat ! Maximum pitch rate (in absolute value) in pitch controller, [rad/s]. + REAL(DbKi) :: PC_MinRat ! Minimum pitch rate (in absolute value) in pitch controller, [rad/s]. + REAL(DbKi) :: PC_RefSpd ! Desired (reference) HSS speed for pitch controller, [rad/s]. + REAL(DbKi) :: PC_FinePit ! Record 5 - Below-rated pitch angle set-point (deg) [used only with Bladed Interface] + REAL(DbKi) :: PC_Switch ! Angle above lowest minimum pitch angle for switch [rad] + INTEGER(IntKi) :: VS_ControlMode ! Generator torque control mode in above rated conditions {0 - constant torque, 1 - constant power, 2 - TSR Tracking, 3 - TSR Tracking w/ const power} + REAL(DbKi) :: VS_GenEff ! Generator efficiency mechanical power -> electrical power [-] + REAL(DbKi) :: VS_ArSatTq ! Above rated generator torque PI control saturation, [Nm] -- 212900 + REAL(DbKi) :: VS_MaxRat ! Maximum torque rate (in absolute value) in torque controller, [Nm/s]. + REAL(DbKi) :: VS_MaxTq ! Maximum generator torque in Region 3 (HSS side), [Nm]. -- chosen to be 10% above VS_RtTq + REAL(DbKi) :: VS_MinTq ! Minimum generator (HSS side), [Nm]. + REAL(DbKi) :: VS_MinOMSpd ! Optimal mode minimum speed, [rad/s] + REAL(DbKi) :: VS_Rgn2K ! Generator torque constant in Region 2 (HSS side), N-m/(rad/s)^2 + REAL(DbKi) :: VS_RtPwr ! Wind turbine rated power [W] + REAL(DbKi) :: VS_RtTq ! Rated torque, [Nm]. + REAL(DbKi) :: VS_RefSpd ! Rated generator speed [rad/s] + INTEGER(IntKi) :: VS_n ! Number of controller gains + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: VS_KP ! Proportional gain for generator PI torque controller, used in the transitional 2.5 region + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: VS_KI ! Integral gain for generator PI torque controller, used in the transitional 2.5 region + REAL(DbKi) :: VS_TSRopt ! Power-maximizing region 2 tip-speed ratio [rad] + INTEGER(IntKi) :: SS_Mode ! Setpoint Smoother mode {0 - no setpoint smoothing, 1 - introduce setpoint smoothing} + REAL(DbKi) :: SS_VSGain ! Variable speed torque controller setpoint smoother gain, [-]. + REAL(DbKi) :: SS_PCGain ! Collective pitch controller setpoint smoother gain, [-]. + INTEGER(IntKi) :: 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(DbKi) :: WE_BladeRadius ! Blade length [m] + INTEGER(IntKi) :: WE_CP_n ! Amount of parameters in the Cp array + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: WE_CP ! Parameters that define the parameterized CP(\lambda) function + REAL(DbKi) :: WE_Gamma ! Adaption gain of the wind speed estimator algorithm [m/rad] + REAL(DbKi) :: WE_GearboxRatio ! Gearbox ratio, >=1 [-] + REAL(DbKi) :: WE_Jtot ! Total drivetrain inertia, including blades, hub and casted generator inertia to LSS [kg m^2] + REAL(DbKi) :: WE_RhoAir ! Air density [kg m^-3] CHARACTER(1024) :: PerfFileName ! File containing rotor performance tables (Cp,Ct,Cq) - INTEGER(4), DIMENSION(:), ALLOCATABLE :: PerfTableSize ! Size of rotor performance tables, first number refers to number of blade pitch angles, second number referse to number of tip-speed ratios - INTEGER(4) :: WE_FOPoles_N ! Number of first-order system poles used in EKF - REAL(8), DIMENSION(:), ALLOCATABLE :: WE_FOPoles_v ! Wind speeds corresponding to first-order system poles [m/s] - REAL(8), DIMENSION(:), ALLOCATABLE :: WE_FOPoles ! First order system poles - INTEGER(4) :: Y_ControlMode ! Yaw control mode {0 - no yaw control, 1 - yaw rate control, 2 - yaw-by-IPC} - REAL(8) :: Y_ErrThresh ! Error threshold [rad]. Turbine begins to yaw when it passes this. (104.71975512) -- 1.745329252 - REAL(8) :: Y_IPC_IntSat ! Integrator saturation (maximum signal amplitude contrbution to pitch from yaw-by-IPC) - INTEGER(4) :: Y_IPC_n ! Number of controller gains (yaw-by-IPC) - REAL(8), DIMENSION(:), ALLOCATABLE :: Y_IPC_KP ! Yaw-by-IPC proportional controller gain Kp - REAL(8), DIMENSION(:), ALLOCATABLE :: Y_IPC_KI ! Yaw-by-IPC integral controller gain Ki - REAL(8) :: Y_IPC_omegaLP ! Low-pass filter corner frequency for the Yaw-by-IPC controller to filtering the yaw alignment error, [rad/s]. - REAL(8) :: Y_IPC_zetaLP ! Low-pass filter damping factor for the Yaw-by-IPC controller to filtering the yaw alignment error, [-]. - REAL(8) :: Y_MErrSet ! Yaw alignment error, setpoint [rad] - REAL(8) :: Y_omegaLPFast ! Corner frequency fast low pass filter, 1.0 [Hz] - REAL(8) :: Y_omegaLPSlow ! Corner frequency slow low pass filter, 1/60 [Hz] - REAL(8) :: Y_Rate ! Yaw rate [rad/s] - INTEGER(4) :: PS_Mode ! Pitch saturation mode {0 - no peak shaving, 1 - implement pitch saturation} - INTEGER(4) :: PS_BldPitchMin_N ! Number of values in minimum blade pitch lookup table (should equal number of values in PS_WindSpeeds and PS_BldPitchMin) - REAL(8), DIMENSION(:), ALLOCATABLE :: PS_WindSpeeds ! Wind speeds corresponding to minimum blade pitch angles [m/s] - REAL(8), DIMENSION(:), ALLOCATABLE :: PS_BldPitchMin ! Minimum blade pitch angles [rad] - INTEGER(4) :: SD_Mode ! Shutdown mode {0 - no shutdown procedure, 1 - pitch to max pitch at shutdown} - REAL(8) :: SD_MaxPit ! Maximum blade pitch angle to initiate shutdown, [rad] - REAL(8) :: SD_CornerFreq ! Cutoff Frequency for first order low-pass filter for blade pitch angle, [rad/s] - INTEGER(4) :: Fl_Mode ! Floating specific feedback mode {0 - no nacelle velocity feedback, 1 - nacelle velocity feedback} - REAL(8) :: Fl_Kp ! Nacelle velocity proportional feedback gain [s] - INTEGER(4) :: Flp_Mode ! Flap actuator mode {0 - off, 1 - fixed flap position, 2 - PI flap control} - REAL(8) :: Flp_Angle ! Fixed flap angle (degrees) - REAL(8) :: Flp_Kp ! PI flap control proportional gain - REAL(8) :: Flp_Ki ! PI flap control integral gain - REAL(8) :: Flp_MaxPit ! Maximum (and minimum) flap pitch angle [rad] + INTEGER(IntKi), DIMENSION(:), ALLOCATABLE :: PerfTableSize ! Size of rotor performance tables, first number refers to number of blade pitch angles, second number referse to number of tip-speed ratios + INTEGER(IntKi) :: WE_FOPoles_N ! Number of first-order system poles used in EKF + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: WE_FOPoles_v ! Wind speeds corresponding to first-order system poles [m/s] + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: WE_FOPoles ! First order system poles + INTEGER(IntKi) :: Y_ControlMode ! Yaw control mode {0 - no yaw control, 1 - yaw rate control, 2 - yaw-by-IPC} + REAL(DbKi) :: Y_ErrThresh ! Error threshold [rad]. Turbine begins to yaw when it passes this. (104.71975512) -- 1.745329252 + REAL(DbKi) :: Y_IPC_IntSat ! Integrator saturation (maximum signal amplitude contrbution to pitch from yaw-by-IPC) + INTEGER(IntKi) :: Y_IPC_n ! Number of controller gains (yaw-by-IPC) + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: Y_IPC_KP ! Yaw-by-IPC proportional controller gain Kp + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: Y_IPC_KI ! Yaw-by-IPC integral controller gain Ki + REAL(DbKi) :: Y_IPC_omegaLP ! Low-pass filter corner frequency for the Yaw-by-IPC controller to filtering the yaw alignment error, [rad/s]. + REAL(DbKi) :: Y_IPC_zetaLP ! Low-pass filter damping factor for the Yaw-by-IPC controller to filtering the yaw alignment error, [-]. + REAL(DbKi) :: Y_MErrSet ! Yaw alignment error, setpoint [rad] + REAL(DbKi) :: Y_omegaLPFast ! Corner frequency fast low pass filter, 1.0 [Hz] + REAL(DbKi) :: Y_omegaLPSlow ! Corner frequency slow low pass filter, 1/60 [Hz] + REAL(DbKi) :: Y_Rate ! Yaw rate [rad/s] + INTEGER(IntKi) :: PS_Mode ! Pitch saturation mode {0 - no peak shaving, 1 - implement pitch saturation} + INTEGER(IntKi) :: PS_BldPitchMin_N ! Number of values in minimum blade pitch lookup table (should equal number of values in PS_WindSpeeds and PS_BldPitchMin) + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: PS_WindSpeeds ! Wind speeds corresponding to minimum blade pitch angles [m/s] + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: PS_BldPitchMin ! Minimum blade pitch angles [rad] + INTEGER(IntKi) :: SD_Mode ! Shutdown mode {0 - no shutdown procedure, 1 - pitch to max pitch at shutdown} + REAL(DbKi) :: SD_MaxPit ! Maximum blade pitch angle to initiate shutdown, [rad] + REAL(DbKi) :: SD_CornerFreq ! Cutoff Frequency for first order low-pass filter for blade pitch angle, [rad/s] + INTEGER(IntKi) :: Fl_Mode ! Floating specific feedback mode {0 - no nacelle velocity feedback, 1 - nacelle velocity feedback} + REAL(DbKi) :: Fl_Kp ! Nacelle velocity proportional feedback gain [s] + INTEGER(IntKi) :: Flp_Mode ! Flap actuator mode {0 - off, 1 - fixed flap position, 2 - PI flap control} + REAL(DbKi) :: Flp_Angle ! Fixed flap angle (degrees) + REAL(DbKi) :: Flp_Kp ! PI flap control proportional gain + REAL(DbKi) :: Flp_Ki ! PI flap control integral gain + REAL(DbKi) :: Flp_MaxPit ! Maximum (and minimum) flap pitch angle [rad] CHARACTER(1024) :: OL_Filename ! Input file with open loop timeseries - INTEGER(4) :: OL_Mode ! Open loop control mode {0 - no open loop control, 1 - open loop control vs. time, 2 - open loop control vs. wind speed} - INTEGER(4) :: Ind_Breakpoint ! The column in OL_Filename that contains the breakpoint (time if OL_Mode = 1) - INTEGER(4) :: Ind_BldPitch ! The column in OL_Filename that contains the blade pitch input in rad - INTEGER(4) :: Ind_GenTq ! The column in OL_Filename that contains the generator torque in Nm - INTEGER(4) :: Ind_YawRate ! The column in OL_Filename that contains the generator torque in Nm - REAL(8), DIMENSION(:), ALLOCATABLE :: OL_Breakpoints ! Open loop breakpoints in timeseries - REAL(8), DIMENSION(:), ALLOCATABLE :: OL_BldPitch ! Open blade pitch timeseries - REAL(8), DIMENSION(:), ALLOCATABLE :: OL_GenTq ! Open generator torque timeseries - REAL(8), DIMENSION(:), ALLOCATABLE :: OL_YawRate ! Open yaw rate timeseries - REAL(8), DIMENSION(:,:), ALLOCATABLE :: OL_Channels ! Open loop channels in timeseries - REAL(8) :: PC_RtTq99 ! 99% of the rated torque value, using for switching between pitch and torque control, [Nm]. - REAL(8) :: VS_MaxOMTq ! Maximum torque at the end of the below-rated region 2, [Nm] - REAL(8) :: VS_MinOMTq ! Minimum torque at the beginning of the below-rated region 2, [Nm] + INTEGER(IntKi) :: OL_Mode ! Open loop control mode {0 - no open loop control, 1 - open loop control vs. time, 2 - open loop control vs. wind speed} + INTEGER(IntKi) :: Ind_Breakpoint ! The column in OL_Filename that contains the breakpoint (time if OL_Mode = 1) + INTEGER(IntKi) :: Ind_BldPitch ! The column in OL_Filename that contains the blade pitch input in rad + INTEGER(IntKi) :: Ind_GenTq ! The column in OL_Filename that contains the generator torque in Nm + INTEGER(IntKi) :: Ind_YawRate ! The column in OL_Filename that contains the generator torque in Nm + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: OL_Breakpoints ! Open loop breakpoints in timeseries + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: OL_BldPitch ! Open blade pitch timeseries + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: OL_GenTq ! Open generator torque timeseries + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: OL_YawRate ! Open yaw rate timeseries + REAL(DbKi), DIMENSION(:,:), ALLOCATABLE :: OL_Channels ! Open loop channels in timeseries + REAL(DbKi) :: PC_RtTq99 ! 99% of the rated torque value, using for switching between pitch and torque control, [Nm]. + REAL(DbKi) :: VS_MaxOMTq ! Maximum torque at the end of the below-rated region 2, [Nm] + REAL(DbKi) :: VS_MinOMTq ! Minimum torque at the beginning of the below-rated region 2, [Nm] END TYPE ControlParameters TYPE, PUBLIC :: WE - REAL(8) :: om_r ! Estimated rotor speed [rad/s] - REAL(8) :: v_t ! Estimated wind speed, turbulent component [m/s] - REAL(8) :: v_m ! Estimated wind speed, 10-minute averaged [m/s] - REAL(8) :: v_h ! Combined estimated wind speed [m/s] - REAL(8), DIMENSION(3,3) :: P ! Covariance estiamte - REAL(8), DIMENSION(3,1) :: xh ! Estimated state matrix - REAL(8), DIMENSION(3,1) :: K ! Kalman gain matrix + REAL(DbKi) :: om_r ! Estimated rotor speed [rad/s] + REAL(DbKi) :: v_t ! Estimated wind speed, turbulent component [m/s] + REAL(DbKi) :: v_m ! Estimated wind speed, 10-minute averaged [m/s] + REAL(DbKi) :: v_h ! Combined estimated wind speed [m/s] + REAL(DbKi), DIMENSION(3,3) :: P ! Covariance estiamte + REAL(DbKi), DIMENSION(3,1) :: xh ! Estimated state matrix + REAL(DbKi), DIMENSION(3,1) :: K ! Kalman gain matrix END TYPE WE TYPE, PUBLIC :: FilterParameters - REAL(8), DIMENSION(99) :: lpf1_a1 ! First order filter - Denominator coefficient 1 - REAL(8), DIMENSION(99) :: lpf1_a0 ! First order filter - Denominator coefficient 0 - REAL(8), DIMENSION(99) :: lpf1_b1 ! First order filter - Numerator coefficient 1 - REAL(8), DIMENSION(99) :: lpf1_b0 ! First order filter - Numerator coefficient 0 - REAL(8), DIMENSION(99) :: lpf1_InputSignalLast ! First order filter - Previous input - REAL(8), DIMENSION(99) :: lpf1_OutputSignalLast ! First order filter - Previous output - REAL(8), DIMENSION(99) :: lpf2_a2 ! Second order filter - Denominator coefficient 2 - REAL(8), DIMENSION(99) :: lpf2_a1 ! Second order filter - Denominator coefficient 1 - REAL(8), DIMENSION(99) :: lpf2_a0 ! Second order filter - Denominator coefficient 0 - REAL(8), DIMENSION(99) :: lpf2_b2 ! Second order filter - Numerator coefficient 2 - REAL(8), DIMENSION(99) :: lpf2_b1 ! Second order filter - Numerator coefficient 1 - REAL(8), DIMENSION(99) :: lpf2_b0 ! Second order filter - Numerator coefficient 0 - REAL(8), DIMENSION(99) :: lpf2_InputSignalLast2 ! Second order filter - Previous input 2 - REAL(8), DIMENSION(99) :: lpf2_OutputSignalLast2 ! Second order filter - Previous output 2 - REAL(8), DIMENSION(99) :: lpf2_InputSignalLast1 ! Second order filter - Previous input 1 - REAL(8), DIMENSION(99) :: lpf2_OutputSignalLast1 ! Second order filter - Previous output 1 - REAL(8), DIMENSION(99) :: hpf_InputSignalLast ! High pass filter - Previous output 1 - REAL(8), DIMENSION(99) :: hpf_OutputSignalLast ! High pass filter - Previous output 1 - REAL(8), DIMENSION(99) :: nfs_OutputSignalLast1 ! Notch filter slopes previous output 1 - REAL(8), DIMENSION(99) :: nfs_OutputSignalLast2 ! Notch filter slopes previous output 2 - REAL(8), DIMENSION(99) :: nfs_InputSignalLast1 ! Notch filter slopes previous input 1 - REAL(8), DIMENSION(99) :: nfs_InputSignalLast2 ! Notch filter slopes previous input 1 - REAL(8), DIMENSION(99) :: nfs_b2 ! Notch filter slopes numerator coefficient 2 - REAL(8), DIMENSION(99) :: nfs_b0 ! Notch filter slopes numerator coefficient 0 - REAL(8), DIMENSION(99) :: nfs_a2 ! Notch filter slopes denominator coefficient 2 - REAL(8), DIMENSION(99) :: nfs_a1 ! Notch filter slopes denominator coefficient 1 - REAL(8), DIMENSION(99) :: nfs_a0 ! Notch filter slopes denominator coefficient 0 - REAL(8), DIMENSION(99) :: nf_OutputSignalLast1 ! Notch filter previous output 1 - REAL(8), DIMENSION(99) :: nf_OutputSignalLast2 ! Notch filter previous output 2 - REAL(8), DIMENSION(99) :: nf_InputSignalLast1 ! Notch filter previous input 1 - REAL(8), DIMENSION(99) :: nf_InputSignalLast2 ! Notch filter previous input 2 - REAL(8), DIMENSION(99) :: nf_b2 ! Notch filter numerator coefficient 2 - REAL(8), DIMENSION(99) :: nf_b1 ! Notch filter numerator coefficient 1 - REAL(8), DIMENSION(99) :: nf_b0 ! Notch filter numerator coefficient 0 - REAL(8), DIMENSION(99) :: nf_a1 ! Notch filter denominator coefficient 1 - REAL(8), DIMENSION(99) :: nf_a0 ! Notch filter denominator coefficient 0 + REAL(DbKi), DIMENSION(99) :: lpf1_a1 ! First order filter - Denominator coefficient 1 + REAL(DbKi), DIMENSION(99) :: lpf1_a0 ! First order filter - Denominator coefficient 0 + REAL(DbKi), DIMENSION(99) :: lpf1_b1 ! First order filter - Numerator coefficient 1 + REAL(DbKi), DIMENSION(99) :: lpf1_b0 ! First order filter - Numerator coefficient 0 + REAL(DbKi), DIMENSION(99) :: lpf1_InputSignalLast ! First order filter - Previous input + REAL(DbKi), DIMENSION(99) :: lpf1_OutputSignalLast ! First order filter - Previous output + REAL(DbKi), DIMENSION(99) :: lpf2_a2 ! Second order filter - Denominator coefficient 2 + REAL(DbKi), DIMENSION(99) :: lpf2_a1 ! Second order filter - Denominator coefficient 1 + REAL(DbKi), DIMENSION(99) :: lpf2_a0 ! Second order filter - Denominator coefficient 0 + REAL(DbKi), DIMENSION(99) :: lpf2_b2 ! Second order filter - Numerator coefficient 2 + REAL(DbKi), DIMENSION(99) :: lpf2_b1 ! Second order filter - Numerator coefficient 1 + REAL(DbKi), DIMENSION(99) :: lpf2_b0 ! Second order filter - Numerator coefficient 0 + REAL(DbKi), DIMENSION(99) :: lpf2_InputSignalLast2 ! Second order filter - Previous input 2 + REAL(DbKi), DIMENSION(99) :: lpf2_OutputSignalLast2 ! Second order filter - Previous output 2 + REAL(DbKi), DIMENSION(99) :: lpf2_InputSignalLast1 ! Second order filter - Previous input 1 + REAL(DbKi), DIMENSION(99) :: lpf2_OutputSignalLast1 ! Second order filter - Previous output 1 + REAL(DbKi), DIMENSION(99) :: hpf_InputSignalLast ! High pass filter - Previous output 1 + REAL(DbKi), DIMENSION(99) :: hpf_OutputSignalLast ! High pass filter - Previous output 1 + REAL(DbKi), DIMENSION(99) :: nfs_OutputSignalLast1 ! Notch filter slopes previous output 1 + REAL(DbKi), DIMENSION(99) :: nfs_OutputSignalLast2 ! Notch filter slopes previous output 2 + REAL(DbKi), DIMENSION(99) :: nfs_InputSignalLast1 ! Notch filter slopes previous input 1 + REAL(DbKi), DIMENSION(99) :: nfs_InputSignalLast2 ! Notch filter slopes previous input 1 + REAL(DbKi), DIMENSION(99) :: nfs_b2 ! Notch filter slopes numerator coefficient 2 + REAL(DbKi), DIMENSION(99) :: nfs_b0 ! Notch filter slopes numerator coefficient 0 + REAL(DbKi), DIMENSION(99) :: nfs_a2 ! Notch filter slopes denominator coefficient 2 + REAL(DbKi), DIMENSION(99) :: nfs_a1 ! Notch filter slopes denominator coefficient 1 + REAL(DbKi), DIMENSION(99) :: nfs_a0 ! Notch filter slopes denominator coefficient 0 + REAL(DbKi), DIMENSION(99) :: nf_OutputSignalLast1 ! Notch filter previous output 1 + REAL(DbKi), DIMENSION(99) :: nf_OutputSignalLast2 ! Notch filter previous output 2 + REAL(DbKi), DIMENSION(99) :: nf_InputSignalLast1 ! Notch filter previous input 1 + REAL(DbKi), DIMENSION(99) :: nf_InputSignalLast2 ! Notch filter previous input 2 + REAL(DbKi), DIMENSION(99) :: nf_b2 ! Notch filter numerator coefficient 2 + REAL(DbKi), DIMENSION(99) :: nf_b1 ! Notch filter numerator coefficient 1 + REAL(DbKi), DIMENSION(99) :: nf_b0 ! Notch filter numerator coefficient 0 + REAL(DbKi), DIMENSION(99) :: nf_a1 ! Notch filter denominator coefficient 1 + REAL(DbKi), DIMENSION(99) :: nf_a0 ! Notch filter denominator coefficient 0 END TYPE FilterParameters TYPE, PUBLIC :: piParams - REAL(8), DIMENSION(99) :: ITerm ! Integrator term - REAL(8), DIMENSION(99) :: ITermLast ! Previous integrator term - REAL(8), DIMENSION(99) :: ITerm2 ! Integrator term - second integrator - REAL(8), DIMENSION(99) :: ITermLast2 ! Previous integrator term - second integrator + REAL(DbKi), DIMENSION(99) :: ITerm ! Integrator term + REAL(DbKi), DIMENSION(99) :: ITermLast ! Previous integrator term + REAL(DbKi), DIMENSION(99) :: ITerm2 ! Integrator term - second integrator + REAL(DbKi), DIMENSION(99) :: ITermLast2 ! Previous integrator term - second integrator END TYPE piParams TYPE, PUBLIC :: LocalVariables - INTEGER(4) :: iStatus ! Initialization status - REAL(8) :: Time ! Time [s] - REAL(8) :: DT ! Time step [s] - REAL(8) :: VS_GenPwr ! Generator power [W] - REAL(8) :: GenSpeed ! Generator speed (HSS) [rad/s] - REAL(8) :: RotSpeed ! Rotor speed (LSS) [rad/s] - REAL(8) :: Y_M ! Yaw direction [rad] - REAL(8) :: HorWindV ! Hub height wind speed m/s - REAL(8) :: rootMOOP(3) ! Blade root bending moment [Nm] - REAL(8) :: BlPitch(3) ! Blade pitch [rad] - REAL(8) :: Azimuth ! Rotor aziumuth angle [rad] - INTEGER(4) :: NumBl ! Number of blades [-] - REAL(8) :: FA_Acc ! Tower fore-aft acceleration [m/s^2] - REAL(8) :: NacIMU_FA_Acc ! Tower fore-aft acceleration [rad/s^2] - REAL(8) :: FA_AccHPF ! High-pass filtered fore-aft acceleration [m/s^2] - REAL(8) :: FA_AccHPFI ! Tower velocity, high-pass filtered and integrated fore-aft acceleration [m/s] - REAL(8) :: FA_PitCom(3) ! Tower fore-aft vibration damping pitch contribution [rad] - REAL(8) :: RotSpeedF ! Filtered LSS (generator) speed [rad/s]. - REAL(8) :: GenSpeedF ! Filtered HSS (generator) speed [rad/s]. - REAL(8) :: GenTq ! Electrical generator torque, [Nm]. - REAL(8) :: GenTqMeas ! Measured generator torque [Nm] - REAL(8) :: GenArTq ! Electrical generator torque, for above-rated PI-control [Nm]. - REAL(8) :: GenBrTq ! Electrical generator torque, for below-rated PI-control [Nm]. - REAL(8) :: IPC_PitComF(3) ! Commanded pitch of each blade as calculated by the individual pitch controller, F stands for low-pass filtered [rad]. - REAL(8) :: PC_KP ! Proportional gain for pitch controller at rated pitch (zero) [s]. - REAL(8) :: PC_KI ! Integral gain for pitch controller at rated pitch (zero) [-]. - REAL(8) :: PC_KD ! Differential gain for pitch controller at rated pitch (zero) [-]. - REAL(8) :: PC_TF ! First-order filter parameter for derivative action - REAL(8) :: PC_MaxPit ! Maximum pitch setting in pitch controller (variable) [rad]. - REAL(8) :: PC_MinPit ! Minimum pitch setting in pitch controller (variable) [rad]. - REAL(8) :: PC_PitComT ! Total command pitch based on the sum of the proportional and integral terms [rad]. - REAL(8) :: PC_PitComT_Last ! Last total command pitch based on the sum of the proportional and integral terms [rad]. - REAL(8) :: PC_PitComTF ! Filtered Total command pitch based on the sum of the proportional and integral terms [rad]. - REAL(8) :: PC_PitComT_IPC(3) ! Total command pitch based on the sum of the proportional and integral terms, including IPC term [rad]. - REAL(8) :: PC_PwrErr ! Power error with respect to rated power [W] - REAL(8) :: PC_SpdErr ! Current speed error (pitch control) [rad/s]. - REAL(8) :: IPC_IntAxisTilt_1P ! Integral of the direct axis, 1P - REAL(8) :: IPC_IntAxisYaw_1P ! Integral of quadrature, 1P - REAL(8) :: IPC_IntAxisTilt_2P ! Integral of the direct axis, 2P - REAL(8) :: IPC_IntAxisYaw_2P ! Integral of quadrature, 2P - INTEGER(4) :: PC_State ! State of the pitch control system - REAL(8) :: PitCom(3) ! Commanded pitch of each blade the last time the controller was called [rad]. - REAL(8) :: SS_DelOmegaF ! Filtered setpoint shifting term defined in setpoint smoother [rad/s]. - REAL(8) :: TestType ! Test variable, no use - REAL(8) :: VS_MaxTq ! Maximum allowable generator torque [Nm]. - REAL(8) :: VS_LastGenTrq ! Commanded electrical generator torque the last time the controller was called [Nm]. - REAL(8) :: VS_LastGenPwr ! Commanded electrical generator torque the last time the controller was called [Nm]. - REAL(8) :: VS_MechGenPwr ! Mechanical power on the generator axis [W] - REAL(8) :: VS_SpdErrAr ! Current speed error for region 2.5 PI controller (generator torque control) [rad/s]. - REAL(8) :: VS_SpdErrBr ! Current speed error for region 1.5 PI controller (generator torque control) [rad/s]. - REAL(8) :: 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(8) :: VS_Rgn3Pitch ! Pitch angle at which the state machine switches to region 3, [rad]. - REAL(8) :: WE_Vw ! Estimated wind speed [m/s] - REAL(8) :: WE_Vw_F ! Filtered estimated wind speed [m/s] - REAL(8) :: WE_VwI ! Integrated wind speed quantity for estimation [m/s] - REAL(8) :: WE_VwIdot ! Differentiated integrated wind speed quantity for estimation [m/s] - REAL(8) :: VS_LastGenTrqF ! Differentiated integrated wind speed quantity for estimation [m/s] - REAL(8) :: Y_AccErr ! Accumulated yaw error [rad]. - REAL(8) :: Y_ErrLPFFast ! Filtered yaw error by fast low pass filter [rad]. - REAL(8) :: Y_ErrLPFSlow ! Filtered yaw error by slow low pass filter [rad]. - REAL(8) :: Y_MErr ! Measured yaw error, measured + setpoint [rad]. - REAL(8) :: Y_YawEndT ! Yaw end time [s]. Indicates the time up until which yaw is active with a fixed rate + INTEGER(IntKi) :: iStatus ! Initialization status + REAL(DbKi) :: Time ! Time [s] + REAL(DbKi) :: DT ! Time step [s] + REAL(DbKi) :: VS_GenPwr ! Generator power [W] + REAL(DbKi) :: GenSpeed ! Generator speed (HSS) [rad/s] + REAL(DbKi) :: RotSpeed ! Rotor speed (LSS) [rad/s] + REAL(DbKi) :: Y_M ! Yaw direction [rad] + REAL(DbKi) :: HorWindV ! Hub height wind speed m/s + REAL(DbKi) :: rootMOOP(3) ! Blade root bending moment [Nm] + REAL(DbKi) :: BlPitch(3) ! Blade pitch [rad] + REAL(DbKi) :: Azimuth ! Rotor aziumuth angle [rad] + INTEGER(IntKi) :: NumBl ! Number of blades [-] + REAL(DbKi) :: FA_Acc ! Tower fore-aft acceleration [m/s^2] + REAL(DbKi) :: NacIMU_FA_Acc ! Tower fore-aft acceleration [rad/s^2] + REAL(DbKi) :: FA_AccHPF ! High-pass filtered fore-aft acceleration [m/s^2] + REAL(DbKi) :: FA_AccHPFI ! Tower velocity, high-pass filtered and integrated fore-aft acceleration [m/s] + REAL(DbKi) :: FA_PitCom(3) ! Tower fore-aft vibration damping pitch contribution [rad] + REAL(DbKi) :: RotSpeedF ! Filtered LSS (generator) speed [rad/s]. + REAL(DbKi) :: GenSpeedF ! Filtered HSS (generator) speed [rad/s]. + REAL(DbKi) :: GenTq ! Electrical generator torque, [Nm]. + REAL(DbKi) :: GenTqMeas ! Measured generator torque [Nm] + REAL(DbKi) :: GenArTq ! Electrical generator torque, for above-rated PI-control [Nm]. + REAL(DbKi) :: GenBrTq ! Electrical generator torque, for below-rated PI-control [Nm]. + REAL(DbKi) :: IPC_PitComF(3) ! Commanded pitch of each blade as calculated by the individual pitch controller, F stands for low-pass filtered [rad]. + REAL(DbKi) :: PC_KP ! Proportional gain for pitch controller at rated pitch (zero) [s]. + REAL(DbKi) :: PC_KI ! Integral gain for pitch controller at rated pitch (zero) [-]. + REAL(DbKi) :: PC_KD ! Differential gain for pitch controller at rated pitch (zero) [-]. + REAL(DbKi) :: PC_TF ! First-order filter parameter for derivative action + REAL(DbKi) :: PC_MaxPit ! Maximum pitch setting in pitch controller (variable) [rad]. + REAL(DbKi) :: PC_MinPit ! Minimum pitch setting in pitch controller (variable) [rad]. + REAL(DbKi) :: PC_PitComT ! Total command pitch based on the sum of the proportional and integral terms [rad]. + REAL(DbKi) :: PC_PitComT_Last ! Last total command pitch based on the sum of the proportional and integral terms [rad]. + REAL(DbKi) :: PC_PitComTF ! Filtered Total command pitch based on the sum of the proportional and integral terms [rad]. + REAL(DbKi) :: PC_PitComT_IPC(3) ! Total command pitch based on the sum of the proportional and integral terms, including IPC term [rad]. + REAL(DbKi) :: PC_PwrErr ! Power error with respect to rated power [W] + REAL(DbKi) :: PC_SpdErr ! Current speed error (pitch control) [rad/s]. + REAL(DbKi) :: IPC_IntAxisTilt_1P ! Integral of the direct axis, 1P + REAL(DbKi) :: IPC_IntAxisYaw_1P ! Integral of quadrature, 1P + REAL(DbKi) :: IPC_IntAxisTilt_2P ! Integral of the direct axis, 2P + REAL(DbKi) :: IPC_IntAxisYaw_2P ! Integral of quadrature, 2P + INTEGER(IntKi) :: PC_State ! State of the pitch control system + REAL(DbKi) :: PitCom(3) ! Commanded pitch of each blade the last time the controller was called [rad]. + REAL(DbKi) :: SS_DelOmegaF ! Filtered setpoint shifting term defined in setpoint smoother [rad/s]. + REAL(DbKi) :: TestType ! Test variable, no use + REAL(DbKi) :: VS_MaxTq ! Maximum allowable generator torque [Nm]. + REAL(DbKi) :: VS_LastGenTrq ! Commanded electrical generator torque the last time the controller was called [Nm]. + REAL(DbKi) :: VS_LastGenPwr ! Commanded electrical generator torque the last time the controller was called [Nm]. + REAL(DbKi) :: VS_MechGenPwr ! Mechanical power on the generator axis [W] + REAL(DbKi) :: VS_SpdErrAr ! Current speed error for region 2.5 PI controller (generator torque control) [rad/s]. + REAL(DbKi) :: VS_SpdErrBr ! Current speed error for region 1.5 PI controller (generator torque control) [rad/s]. + REAL(DbKi) :: VS_SpdErr ! Current speed error for tip-speed-ratio tracking controller (generator torque control) [rad/s]. + INTEGER(IntKi) :: VS_State ! State of the torque control system + REAL(DbKi) :: VS_Rgn3Pitch ! Pitch angle at which the state machine switches to region 3, [rad]. + REAL(DbKi) :: WE_Vw ! Estimated wind speed [m/s] + REAL(DbKi) :: WE_Vw_F ! Filtered estimated wind speed [m/s] + REAL(DbKi) :: WE_VwI ! Integrated wind speed quantity for estimation [m/s] + REAL(DbKi) :: WE_VwIdot ! Differentiated integrated wind speed quantity for estimation [m/s] + REAL(DbKi) :: VS_LastGenTrqF ! Differentiated integrated wind speed quantity for estimation [m/s] + REAL(DbKi) :: Y_AccErr ! Accumulated yaw error [rad]. + REAL(DbKi) :: Y_ErrLPFFast ! Filtered yaw error by fast low pass filter [rad]. + REAL(DbKi) :: Y_ErrLPFSlow ! Filtered yaw error by slow low pass filter [rad]. + REAL(DbKi) :: Y_MErr ! Measured yaw error, measured + setpoint [rad]. + REAL(DbKi) :: Y_YawEndT ! Yaw end time [s]. Indicates the time up until which yaw is active with a fixed rate LOGICAL :: SD ! Shutdown, .FALSE. if inactive, .TRUE. if active - REAL(8) :: Fl_PitCom ! Shutdown, .FALSE. if inactive, .TRUE. if active - REAL(8) :: NACIMU_FA_AccF ! None - REAL(8) :: FA_AccF ! None - REAL(8) :: Flp_Angle(3) ! Flap Angle (rad) - REAL(8) :: RootMyb_Last(3) ! Last blade root bending moment (Nm) - INTEGER(4) :: ACC_INFILE_SIZE ! Length of parameter input filename + REAL(DbKi) :: Fl_PitCom ! Shutdown, .FALSE. if inactive, .TRUE. if active + REAL(DbKi) :: NACIMU_FA_AccF ! None + REAL(DbKi) :: FA_AccF ! None + REAL(DbKi) :: Flp_Angle(3) ! Flap Angle (rad) + REAL(DbKi) :: RootMyb_Last(3) ! Last blade root bending moment (Nm) + INTEGER(IntKi) :: ACC_INFILE_SIZE ! Length of parameter input filename CHARACTER, DIMENSION(:), ALLOCATABLE :: ACC_INFILE ! Parameter input filename LOGICAL :: restart ! Restart flag TYPE(WE) :: WE ! Wind speed estimator parameters derived type @@ -249,35 +250,35 @@ MODULE ROSCO_Types END TYPE LocalVariables TYPE, PUBLIC :: ObjectInstances - INTEGER(4) :: instLPF ! Low-pass filter instance - INTEGER(4) :: instSecLPF ! Second order low-pass filter instance - INTEGER(4) :: instHPF ! High-pass filter instance - INTEGER(4) :: instNotchSlopes ! Notch filter slopes instance - INTEGER(4) :: instNotch ! Notch filter instance - INTEGER(4) :: instPI ! PI controller instance + INTEGER(IntKi) :: instLPF ! Low-pass filter instance + INTEGER(IntKi) :: instSecLPF ! Second order low-pass filter instance + INTEGER(IntKi) :: instHPF ! High-pass filter instance + INTEGER(IntKi) :: instNotchSlopes ! Notch filter slopes instance + INTEGER(IntKi) :: instNotch ! Notch filter instance + INTEGER(IntKi) :: instPI ! PI controller instance END TYPE ObjectInstances TYPE, PUBLIC :: PerformanceData - REAL(8), DIMENSION(:), ALLOCATABLE :: TSR_vec ! TSR vector for performance surfaces - REAL(8), DIMENSION(:), ALLOCATABLE :: Beta_vec ! Blade pitch vector for performance surfaces [deg] - REAL(8), DIMENSION(:,:), ALLOCATABLE :: Cp_mat ! Power coefficient surface - REAL(8), DIMENSION(:,:), ALLOCATABLE :: Ct_mat ! Thrust coefficient surface - REAL(8), DIMENSION(:,:), ALLOCATABLE :: Cq_mat ! Torque coefficient surface + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: TSR_vec ! TSR vector for performance surfaces + REAL(DbKi), DIMENSION(:), ALLOCATABLE :: Beta_vec ! Blade pitch vector for performance surfaces [deg] + REAL(DbKi), DIMENSION(:,:), ALLOCATABLE :: Cp_mat ! Power coefficient surface + REAL(DbKi), DIMENSION(:,:), ALLOCATABLE :: Ct_mat ! Thrust coefficient surface + REAL(DbKi), DIMENSION(:,:), ALLOCATABLE :: Cq_mat ! Torque coefficient surface END TYPE PerformanceData TYPE, PUBLIC :: DebugVariables - REAL(8) :: WE_Cp ! Cp that WSE uses to determine aerodynamic torque[-] - REAL(8) :: WE_b ! Pitch that WSE uses to determine aerodynamic torque[-] - REAL(8) :: WE_w ! Rotor Speed that WSE uses to determine aerodynamic torque[-] - REAL(8) :: WE_t ! Torque that WSE uses[-] - REAL(8) :: WE_Vm ! Mean wind speed component in WSE [m/s] - REAL(8) :: WE_Vt ! Turbulent wind speed component in WSE [m/s] - REAL(8) :: WE_lambda ! TSR in WSE [rad] - REAL(8) :: PC_PICommand ! Commanded collective pitch from pitch PI controller [rad] + REAL(DbKi) :: WE_Cp ! Cp that WSE uses to determine aerodynamic torque [-] + REAL(DbKi) :: WE_b ! Pitch that WSE uses to determine aerodynamic torque [-] + REAL(DbKi) :: WE_w ! Rotor Speed that WSE uses to determine aerodynamic torque [-] + REAL(DbKi) :: WE_t ! Torque that WSE uses [-] + REAL(DbKi) :: WE_Vm ! Mean wind speed component in WSE [m/s] + REAL(DbKi) :: WE_Vt ! Turbulent wind speed component in WSE [m/s] + REAL(DbKi) :: WE_lambda ! TSR in WSE [rad] + REAL(DbKi) :: PC_PICommand ! Commanded collective pitch from pitch PI controller [rad] END TYPE DebugVariables TYPE, PUBLIC :: ErrorVariables - INTEGER(4) :: size_avcMSG ! None + INTEGER(IntKi) :: size_avcMSG ! None 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] END TYPE ErrorVariables From 24fa6b7e2421c6e827dc9f443bc6704188314565 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 11:23:27 -0700 Subject: [PATCH 16/40] delete DFController --- ROSCO/src/Functions.f90 | 30 ------------------------------ 1 file changed, 30 deletions(-) diff --git a/ROSCO/src/Functions.f90 b/ROSCO/src/Functions.f90 index 18bb27df..d0361ac7 100644 --- a/ROSCO/src/Functions.f90 +++ b/ROSCO/src/Functions.f90 @@ -410,36 +410,6 @@ FUNCTION identity(n) RESULT(A) END FUNCTION identity -!------------------------------------------------------------------------------------------------------------------------------- - REAL(DbKi) FUNCTION DFController(error, Kd, Tf, DT, inst) - ! DF controller, with output saturation - - IMPLICIT NONE - ! Inputs - REAL(DbKi), INTENT(IN) :: error - REAL(DbKi), INTENT(IN) :: kd - REAL(DbKi), INTENT(IN) :: tf - REAL(DbKi), INTENT(IN) :: DT - INTEGER(IntKi), INTENT(IN) :: inst - ! Local - REAL(DbKi) :: B ! - INTEGER(IntKi) :: i ! Counter for making arrays - REAL(DbKi), DIMENSION(99), SAVE :: errorLast = (/ (0, i=1,99) /) ! - REAL(DbKi), DIMENSION(99), SAVE :: DFControllerLast = (/ (0, i=1,99) /) ! - INTEGER(IntKi), DIMENSION(99), SAVE :: FirstCall = (/ (1, i=1,99) /) ! First call of this function? - - ! Initialize persistent variables/arrays, and set inital condition for integrator term - ! IF (FirstCall(inst) == 1) THEN - ! FirstCall(inst) = 0 - ! END IF - - B = 2.0/DT - DFController = (Kd*B)/(B*Tf+1.0)*error - (Kd*B)/(B*Tf+1.0)*errorLast(inst) - (1.0-B*Tf)/(B*Tf+1.0)*DFControllerLast(inst) - - errorLast(inst) = error - DFControllerLast(inst) = DFController - END FUNCTION DFController - !------------------------------------------------------------------------------------------------------------------------------- SUBROUTINE ColemanTransform(rootMOOP, aziAngle, nHarmonic, axTOut, axYOut) ! The Coleman or d-q axis transformation transforms the root out of plane bending moments of each turbine blade From 9149b126a8c6bd86b2152f16872b14cd1c574179 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 11:43:18 -0700 Subject: [PATCH 17/40] fix timestep mismatch --- ROSCO/src/DISCON.F90 | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/ROSCO/src/DISCON.F90 b/ROSCO/src/DISCON.F90 index cf9b2c41..e7446688 100644 --- a/ROSCO/src/DISCON.F90 +++ b/ROSCO/src/DISCON.F90 @@ -78,7 +78,11 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME ! Filter signals CALL PreFilterMeasuredSignals(CntrPar, LocalVar, objInst, ErrVar) -IF (((LocalVar%iStatus >= 0) .OR. (LocalVar%iStatus <= -9)) .AND. (ErrVar%aviFAIL >= 0)) THEN ! Only compute control calculations if no error has occurred and we are not on the last time step +IF (((LocalVar%iStatus >= 0) .OR. (LocalVar%iStatus <= -8)) .AND. (ErrVar%aviFAIL >= 0)) THEN ! Only compute control calculations if no error has occurred and we are not on the last time step + IF ((LocalVar%iStatus == -8) .AND. (ErrVar%aviFAIL >= 0)) THEN ! Write restart files + CALL WriteRestartFile(LocalVar, CntrPar, objInst, RootName, SIZE(avcOUTNAME)) + ENDIF + CALL WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, ErrVar) CALL ComputeVariablesSetpoints(CntrPar, LocalVar, objInst) CALL StateMachine(CntrPar, LocalVar) @@ -94,9 +98,8 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME CALL FlapControl(avrSWAP, CntrPar, LocalVar, objInst) END IF -ELSEIF ((LocalVar%iStatus == -8) .AND. (ErrVar%aviFAIL >= 0)) THEN ! Write restart files - CALL WriteRestartFile(LocalVar, CntrPar, objInst, RootName, SIZE(avcOUTNAME)) -ENDIF + +END IF CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) From 3c9c89d6cd75f51fbec638b6e7da4d546a552048 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 11:43:30 -0700 Subject: [PATCH 18/40] remove unnecessaray istatus check --- ROSCO/src/Controllers.f90 | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/ROSCO/src/Controllers.f90 b/ROSCO/src/Controllers.f90 index 629aadd1..05b99692 100644 --- a/ROSCO/src/Controllers.f90 +++ b/ROSCO/src/Controllers.f90 @@ -57,11 +57,7 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar, ErrVar) 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 - LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_FinePit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%PitCom(1), LocalVar%piP, LocalVar%restart, objInst%instPI) - ELSE - LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, LocalVar%PC_MinPit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%BlPitch(1), LocalVar%piP, LocalVar%restart, objInst%instPI) - END IF + LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, LocalVar%PC_MinPit, LocalVar%PC_MaxPit, LocalVar%DT, LocalVar%BlPitch(1), LocalVar%piP, LocalVar%restart, objInst%instPI) DebugVar%PC_PICommand = LocalVar%PC_PitComT ! Find individual pitch control contribution IF ((CntrPar%IPC_ControlMode >= 1) .OR. (CntrPar%Y_ControlMode == 2)) THEN From 492348c9eebb3d7f46b7445eea10d8da06343c4f Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 11:46:26 -0700 Subject: [PATCH 19/40] close files --- ROSCO_toolbox/ofTools/fast_io/FAST_reader.py | 1 + ROSCO_toolbox/ofTools/util/FileTools.py | 1 + 2 files changed, 2 insertions(+) diff --git a/ROSCO_toolbox/ofTools/fast_io/FAST_reader.py b/ROSCO_toolbox/ofTools/fast_io/FAST_reader.py index 9297464e..18a220fa 100644 --- a/ROSCO_toolbox/ofTools/fast_io/FAST_reader.py +++ b/ROSCO_toolbox/ofTools/fast_io/FAST_reader.py @@ -2239,6 +2239,7 @@ def read_MoorDyn(self): channel_list = channels.split(',') self.set_outlist(self.fst_vt['outlist']['MoorDyn'], channel_list) data = f.readline() + f.close() class InputReader_FAST7(InputReader_Common): diff --git a/ROSCO_toolbox/ofTools/util/FileTools.py b/ROSCO_toolbox/ofTools/util/FileTools.py index b66ac522..2262f10e 100644 --- a/ROSCO_toolbox/ofTools/util/FileTools.py +++ b/ROSCO_toolbox/ofTools/util/FileTools.py @@ -127,6 +127,7 @@ def save_yaml(outdir, fname, data_out): yaml.width = float("inf") yaml.indent(mapping=4, sequence=6, offset=3) yaml.dump(data_out, f) + f.close() def select_cases(cases, var_sel, val_sel): From 7897a83412ca005ee95e31c26879042d7124460c Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 11:51:03 -0700 Subject: [PATCH 20/40] add reg test for restart --- ROSCO_testing/regtest.py | 124 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 124 insertions(+) create mode 100644 ROSCO_testing/regtest.py diff --git a/ROSCO_testing/regtest.py b/ROSCO_testing/regtest.py new file mode 100644 index 00000000..12b89310 --- /dev/null +++ b/ROSCO_testing/regtest.py @@ -0,0 +1,124 @@ +''' +Regression testing for ROSCO + +Tests: + restart +''' + +import os +import platform +import unittest +import numpy as np + +# Python Modules +import os +import platform +from shutil import copyfile + +# ROSCO toolbox modules +from ROSCO_toolbox.inputs.validation import load_rosco_yaml +from ROSCO_toolbox.ofTools.fast_io import output_processing +from ROSCO_toolbox.ofTools.case_gen.CaseLibrary import set_channels +from ROSCO_toolbox.ofTools.case_gen.CaseGen_General import CaseGen_General +from ROSCO_toolbox.ofTools.fast_io.FAST_writer import InputReader_OpenFAST, InputWriter_OpenFAST +from ROSCO_toolbox.utilities import run_openfast + + +class RegressionTesting(unittest.TestCase): + def test_restart(self): + this_dir = os.path.dirname(os.path.abspath(__file__)) + rosco_dir = os.path.dirname(this_dir) + test_out_dir = os.path.join(this_dir, 'test_out') + + # Load yaml file (Open Loop Case) + parameter_filename = os.path.join(rosco_dir, 'Tune_Cases/IEA15MW.yaml') + + inps = load_rosco_yaml(parameter_filename) + path_params = inps['path_params'] + turbine_params = inps['turbine_params'] + controller_params = inps['controller_params'] + + # Set rosco_dll + if platform.system() == 'Windows': + rosco_dll = os.path.join(rosco_dir, 'ROSCO/build/libdiscon.dll') + elif platform.system() == 'Darwin': + rosco_dll = os.path.join(rosco_dir, 'ROSCO/build/libdiscon.dylib') + else: + rosco_dll = os.path.join(rosco_dir, 'ROSCO/build/libdiscon.so') + + case_inputs = {} + case_inputs[('Fst', 'TMax')] = {'vals': [3.], 'group': 0} + case_inputs[('ServoDyn', 'DLL_FileName')] = {'vals': [rosco_dll], 'group': 0} + case_inputs[('Fst', 'ChkptTime')] = {'vals': [1.], 'group': 1} + case_inputs[('Fst', 'OutFileFmt')] = {'vals': [2], 'group': 1} + case_inputs[('DISCON_in', 'LoggingLevel')] = {'vals': [2], 'group': 1} + + # Generate cases + run_dir = os.path.join(test_out_dir, 'restart') + if not os.path.exists(run_dir): + os.makedirs(run_dir) + + case_list, case_name_list = CaseGen_General(case_inputs, dir_matrix=run_dir, namebase='iea15_restart') + channels = set_channels() + + # write files + reader = InputReader_OpenFAST() + writer = InputWriter_OpenFAST() + reader.FAST_InputFile = path_params['FAST_InputFile'] + reader.FAST_directory = os.path.realpath(os.path.join( rosco_dir, 'Tune_Cases', path_params['FAST_directory'])) + reader.execute() + writer.fst_vt = reader.fst_vt + writer.FAST_runDirectory = test_out_dir + for case, case_name in zip(case_list, case_name_list): + writer.FAST_namingOut = case_name + writer.update(fst_update=case) + writer.update_outlist(channels) + writer.execute() + + # Run first case + fastcall = '/Users/nabbas/.conda/envs/rosco-env/bin/openfast' + run_openfast( + test_out_dir, + fastcall=fastcall, + fastfile=writer.FAST_InputFileOut, + chdir=True + ) + copyfile(os.path.join(test_out_dir, writer.FAST_InputFileOut[:-3]+'outb'), + os.path.join(test_out_dir, 'orig.outb') ) + # copyfile(os.path.join(test_out_dir, writer.FAST_InputFileOut[:-3]+'RO.dbg'), + # os.path.join(test_out_dir, 'orig.dbg') ) + + # run restart case + run_openfast( + test_out_dir, + fastcall=fastcall, + fastfile=writer.FAST_InputFileOut[:-3]+'40', + chdir=True, + restart=True + ) + copyfile(os.path.join(test_out_dir, writer.FAST_InputFileOut[:-3]+'outb'), + os.path.join(test_out_dir, 'restart.outb')) + # copyfile(os.path.join(test_out_dir, writer.FAST_InputFileOut[:-3]+'RO.dbg'), + # os.path.join(test_out_dir, 'restart.dbg')) + + output_files = [os.path.join(test_out_dir, 'orig.outb'), + os.path.join(test_out_dir, 'restart.outb')] + op = output_processing.output_processing() + fastout = op.load_fast_out(output_files, tmin=0) + + if False: # Plotting for debug + import matplotlib.pyplot as plt + cases = {} + cases['Baseline'] = ['Wind1VelX', 'BldPitch1', 'GenTq', 'RotSpeed', 'NacYaw'] + fig, ax = op.plot_fast_out(cases=cases, showplot=False) + plt.show() + + self.check_relative_error(fastout[1]['GenPwr'], fastout[0]['GenPwr'], 1e-3) + + def check_relative_error(self, meas, real, tol): + '''check relative error''' + error = np.linalg.norm(meas - real) / np.linalg.norm(real) + self.assertTrue(error<=tol) + +if __name__ == "__main__": + unittest.main() From 969beb64540e70e394a14d438d13904c55a465f8 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 11:51:19 -0700 Subject: [PATCH 21/40] add restart option to run_openfast --- ROSCO_toolbox/utilities.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ROSCO_toolbox/utilities.py b/ROSCO_toolbox/utilities.py index d5aa0982..4eeac653 100644 --- a/ROSCO_toolbox/utilities.py +++ b/ROSCO_toolbox/utilities.py @@ -475,7 +475,7 @@ def DISCON_dict(turbine, controller, txt_filename=None): return DISCON_dict -def run_openfast(fast_dir, fastcall='openfast', fastfile=None, chdir=True): +def run_openfast(fast_dir, fastcall='openfast', fastfile=None, chdir=True, restart=False): ''' Runs a openfast openfast simulation. @@ -509,7 +509,10 @@ def run_openfast(fast_dir, fastcall='openfast', fastfile=None, chdir=True): print('Running OpenFAST simulation for {} through the ROSCO toolbox...'.format(fastfile)) # os.system('{} {}'.format(fastcall, os.path.join(fastfile))) - subprocess.run([fastcall, os.path.join(fastfile)], check=True, cwd=cwd) + if restart: + subprocess.run([fastcall,'-restart', os.path.join(fastfile)], check=True, cwd=cwd) + else: + subprocess.run([fastcall, os.path.join(fastfile)], check=True, cwd=cwd) print('OpenFAST simulation complete.') From ace827ded8f3e6d95ba244cc255dc99ec93e1012 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 11:53:45 -0700 Subject: [PATCH 22/40] add testing to CI, ignore generate files --- .github/workflows/CI_rosco-pytools.yml | 1 + .gitignore | 1 + 2 files changed, 2 insertions(+) diff --git a/.github/workflows/CI_rosco-pytools.yml b/.github/workflows/CI_rosco-pytools.yml index 77a029ab..47357e84 100644 --- a/.github/workflows/CI_rosco-pytools.yml +++ b/.github/workflows/CI_rosco-pytools.yml @@ -160,3 +160,4 @@ jobs: run: | cd ROSCO_testing python ROSCO_testing.py + python regtest.py diff --git a/.gitignore b/.gitignore index 524a244a..be2ed050 100644 --- a/.gitignore +++ b/.gitignore @@ -11,6 +11,7 @@ Source/Release/ *build* Makefile */install/* +*test_out* # Archive Scripts/CompileDISCONHereCopyRun\.cmd From 134776701dbfd39d595b5c9929a081c3fd30f27a Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 12:14:41 -0700 Subject: [PATCH 23/40] fix fastcall --- ROSCO_testing/regtest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROSCO_testing/regtest.py b/ROSCO_testing/regtest.py index 12b89310..a0a56fbb 100644 --- a/ROSCO_testing/regtest.py +++ b/ROSCO_testing/regtest.py @@ -76,7 +76,7 @@ def test_restart(self): writer.execute() # Run first case - fastcall = '/Users/nabbas/.conda/envs/rosco-env/bin/openfast' + fastcall = 'openfast' run_openfast( test_out_dir, fastcall=fastcall, From 67ada2968632fdc111b09b6167f201c56ef1d2fe Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 12:21:31 -0700 Subject: [PATCH 24/40] remove extra commas --- ROSCO/rosco_registry/write_registry.py | 4 ++-- ROSCO/src/ROSCO_IO.f90 | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROSCO/rosco_registry/write_registry.py b/ROSCO/rosco_registry/write_registry.py index 380a26be..f3f37ad4 100644 --- a/ROSCO/rosco_registry/write_registry.py +++ b/ROSCO/rosco_registry/write_registry.py @@ -62,7 +62,7 @@ def write_roscoio(yfile): file.write(" CHARACTER(128) :: ErrMsg \n") file.write(" CHARACTER(128) :: n_t_global ! timestep number as a string\n") file.write("\n") - file.write(" WRITE(n_t_global, '(I0.0)' ), NINT(LocalVar%Time/LocalVar%DT)\n") + file.write(" WRITE(n_t_global, '(I0.0)' ) NINT(LocalVar%Time/LocalVar%DT)\n") file.write(" InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp'\n") file.write(" OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='WRITE' )\n") file.write("\n") @@ -103,7 +103,7 @@ def write_roscoio(yfile): file.write(" CHARACTER(128) :: ErrMsg \n") file.write(" CHARACTER(128) :: n_t_global ! timestep number as a string\n") file.write("\n") - file.write(" WRITE(n_t_global, '(I0.0)' ), NINT(avrSWAP(2)/avrSWAP(3))\n") + file.write(" WRITE(n_t_global, '(I0.0)' ) NINT(avrSWAP(2)/avrSWAP(3))\n") file.write(" InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp'\n") file.write(" OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='READ' )\n") file.write("\n") diff --git a/ROSCO/src/ROSCO_IO.f90 b/ROSCO/src/ROSCO_IO.f90 index 98510c21..71f253ca 100644 --- a/ROSCO/src/ROSCO_IO.f90 +++ b/ROSCO/src/ROSCO_IO.f90 @@ -25,7 +25,7 @@ SUBROUTINE WriteRestartFile(LocalVar, CntrPar, objInst, RootName, size_avcOUTNAM CHARACTER(128) :: ErrMsg CHARACTER(128) :: n_t_global ! timestep number as a string - WRITE(n_t_global, '(I0.0)' ), NINT(LocalVar%Time/LocalVar%DT) + WRITE(n_t_global, '(I0.0)' ) NINT(LocalVar%Time/LocalVar%DT) InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp' OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='WRITE' ) @@ -196,7 +196,7 @@ SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootNa CHARACTER(128) :: ErrMsg CHARACTER(128) :: n_t_global ! timestep number as a string - WRITE(n_t_global, '(I0.0)' ), NINT(avrSWAP(2)/avrSWAP(3)) + WRITE(n_t_global, '(I0.0)' ) NINT(avrSWAP(2)/avrSWAP(3)) InFile = RootName(1:size_avcOUTNAME-5)//TRIM( n_t_global )//'.RO.chkp' OPEN(unit=Un, FILE=TRIM(InFile), STATUS='UNKNOWN', FORM='UNFORMATTED' , ACCESS='STREAM', IOSTAT=ErrStat, ACTION='READ' ) From 4c3154491523bcd542133ca2bce5803cdc94523f Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 12:46:31 -0700 Subject: [PATCH 25/40] specify gfortran-10 --- .github/workflows/CI_rosco-pytools.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/CI_rosco-pytools.yml b/.github/workflows/CI_rosco-pytools.yml index 47357e84..dc881260 100644 --- a/.github/workflows/CI_rosco-pytools.yml +++ b/.github/workflows/CI_rosco-pytools.yml @@ -5,7 +5,7 @@ on: [push, pull_request] # Specify FORTRAN compiler env: - FC: gfortran + FC: gfortran-10 # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: From 6f295563832413e96347acd82716f9aadac6d46c Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 13:47:55 -0700 Subject: [PATCH 26/40] testing flag cleanup --- setup.py | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/setup.py b/setup.py index 716f9fbb..2b4517f2 100644 --- a/setup.py +++ b/setup.py @@ -84,25 +84,26 @@ def build_extension(self, ext): localdir = os.path.join(this_directory, 'ROSCO','install') os.makedirs(localdir, exist_ok=True) - cmake_args = ['-DBUILD_SHARED_LIBS=OFF'] - cmake_args += ['-DCMAKE_Fortran_FLAGS=-ffree-line-length-0'] - cmake_args += ['-DCMAKE_INSTALL_PREFIX={}'.format(localdir)] - - if platform.system() == 'Windows': - if "gfortran" in os.environ["FC"].lower(): - cmake_args += ['-G', 'MinGW Makefiles'] - elif self.compiler.compiler_type == 'msvc': - cmake_args += ['-DCMAKE_GENERATOR_PLATFORM=x64'] - else: - raise ValueError("Unable to find the system's Fortran compiler.") + # cmake_args = ['-DBUILD_SHARED_LIBS=OFF'] + # cmake_args += ['-DCMAKE_Fortran_FLAGS=-ffree-line-length-0'] + # cmake_args += ['-DCMAKE_INSTALL_PREFIX={}'.format(localdir)] + + # if platform.system() == 'Windows': + # if "gfortran" in os.environ["FC"].lower(): + # cmake_args += ['-G', 'MinGW Makefiles'] + # elif self.compiler.compiler_type == 'msvc': + # cmake_args += ['-DCMAKE_GENERATOR_PLATFORM=x64'] + # else: + # raise ValueError("Unable to find the system's Fortran compiler.") self.build_temp = os.path.join( os.path.dirname( os.path.realpath(__file__) ), 'ROSCO', 'build') os.makedirs(localdir, exist_ok=True) # Need fresh build directory for CMake os.makedirs(self.build_temp, exist_ok=True) - self.spawn(['cmake', '-S', ext.sourcedir, '-B', self.build_temp] + cmake_args) - self.spawn(['cmake', '--build', self.build_temp, '--target', 'install', '--config', 'Release']) + # self.spawn(['cmake', '-S', ext.sourcedir, '-B', self.build_temp] + cmake_args) + self.spawn(['cmake', '-B', self.build_temp, os.path.join(this_directory, 'ROSCO')]) + self.spawn(['cmake', '--build', self.build_temp, '--target', 'install']) From 3e432063d4412e16d2fa7dc32ab906b7f2f9b1e9 Mon Sep 17 00:00:00 2001 From: dzalkind Date: Tue, 21 Dec 2021 14:29:05 -0700 Subject: [PATCH 27/40] Use lv_strings to generate debug output --- ROSCO/rosco_registry/write_registry.py | 11 +++++------ ROSCO/src/ROSCO_IO.f90 | 12 ++++++------ 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/ROSCO/rosco_registry/write_registry.py b/ROSCO/rosco_registry/write_registry.py index f3f37ad4..ba786dd3 100644 --- a/ROSCO/rosco_registry/write_registry.py +++ b/ROSCO/rosco_registry/write_registry.py @@ -194,12 +194,11 @@ def write_roscoio(yfile): file.write(' nLocalVars = {}\n'.format(len(lv_strings))) file.write(' Allocate(LocalVarOutData(nLocalVars))\n') file.write(' Allocate(LocalVarOutStrings(nLocalVars))\n') - for lv_idx, localvar in enumerate(reg['LocalVariables']): - if reg['LocalVariables'][localvar]['type'] in ['integer', 'real']: - if reg['LocalVariables'][localvar]['size'] > 0: - file.write(' LocalVarOutData({}) = LocalVar%{}(1)\n'.format(lv_idx+1, localvar)) - else: - file.write(' LocalVarOutData({}) = LocalVar%{}\n'.format(lv_idx+1, localvar)) + for lv_idx, localvar in enumerate(lv_strings): + if reg['LocalVariables'][localvar]['size'] > 0: + file.write(' LocalVarOutData({}) = LocalVar%{}(1)\n'.format(lv_idx+1, localvar)) + else: + file.write(' LocalVarOutData({}) = LocalVar%{}\n'.format(lv_idx+1, localvar)) file.write(' LocalVarOutStrings = [CHARACTER(15) :: ') counter = 0 for string in lv_strings: diff --git a/ROSCO/src/ROSCO_IO.f90 b/ROSCO/src/ROSCO_IO.f90 index 71f253ca..cbc9e91b 100644 --- a/ROSCO/src/ROSCO_IO.f90 +++ b/ROSCO/src/ROSCO_IO.f90 @@ -461,12 +461,12 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME LocalVarOutData(61) = LocalVar%Y_ErrLPFSlow LocalVarOutData(62) = LocalVar%Y_MErr LocalVarOutData(63) = LocalVar%Y_YawEndT - LocalVarOutData(65) = LocalVar%Fl_PitCom - LocalVarOutData(66) = LocalVar%NACIMU_FA_AccF - LocalVarOutData(67) = LocalVar%FA_AccF - LocalVarOutData(68) = LocalVar%Flp_Angle(1) - LocalVarOutData(69) = LocalVar%RootMyb_Last(1) - LocalVarOutData(70) = LocalVar%ACC_INFILE_SIZE + LocalVarOutData(64) = LocalVar%Fl_PitCom + LocalVarOutData(65) = LocalVar%NACIMU_FA_AccF + LocalVarOutData(66) = LocalVar%FA_AccF + LocalVarOutData(67) = LocalVar%Flp_Angle(1) + LocalVarOutData(68) = LocalVar%RootMyb_Last(1) + LocalVarOutData(69) = LocalVar%ACC_INFILE_SIZE LocalVarOutStrings = [CHARACTER(15) :: 'iStatus', 'Time', 'DT', 'VS_GenPwr', 'GenSpeed', & 'RotSpeed', 'Y_M', 'HorWindV', 'rootMOOP', 'BlPitch', & 'Azimuth', 'NumBl', 'FA_Acc', 'NacIMU_FA_Acc', 'FA_AccHPF', & From b1607e0d103bff7f93b3a43851490d979f211b2e Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 15:07:02 -0700 Subject: [PATCH 28/40] Revert "testing flag cleanup" This reverts commit 6f295563832413e96347acd82716f9aadac6d46c. --- setup.py | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/setup.py b/setup.py index 2b4517f2..716f9fbb 100644 --- a/setup.py +++ b/setup.py @@ -84,26 +84,25 @@ def build_extension(self, ext): localdir = os.path.join(this_directory, 'ROSCO','install') os.makedirs(localdir, exist_ok=True) - # cmake_args = ['-DBUILD_SHARED_LIBS=OFF'] - # cmake_args += ['-DCMAKE_Fortran_FLAGS=-ffree-line-length-0'] - # cmake_args += ['-DCMAKE_INSTALL_PREFIX={}'.format(localdir)] - - # if platform.system() == 'Windows': - # if "gfortran" in os.environ["FC"].lower(): - # cmake_args += ['-G', 'MinGW Makefiles'] - # elif self.compiler.compiler_type == 'msvc': - # cmake_args += ['-DCMAKE_GENERATOR_PLATFORM=x64'] - # else: - # raise ValueError("Unable to find the system's Fortran compiler.") + cmake_args = ['-DBUILD_SHARED_LIBS=OFF'] + cmake_args += ['-DCMAKE_Fortran_FLAGS=-ffree-line-length-0'] + cmake_args += ['-DCMAKE_INSTALL_PREFIX={}'.format(localdir)] + + if platform.system() == 'Windows': + if "gfortran" in os.environ["FC"].lower(): + cmake_args += ['-G', 'MinGW Makefiles'] + elif self.compiler.compiler_type == 'msvc': + cmake_args += ['-DCMAKE_GENERATOR_PLATFORM=x64'] + else: + raise ValueError("Unable to find the system's Fortran compiler.") self.build_temp = os.path.join( os.path.dirname( os.path.realpath(__file__) ), 'ROSCO', 'build') os.makedirs(localdir, exist_ok=True) # Need fresh build directory for CMake os.makedirs(self.build_temp, exist_ok=True) - # self.spawn(['cmake', '-S', ext.sourcedir, '-B', self.build_temp] + cmake_args) - self.spawn(['cmake', '-B', self.build_temp, os.path.join(this_directory, 'ROSCO')]) - self.spawn(['cmake', '--build', self.build_temp, '--target', 'install']) + self.spawn(['cmake', '-S', ext.sourcedir, '-B', self.build_temp] + cmake_args) + self.spawn(['cmake', '--build', self.build_temp, '--target', 'install', '--config', 'Release']) From 24e5daf9e9f09c82b93b8d3938f9b4d4431f2ac6 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 15:22:05 -0700 Subject: [PATCH 29/40] Revert "specify gfortran-10" This reverts commit 4c3154491523bcd542133ca2bce5803cdc94523f. --- .github/workflows/CI_rosco-pytools.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/CI_rosco-pytools.yml b/.github/workflows/CI_rosco-pytools.yml index dc881260..47357e84 100644 --- a/.github/workflows/CI_rosco-pytools.yml +++ b/.github/workflows/CI_rosco-pytools.yml @@ -5,7 +5,7 @@ on: [push, pull_request] # Specify FORTRAN compiler env: - FC: gfortran-10 + FC: gfortran # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: From 76caca3aca235f378798c5d91878d03e1fee544c Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Tue, 21 Dec 2021 15:34:35 -0700 Subject: [PATCH 30/40] minor cleanup --- ROSCO/rosco_registry/write_registry.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/ROSCO/rosco_registry/write_registry.py b/ROSCO/rosco_registry/write_registry.py index ba786dd3..fd43e899 100644 --- a/ROSCO/rosco_registry/write_registry.py +++ b/ROSCO/rosco_registry/write_registry.py @@ -86,7 +86,6 @@ def write_roscoio(yfile): file.write('END SUBROUTINE WriteRestartFile\n') file.write('\n \n') file.write('SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootName, size_avcOUTNAME, ErrVar)\n') - # file.write(' USE ROSCO_Types, ONLY: LocalVariables, ControlParameters, ObjectInstances, PerformanceData, ErrorVariables\n') file.write(" TYPE(LocalVariables), INTENT(INOUT) :: LocalVar\n") file.write(" TYPE(ControlParameters), INTENT(INOUT) :: CntrPar\n") file.write(" TYPE(ObjectInstances), INTENT(INOUT) :: objInst\n") @@ -213,8 +212,6 @@ def write_roscoio(yfile): file.write(" ! Initialize debug file\n") file.write(" IF ((LocalVar%iStatus == 0) .OR. (LocalVar%iStatus == -9)) THEN ! .TRUE. if we're on the first call to the DLL\n") - file.write(" ! If we're debugging, open the debug file and write the header:\n") - file.write(" ! Note that the headers will be Truncated to 10 characters!!\n") file.write(" IF (CntrPar%LoggingLevel > 0) THEN\n") file.write(" OPEN(unit=UnDb, FILE=RootName(1: size_avcOUTNAME-5)//'RO.dbg')\n") file.write(" WRITE(UnDb, *) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version)\n") From 4b6041a7c339ab58a535e92a99e4836fdea363a0 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Wed, 22 Dec 2021 07:39:55 -0700 Subject: [PATCH 31/40] Use kind from constants --- ROSCO/src/Filters.f90 | 72 ++++++++++++++++++++--------------------- ROSCO/src/Functions.f90 | 48 +++++++++++++-------------- 2 files changed, 60 insertions(+), 60 deletions(-) diff --git a/ROSCO/src/Filters.f90 b/ROSCO/src/Filters.f90 index 1987906f..1e79701c 100644 --- a/ROSCO/src/Filters.f90 +++ b/ROSCO/src/Filters.f90 @@ -26,7 +26,7 @@ MODULE Filters CONTAINS !------------------------------------------------------------------------------------------------------------------------------- - REAL FUNCTION LPFilter(InputSignal, DT, CornerFreq, FP, iStatus, reset, inst) + REAL(DbKi) FUNCTION LPFilter(InputSignal, DT, CornerFreq, FP, iStatus, reset, inst) ! Discrete time Low-Pass Filter of the form: ! Continuous Time Form: H(s) = CornerFreq/(1 + CornerFreq) ! Discrete Time Form: H(z) = (b1z + b0) / (a1*z + a0) @@ -34,11 +34,11 @@ REAL FUNCTION LPFilter(InputSignal, DT, CornerFreq, FP, iStatus, reset, inst) USE ROSCO_Types, ONLY : FilterParameters TYPE(FilterParameters), INTENT(INOUT) :: FP - REAL(8), INTENT(IN) :: InputSignal - REAL(8), INTENT(IN) :: DT ! time step [s] - REAL(8), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] - INTEGER(4), INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. - INTEGER(4), INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. + REAL(DbKi), INTENT(IN) :: InputSignal + REAL(DbKi), INTENT(IN) :: DT ! time step [s] + REAL(DbKi), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] + INTEGER(IntKi), INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. + INTEGER(IntKi), INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. LOGICAL(4), INTENT(IN) :: reset ! Reset the filter to the input signal ! Initialization @@ -63,18 +63,18 @@ REAL FUNCTION LPFilter(InputSignal, DT, CornerFreq, FP, iStatus, reset, inst) END FUNCTION LPFilter !------------------------------------------------------------------------------------------------------------------------------- - REAL FUNCTION SecLPFilter(InputSignal, DT, CornerFreq, Damp, FP, iStatus, reset, inst) + REAL(DbKi) FUNCTION SecLPFilter(InputSignal, DT, CornerFreq, Damp, FP, iStatus, reset, inst) ! Discrete time Low-Pass Filter of the form: ! Continuous Time Form: H(s) = CornerFreq^2/(s^2 + 2*CornerFreq*Damp*s + CornerFreq^2) ! Discrete Time From: H(z) = (b2*z^2 + b1*z + b0) / (a2*z^2 + a1*z + a0) USE ROSCO_Types, ONLY : FilterParameters TYPE(FilterParameters), INTENT(INOUT) :: FP - REAL(8), INTENT(IN) :: InputSignal - REAL(8), INTENT(IN) :: DT ! time step [s] - REAL(8), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] - REAL(8), INTENT(IN) :: Damp ! Dampening constant - INTEGER(4), INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. - INTEGER(4), INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. + REAL(DbKi), INTENT(IN) :: InputSignal + REAL(DbKi), INTENT(IN) :: DT ! time step [s] + REAL(DbKi), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] + REAL(DbKi), INTENT(IN) :: Damp ! Dampening constant + INTEGER(IntKi), INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. + INTEGER(IntKi), INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. LOGICAL(4), INTENT(IN) :: reset ! Reset the filter to the input signal ! Initialization @@ -106,19 +106,19 @@ REAL FUNCTION SecLPFilter(InputSignal, DT, CornerFreq, Damp, FP, iStatus, reset, END FUNCTION SecLPFilter !------------------------------------------------------------------------------------------------------------------------------- - REAL FUNCTION HPFilter( InputSignal, DT, CornerFreq, FP, iStatus, reset, inst) + REAL(DbKi) FUNCTION HPFilter( InputSignal, DT, CornerFreq, FP, iStatus, reset, inst) ! Discrete time High-Pass Filter USE ROSCO_Types, ONLY : FilterParameters TYPE(FilterParameters), INTENT(INOUT) :: FP - REAL(8), INTENT(IN) :: InputSignal - REAL(8), INTENT(IN) :: DT ! time step [s] - REAL(8), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] - INTEGER, INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. - INTEGER, INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. + REAL(DbKi), INTENT(IN) :: InputSignal + REAL(DbKi), INTENT(IN) :: DT ! time step [s] + REAL(DbKi), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] + INTEGER(IntKi), INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. + INTEGER(IntKi), INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. LOGICAL(4), INTENT(IN) :: reset ! Reset the filter to the input signal ! Local - REAL(8) :: K ! Constant gain + REAL(DbKi) :: K ! Constant gain ! Initialization IF ((iStatus == 0) .OR. reset) THEN @@ -137,17 +137,17 @@ REAL FUNCTION HPFilter( InputSignal, DT, CornerFreq, FP, iStatus, reset, inst) END FUNCTION HPFilter !------------------------------------------------------------------------------------------------------------------------------- - REAL FUNCTION NotchFilterSlopes(InputSignal, DT, CornerFreq, Damp, FP, iStatus, reset, inst) + REAL(DbKi) FUNCTION NotchFilterSlopes(InputSignal, DT, CornerFreq, Damp, FP, iStatus, reset, inst) ! Discrete time inverted Notch Filter with descending slopes, G = CornerFreq*s/(Damp*s^2+CornerFreq*s+Damp*CornerFreq^2) USE ROSCO_Types, ONLY : FilterParameters TYPE(FilterParameters), INTENT(INOUT) :: FP - REAL(8), INTENT(IN) :: InputSignal - REAL(8), INTENT(IN) :: DT ! time step [s] - REAL(8), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] - REAL(8), INTENT(IN) :: Damp ! Dampening constant - INTEGER, INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. - INTEGER, INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. + REAL(DbKi), INTENT(IN) :: InputSignal + REAL(DbKi), INTENT(IN) :: DT ! time step [s] + REAL(DbKi), INTENT(IN) :: CornerFreq ! corner frequency [rad/s] + REAL(DbKi), INTENT(IN) :: Damp ! Dampening constant + INTEGER(IntKi), INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. + INTEGER(IntKi), INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. LOGICAL(4), INTENT(IN) :: reset ! Reset the filter to the input signal ! Initialization @@ -175,23 +175,23 @@ REAL FUNCTION NotchFilterSlopes(InputSignal, DT, CornerFreq, Damp, FP, iStatus, END FUNCTION NotchFilterSlopes !------------------------------------------------------------------------------------------------------------------------------- - REAL FUNCTION NotchFilter(InputSignal, DT, omega, betaNum, betaDen, FP, iStatus, reset, inst) + REAL(DbKi) FUNCTION NotchFilter(InputSignal, DT, omega, betaNum, betaDen, FP, iStatus, reset, inst) ! Discrete time Notch Filter ! Continuous Time Form: G(s) = (s^2 + 2*omega*betaNum*s + omega^2)/(s^2 + 2*omega*betaDen*s + omega^2) ! Discrete Time Form: H(z) = (b2*z^2 +b1*z^2 + b0*z)/((z^2 +a1*z^2 + a0*z)) USE ROSCO_Types, ONLY : FilterParameters TYPE(FilterParameters), INTENT(INOUT) :: FP - REAL(8), INTENT(IN) :: InputSignal - REAL(8), INTENT(IN) :: DT ! time step [s] - REAL(8), INTENT(IN) :: omega ! corner frequency [rad/s] - REAL(8), INTENT(IN) :: betaNum ! Dampening constant in numerator of filter transfer function - REAL(8), INTENT(IN) :: betaDen ! Dampening constant in denominator of filter transfer function - INTEGER, INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. - INTEGER, INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. + REAL(DbKi), INTENT(IN) :: InputSignal + REAL(DbKi), INTENT(IN) :: DT ! time step [s] + REAL(DbKi), INTENT(IN) :: omega ! corner frequency [rad/s] + REAL(DbKi), INTENT(IN) :: betaNum ! Dampening constant in numerator of filter transfer function + REAL(DbKi), INTENT(IN) :: betaDen ! Dampening constant in denominator of filter transfer function + INTEGER(IntKi), INTENT(IN) :: iStatus ! A status flag set by the simulation as follows: 0 if this is the first call, 1 for all subsequent time steps, -1 if this is the final call at the end of the simulation. + INTEGER(IntKi), INTENT(INOUT) :: inst ! Instance number. Every instance of this function needs to have an unique instance number to ensure instances don't influence each other. LOGICAL(4), INTENT(IN) :: reset ! Reset the filter to the input signal ! Local - REAL(8) :: K ! Constant gain + REAL(DbKi) :: K ! Constant gain ! Initialization K = 2.0/DT diff --git a/ROSCO/src/Functions.f90 b/ROSCO/src/Functions.f90 index d0361ac7..754206a2 100644 --- a/ROSCO/src/Functions.f90 +++ b/ROSCO/src/Functions.f90 @@ -67,26 +67,26 @@ REAL(DbKi) FUNCTION ratelimit(inputSignal, inputSignalPrev, minRate, maxRate, DT END FUNCTION ratelimit !------------------------------------------------------------------------------------------------------------------------------- - REAL FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, piP, reset, inst) + REAL(DbKi) FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, piP, reset, inst) USE ROSCO_Types, ONLY : piParams ! PI controller, with output saturation IMPLICIT NONE ! Allocate Inputs - REAL(8), INTENT(IN) :: error - REAL(8), INTENT(IN) :: kp - REAL(8), INTENT(IN) :: ki - REAL(8), INTENT(IN) :: minValue - REAL(8), INTENT(IN) :: maxValue - REAL(8), INTENT(IN) :: DT - INTEGER(4), INTENT(INOUT) :: inst - REAL(8), INTENT(IN) :: I0 + REAL(DbKi), INTENT(IN) :: error + REAL(DbKi), INTENT(IN) :: kp + REAL(DbKi), INTENT(IN) :: ki + REAL(DbKi), INTENT(IN) :: minValue + REAL(DbKi), INTENT(IN) :: maxValue + REAL(DbKi), INTENT(IN) :: DT + INTEGER(IntKi), INTENT(INOUT) :: inst + REAL(DbKi), INTENT(IN) :: I0 TYPE(piParams), INTENT(INOUT) :: piP LOGICAL, INTENT(IN) :: reset ! Allocate local variables - INTEGER(4) :: i ! Counter for making arrays - REAL(8) :: PTerm ! Proportional term + INTEGER(IntKi) :: i ! Counter for making arrays + REAL(DbKi) :: PTerm ! Proportional term ! Initialize persistent variables/arrays, and set inital condition for integrator term IF (reset) THEN @@ -107,28 +107,28 @@ REAL FUNCTION PIController(error, kp, ki, minValue, maxValue, DT, I0, piP, reset END FUNCTION PIController !------------------------------------------------------------------------------------------------------------------------------- - REAL(8) FUNCTION PIIController(error, error2, kp, ki, ki2, minValue, maxValue, DT, I0, piP, reset, inst) + REAL(DbKi) FUNCTION PIIController(error, error2, kp, ki, ki2, minValue, maxValue, DT, I0, piP, reset, inst) ! PI controller, with output saturation. ! Added error2 term for additional integral control input USE ROSCO_Types, ONLY : piParams IMPLICIT NONE ! Allocate Inputs - REAL(8), INTENT(IN) :: error - REAL(8), INTENT(IN) :: error2 - REAL(8), INTENT(IN) :: kp - REAL(8), INTENT(IN) :: ki2 - REAL(8), INTENT(IN) :: ki - REAL(8), INTENT(IN) :: minValue - REAL(8), INTENT(IN) :: maxValue - REAL(8), INTENT(IN) :: DT - INTEGER(4), INTENT(INOUT) :: inst - REAL(8), INTENT(IN) :: I0 + REAL(DbKi), INTENT(IN) :: error + REAL(DbKi), INTENT(IN) :: error2 + REAL(DbKi), INTENT(IN) :: kp + REAL(DbKi), INTENT(IN) :: ki2 + REAL(DbKi), INTENT(IN) :: ki + REAL(DbKi), INTENT(IN) :: minValue + REAL(DbKi), INTENT(IN) :: maxValue + REAL(DbKi), INTENT(IN) :: DT + INTEGER(IntKi), INTENT(INOUT) :: inst + REAL(DbKi), INTENT(IN) :: I0 TYPE(piParams), INTENT(INOUT) :: piP LOGICAL, INTENT(IN) :: reset ! Allocate local variables - INTEGER(4) :: i ! Counter for making arrays - REAL(8) :: PTerm ! Proportional term + INTEGER(IntKi) :: i ! Counter for making arrays + REAL(DbKi) :: PTerm ! Proportional term ! Initialize persistent variables/arrays, and set inital condition for integrator term IF (reset) THEN From 878b1437dee16d05a7a945c795c04d4975b51d10 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Wed, 22 Dec 2021 07:53:37 -0700 Subject: [PATCH 32/40] Add some comments for clarity --- ROSCO/rosco_registry/write_registry.py | 46 ++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/ROSCO/rosco_registry/write_registry.py b/ROSCO/rosco_registry/write_registry.py index fd43e899..d1957748 100644 --- a/ROSCO/rosco_registry/write_registry.py +++ b/ROSCO/rosco_registry/write_registry.py @@ -4,14 +4,27 @@ from ROSCO_toolbox.ofTools.util.FileTools import load_yaml def generate(yfile): + ''' + Generates full registry and ROSCO I/O files + ''' write_types(yfile) write_roscoio(yfile) def write_types(yfile): + ''' + Writes ROSCO_types.f90 + -- All additions to the types should be added to rosco_types.yaml! + + Parameters: + ----------- + yfile: string + path to rosco_types.yaml + ''' reg = load_yaml(yfile) reg.pop('default_types') registry_fname = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))),'src','ROSCO_Types.f90') file = open(registry_fname, 'w') + # Header file.write('! ROSCO Registry\n') file.write('! This file is automatically generated by write_registry.py using ROSCO v{}\n'.format(ROSCO_toolbox.__version__)) file.write('! For any modification to the registry, please edit the rosco_types.yaml accordingly\n \n') @@ -20,6 +33,7 @@ def write_types(yfile): file.write('USE Constants\n') file.write('IMPLICIT NONE\n') file.write('\n') + # Loop Types for toptype in reg.keys(): file.write('TYPE, PUBLIC :: {}\n'.format(toptype)) for attype in reg[toptype].keys(): @@ -34,6 +48,15 @@ def write_types(yfile): file.close() def write_roscoio(yfile): + ''' + Writes ROSCO_IO.f90 + -- All additions to the types should be added to rosco_types.yaml! + + Parameters: + ----------- + yfile: string + path to rosco_types.yaml + ''' reg = load_yaml(yfile) reg.pop('default_types') registry_fname = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))),'src','ROSCO_IO.f90') @@ -48,6 +71,9 @@ def write_roscoio(yfile): file.write(' USE Constants\n') file.write('IMPLICIT NONE\n\n') file.write('CONTAINS\n\n') + # ------------------------------------------------ + # ------------ WriteRestartFile ------------------ + # ------------------------------------------------ file.write('SUBROUTINE WriteRestartFile(LocalVar, CntrPar, objInst, RootName, size_avcOUTNAME)\n') file.write(" TYPE(LocalVariables), INTENT(IN) :: LocalVar\n") file.write(" TYPE(ControlParameters), INTENT(INOUT) :: CntrPar\n") @@ -70,6 +96,8 @@ def write_roscoio(yfile): file.write(" ErrMsg = 'Cannot open file "'//TRIM( InFile )//'". Another program may have locked it for writing.'\n") file.write("\n") file.write(" ELSE\n") + + # Loop Variable Tree for var in reg['LocalVariables']: if reg['LocalVariables'][var]['type'] == 'derived_type': for dvar in reg[reg['LocalVariables'][var]['id']]: @@ -85,6 +113,9 @@ def write_roscoio(yfile): file.write(' ENDIF\n') file.write('END SUBROUTINE WriteRestartFile\n') file.write('\n \n') + # ------------------------------------------------ + # ------------ ReadRestartFile ------------------ + # ------------------------------------------------ file.write('SUBROUTINE ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootName, size_avcOUTNAME, ErrVar)\n') file.write(" TYPE(LocalVariables), INTENT(INOUT) :: LocalVar\n") file.write(" TYPE(ControlParameters), INTENT(INOUT) :: CntrPar\n") @@ -110,6 +141,8 @@ def write_roscoio(yfile): file.write(" ErrMsg = 'Cannot open file "'//TRIM( InFile )//'". Another program may have locked it for writing.'\n") file.write("\n") file.write(" ELSE\n") + + # Loop Variable Tree for var in reg['LocalVariables']: if reg['LocalVariables'][var]['type'] == 'derived_type': for dvar in reg[reg['LocalVariables'][var]['id']]: @@ -132,6 +165,9 @@ def write_roscoio(yfile): file.write(' ENDIF\n') file.write('END SUBROUTINE ReadRestartFile\n') file.write('\n \n') + # ------------------------------------------------ + # ------------------ Debug ----------------------- + # ------------------------------------------------ file.write('SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME)\n') file.write('! Debug routine, defines what gets printed to DEBUG.dbg if LoggingLevel = 1\n') file.write('\n') @@ -159,6 +195,8 @@ def write_roscoio(yfile): file.write(' Allocate(DebugOutUnits(nDebugOuts))\n') dbg_strings = [] dbg_units = [] + + # Print Debug variables for dbg_idx, dbgvar in enumerate(reg['DebugVariables']): dbg_strings.append(dbgvar) desc = reg['DebugVariables'][dbgvar]['description'] @@ -186,6 +224,9 @@ def write_roscoio(yfile): if (counter % 5 == 0): file.write(' & \n ') file.write(']\n') + + # Print Local Variables + # -- Always prints blade(1) for multi-bladed inputss (e.g. BldPitch(1)) lv_strings = [] for lv_idx, localvar in enumerate(reg['LocalVariables']): if reg['LocalVariables'][localvar]['type'] in ['integer', 'real']: @@ -210,8 +251,11 @@ def write_roscoio(yfile): file.write(' & \n ') file.write(']\n') + # Debug file initialization + # -- iStatus = 0 on first call, iStatus = -9 in restart file.write(" ! Initialize debug file\n") file.write(" IF ((LocalVar%iStatus == 0) .OR. (LocalVar%iStatus == -9)) THEN ! .TRUE. if we're on the first call to the DLL\n") + # Standar debug file.write(" IF (CntrPar%LoggingLevel > 0) THEN\n") file.write(" OPEN(unit=UnDb, FILE=RootName(1: size_avcOUTNAME-5)//'RO.dbg')\n") file.write(" WRITE(UnDb, *) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version)\n") @@ -219,6 +263,7 @@ def write_roscoio(yfile): file.write(" WRITE(UnDb, '(99(a20,TR5:))') '(sec)', DebugOutUnits\n") file.write(" END IF\n") file.write("\n") + # LocalVar debug file.write(" IF (CntrPar%LoggingLevel > 1) THEN\n") file.write(" OPEN(unit=UnDb2, FILE=RootName(1: size_avcOUTNAME-5)//'RO.dbg2')\n") file.write(" WRITE(UnDb2, *) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version)\n") @@ -226,6 +271,7 @@ def write_roscoio(yfile): file.write(" WRITE(UnDb2, '(99(a20,TR5:))')\n") file.write(" END IF\n") file.write("\n") + # avrSWAP debug file.write(" IF (CntrPar%LoggingLevel > 2) THEN\n") file.write(" OPEN(unit=UnDb3, FILE=RootName(1: size_avcOUTNAME-5)//'RO.dbg3')\n") file.write(" WRITE(UnDb3,'(/////)')\n") From 59519a1bd25ca67eedaf7c675a84552fb3893fc7 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Wed, 22 Dec 2021 07:55:26 -0700 Subject: [PATCH 33/40] put debug in if statements --- ROSCO/src/DISCON.F90 | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ROSCO/src/DISCON.F90 b/ROSCO/src/DISCON.F90 index e7446688..ecafed9f 100644 --- a/ROSCO/src/DISCON.F90 +++ b/ROSCO/src/DISCON.F90 @@ -67,6 +67,7 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME ! Check for restart IF ( (NINT(avrSWAP(1)) == -9) .AND. (aviFAIL >= 0)) THEN ! Read restart files CALL ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootName, SIZE(avcOUTNAME), ErrVar) + CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) END IF ! Read avrSWAP array into derived types/variables @@ -98,10 +99,10 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME CALL FlapControl(avrSWAP, CntrPar, LocalVar, objInst) END IF + CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) END IF -CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) ! Add RoutineName to error message IF (ErrVar%aviFAIL < 0) THEN From 874971305628e6d5e9e7db99c0b048912f10a984 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Wed, 22 Dec 2021 07:55:37 -0700 Subject: [PATCH 34/40] separate reg tests from oother tests --- .github/workflows/CI_rosco-pytools.yml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/.github/workflows/CI_rosco-pytools.yml b/.github/workflows/CI_rosco-pytools.yml index 47357e84..ad8b8e12 100644 --- a/.github/workflows/CI_rosco-pytools.yml +++ b/.github/workflows/CI_rosco-pytools.yml @@ -155,9 +155,13 @@ jobs: run: | conda install openfast==3.0.0 - # Run Testing - - name: Run testing + # Run ROSCO Testing + - name: Run ROSCO testing run: | cd ROSCO_testing python ROSCO_testing.py + + # Regression testing + - name: Run regression testing + run: | python regtest.py From b5ac62de1f92e4cdeff32b77f17bee3090bc15be Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Wed, 22 Dec 2021 08:01:07 -0700 Subject: [PATCH 35/40] Fl_Mode>0 --- ROSCO/src/Filters.f90 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROSCO/src/Filters.f90 b/ROSCO/src/Filters.f90 index 1e79701c..edc19045 100644 --- a/ROSCO/src/Filters.f90 +++ b/ROSCO/src/Filters.f90 @@ -248,7 +248,7 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst, ErrVar) ENDIF ! Filtering the tower fore-aft acceleration signal - IF (CntrPar%Fl_Mode == 1) THEN + IF (CntrPar%Fl_Mode > 0) THEN ! Force to start at 0 LocalVar%NacIMU_FA_AccF = SecLPFilter(LocalVar%NacIMU_FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping LocalVar%FA_AccF = SecLPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping From 3fdbdad60a1914bea43315819dbbf2d82c757a88 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Wed, 22 Dec 2021 08:02:48 -0700 Subject: [PATCH 36/40] Remove hard coded values --- ROSCO/src/Filters.f90 | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ROSCO/src/Filters.f90 b/ROSCO/src/Filters.f90 index edc19045..1b00968b 100644 --- a/ROSCO/src/Filters.f90 +++ b/ROSCO/src/Filters.f90 @@ -252,8 +252,8 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst, ErrVar) ! Force to start at 0 LocalVar%NacIMU_FA_AccF = SecLPFilter(LocalVar%NacIMU_FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping LocalVar%FA_AccF = SecLPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%F_FlCornerFreq(1), CntrPar%F_FlCornerFreq(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) ! Fixed Damping - LocalVar%NacIMU_FA_AccF = HPFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, 0.0167, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) - LocalVar%FA_AccF = HPFilter(LocalVar%FA_AccF, LocalVar%DT, 0.0167, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) + LocalVar%NacIMU_FA_AccF = HPFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, CntrPar%F_FlHighPassFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) + LocalVar%FA_AccF = HPFilter(LocalVar%FA_AccF, LocalVar%DT, CntrPar%F_FlHighPassFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) IF (CntrPar%F_NotchType >= 2) THEN LocalVar%NACIMU_FA_AccF = NotchFilter(LocalVar%NacIMU_FA_AccF, LocalVar%DT, CntrPar%F_NotchCornerFreq, CntrPar%F_NotchBetaNumDen(1), CntrPar%F_NotchBetaNumDen(2), LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instNotch) ! Fixed Damping @@ -264,12 +264,12 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst, ErrVar) LocalVar%FA_AccHPF = HPFilter(LocalVar%FA_Acc, LocalVar%DT, CntrPar%FA_HPFCornerFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instHPF) ! Filter Wind Speed Estimator Signal - LocalVar%We_Vw_F = LPFilter(LocalVar%WE_Vw, LocalVar%DT, 0.209, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ! 30 second time constant + LocalVar%We_Vw_F = LPFilter(LocalVar%WE_Vw, LocalVar%DT,CntrPar%F_WECornerFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ! 30 second time constant ! Control commands (used by WSE, mostly) - LocalVar%VS_LastGenTrqF = SecLPFilter(LocalVar%VS_LastGenTrq, LocalVar%DT, CntrPar%F_LPFCornerFreq, 0.7, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) - LocalVar%PC_PitComTF = SecLPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%F_LPFCornerFreq*0.25, 0.7, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) + LocalVar%VS_LastGenTrqF = SecLPFilter(LocalVar%VS_LastGenTrq, LocalVar%DT, CntrPar%F_LPFCornerFreq, 0.7_DbKi, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) + LocalVar%PC_PitComTF = SecLPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%F_LPFCornerFreq*0.25, 0.7_DbKi, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) END SUBROUTINE PreFilterMeasuredSignals END MODULE Filters \ No newline at end of file From 52492d3ec9432e306c927dac2e3666ac4c7a2bf1 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Wed, 22 Dec 2021 08:20:25 -0700 Subject: [PATCH 37/40] Add filtered signals and WE_Vw to debug varrs --- ROSCO/rosco_registry/rosco_types.yaml | 18 +++++++++++++++++- ROSCO/src/ControllerBlocks.f90 | 2 +- ROSCO/src/DISCON.F90 | 2 +- ROSCO/src/Filters.f90 | 10 ++++++++-- ROSCO/src/ROSCO_IO.f90 | 19 ++++++++++++------- ROSCO/src/ROSCO_Types.f90 | 5 +++++ 6 files changed, 44 insertions(+), 12 deletions(-) diff --git a/ROSCO/rosco_registry/rosco_types.yaml b/ROSCO/rosco_registry/rosco_types.yaml index 13ebd35b..400ea83c 100644 --- a/ROSCO/rosco_registry/rosco_types.yaml +++ b/ROSCO/rosco_registry/rosco_types.yaml @@ -918,12 +918,28 @@ DebugVariables: WE_Vt: <<: *real description: Turbulent wind speed component in WSE [m/s] + WE_Vw: + <<: *real + description: Estimated wind speed in WSE [m/s] WE_lambda: <<: *real description: TSR in WSE [rad] PC_PICommand: <<: *real - description: Commanded collective pitch from pitch PI controller [rad] + description: Commanded collective pitch from pitch PI controller [rad] + GenSpeedF: + <<: *real + description: Filtered generator speed [rad/s] + RotSpeedF: + <<: *real + description: Filtered rotor speed [rad/s] + NacIMU_FA_AccF: + <<: *real + description: Filtered NacIMU_FA_Acc [rad/s] + FA_AccF: + <<: *real + description: Filtered FA_Acc [m/s] + ErrorVariables: size_avcMSG: diff --git a/ROSCO/src/ControllerBlocks.f90 b/ROSCO/src/ControllerBlocks.f90 index b8031706..a025f0ca 100644 --- a/ROSCO/src/ControllerBlocks.f90 +++ b/ROSCO/src/ControllerBlocks.f90 @@ -323,7 +323,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er ! Filter wind speed at hub height as directly passed from OpenFAST LocalVar%WE_Vw = LPFilter(LocalVar%HorWindV, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instLPF) ENDIF - + DebugVar%WE_Vw = LocalVar%WE_Vw ! Add RoutineName to error message IF (ErrVar%aviFAIL < 0) THEN ErrVar%ErrMsg = RoutineName//':'//TRIM(ErrVar%ErrMsg) diff --git a/ROSCO/src/DISCON.F90 b/ROSCO/src/DISCON.F90 index ecafed9f..07c0ad38 100644 --- a/ROSCO/src/DISCON.F90 +++ b/ROSCO/src/DISCON.F90 @@ -77,7 +77,7 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME CALL SetParameters(avrSWAP, accINFILE, SIZE(avcMSG), CntrPar, LocalVar, objInst, PerfData, ErrVar) ! Filter signals -CALL PreFilterMeasuredSignals(CntrPar, LocalVar, objInst, ErrVar) +CALL PreFilterMeasuredSignals(CntrPar, LocalVar, DebugVar, objInst, ErrVar) IF (((LocalVar%iStatus >= 0) .OR. (LocalVar%iStatus <= -8)) .AND. (ErrVar%aviFAIL >= 0)) THEN ! Only compute control calculations if no error has occurred and we are not on the last time step IF ((LocalVar%iStatus == -8) .AND. (ErrVar%aviFAIL >= 0)) THEN ! Write restart files diff --git a/ROSCO/src/Filters.f90 b/ROSCO/src/Filters.f90 index 1b00968b..e2a99f94 100644 --- a/ROSCO/src/Filters.f90 +++ b/ROSCO/src/Filters.f90 @@ -219,13 +219,14 @@ REAL(DbKi) FUNCTION NotchFilter(InputSignal, DT, omega, betaNum, betaDen, FP, iS END FUNCTION NotchFilter !------------------------------------------------------------------------------------------------------------------------------- - SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst, ErrVar) + SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, DebugVar, objInst, ErrVar) ! Prefilter measured wind turbine signals to separate the filtering from the actual control actions - USE ROSCO_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances, ErrorVariables + USE ROSCO_Types, ONLY : ControlParameters, LocalVariables, DebugVariables, ObjectInstances, ErrorVariables TYPE(ControlParameters), INTENT(INOUT) :: CntrPar TYPE(LocalVariables), INTENT(INOUT) :: LocalVar + TYPE(DebugVariables), INTENT(INOUT) :: DebugVar TYPE(ObjectInstances), INTENT(INOUT) :: objInst TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar @@ -271,5 +272,10 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, objInst, ErrVar) LocalVar%VS_LastGenTrqF = SecLPFilter(LocalVar%VS_LastGenTrq, LocalVar%DT, CntrPar%F_LPFCornerFreq, 0.7_DbKi, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) LocalVar%PC_PitComTF = SecLPFilter(LocalVar%PC_PitComT, LocalVar%DT, CntrPar%F_LPFCornerFreq*0.25, 0.7_DbKi, LocalVar%FP, LocalVar%iStatus, LocalVar%restart, objInst%instSecLPF) + ! Debug Variables + DebugVar%GenSpeedF = LocalVar%GenSpeedF + DebugVar%RotSpeedF = LocalVar%RotSpeedF + DebugVar%NacIMU_FA_AccF = LocalVar%NacIMU_FA_AccF + DebugVar%FA_AccF = LocalVar%FA_AccF END SUBROUTINE PreFilterMeasuredSignals END MODULE Filters \ No newline at end of file diff --git a/ROSCO/src/ROSCO_IO.f90 b/ROSCO/src/ROSCO_IO.f90 index cbc9e91b..ce7ab4d5 100644 --- a/ROSCO/src/ROSCO_IO.f90 +++ b/ROSCO/src/ROSCO_IO.f90 @@ -379,7 +379,7 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME CHARACTER(15), ALLOCATABLE :: LocalVarOutStrings(:) REAL(DbKi), ALLOCATABLE :: LocalVarOutData(:) - nDebugOuts = 8 + nDebugOuts = 13 Allocate(DebugOutData(nDebugOuts)) Allocate(DebugOutStrings(nDebugOuts)) Allocate(DebugOutUnits(nDebugOuts)) @@ -389,12 +389,19 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME DebugOutData(4) = DebugVar%WE_t DebugOutData(5) = DebugVar%WE_Vm DebugOutData(6) = DebugVar%WE_Vt - DebugOutData(7) = DebugVar%WE_lambda - DebugOutData(8) = DebugVar%PC_PICommand + DebugOutData(7) = DebugVar%WE_Vw + DebugOutData(8) = DebugVar%WE_lambda + DebugOutData(9) = DebugVar%PC_PICommand + DebugOutData(10) = DebugVar%GenSpeedF + DebugOutData(11) = DebugVar%RotSpeedF + DebugOutData(12) = DebugVar%NacIMU_FA_AccF + DebugOutData(13) = DebugVar%FA_AccF DebugOutStrings = [CHARACTER(15) :: 'WE_Cp', 'WE_b', 'WE_w', 'WE_t', 'WE_Vm', & - 'WE_Vt', 'WE_lambda', 'PC_PICommand'] + 'WE_Vt', 'WE_Vw', 'WE_lambda', 'PC_PICommand', 'GenSpeedF', & + 'RotSpeedF', 'NacIMU_FA_AccF', 'FA_AccF'] DebugOutUnits = [CHARACTER(15) :: '[-]', '[-]', '[-]', '[-]', '[m/s]', & - '[m/s]', '[rad]', '[rad]'] + '[m/s]', '[m/s]', '[rad]', '[rad]', '[rad/s]', & + '[rad/s]', '[rad/s]', '[m/s]'] nLocalVars = 69 Allocate(LocalVarOutData(nLocalVars)) Allocate(LocalVarOutStrings(nLocalVars)) @@ -483,8 +490,6 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME 'FA_AccF', 'Flp_Angle', 'RootMyb_Last', 'ACC_INFILE_SIZE'] ! Initialize debug file IF ((LocalVar%iStatus == 0) .OR. (LocalVar%iStatus == -9)) THEN ! .TRUE. if we're on the first call to the DLL - ! If we're debugging, open the debug file and write the header: - ! Note that the headers will be Truncated to 10 characters!! IF (CntrPar%LoggingLevel > 0) THEN OPEN(unit=UnDb, FILE=RootName(1: size_avcOUTNAME-5)//'RO.dbg') WRITE(UnDb, *) 'Generated on '//CurDate()//' at '//CurTime()//' using ROSCO-'//TRIM(rosco_version) diff --git a/ROSCO/src/ROSCO_Types.f90 b/ROSCO/src/ROSCO_Types.f90 index 80b0c50f..2cb3c088 100644 --- a/ROSCO/src/ROSCO_Types.f90 +++ b/ROSCO/src/ROSCO_Types.f90 @@ -273,8 +273,13 @@ MODULE ROSCO_Types REAL(DbKi) :: WE_t ! Torque that WSE uses [-] REAL(DbKi) :: WE_Vm ! Mean wind speed component in WSE [m/s] REAL(DbKi) :: WE_Vt ! Turbulent wind speed component in WSE [m/s] + REAL(DbKi) :: WE_Vw ! Estimated wind speed in WSE [m/s] REAL(DbKi) :: WE_lambda ! TSR in WSE [rad] REAL(DbKi) :: PC_PICommand ! Commanded collective pitch from pitch PI controller [rad] + REAL(DbKi) :: GenSpeedF ! Filtered generator speed [rad/s] + REAL(DbKi) :: RotSpeedF ! Filtered rotor speed [rad/s] + REAL(DbKi) :: NacIMU_FA_AccF ! Filtered NacIMU_FA_Acc [rad/s] + REAL(DbKi) :: FA_AccF ! Filtered FA_Acc [m/s] END TYPE DebugVariables TYPE, PUBLIC :: ErrorVariables From 7cb1151cca30709fa5a7ea906f2e09d2570a4f3a Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Wed, 22 Dec 2021 08:21:43 -0700 Subject: [PATCH 38/40] cd for regtest --- .github/workflows/CI_rosco-pytools.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/CI_rosco-pytools.yml b/.github/workflows/CI_rosco-pytools.yml index ad8b8e12..0d399512 100644 --- a/.github/workflows/CI_rosco-pytools.yml +++ b/.github/workflows/CI_rosco-pytools.yml @@ -164,4 +164,5 @@ jobs: # Regression testing - name: Run regression testing run: | + cd ROSCO_testing python regtest.py From a2d6de45afe47a22512ba0287c63f6fe076b8c95 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Wed, 22 Dec 2021 10:08:52 -0700 Subject: [PATCH 39/40] Check logging level before calling debug --- ROSCO/src/DISCON.F90 | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/ROSCO/src/DISCON.F90 b/ROSCO/src/DISCON.F90 index 07c0ad38..ef8e46d6 100644 --- a/ROSCO/src/DISCON.F90 +++ b/ROSCO/src/DISCON.F90 @@ -67,7 +67,9 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME ! Check for restart IF ( (NINT(avrSWAP(1)) == -9) .AND. (aviFAIL >= 0)) THEN ! Read restart files CALL ReadRestartFile(avrSWAP, LocalVar, CntrPar, objInst, PerfData, RootName, SIZE(avcOUTNAME), ErrVar) - CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) + IF ( CntrPar%LoggingLevel > 0 ) THEN + CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) + END IF END IF ! Read avrSWAP array into derived types/variables @@ -98,9 +100,11 @@ SUBROUTINE DISCON(avrSWAP, aviFAIL, accINFILE, avcOUTNAME, avcMSG) BIND (C, NAME IF (CntrPar%Flp_Mode > 0) THEN CALL FlapControl(avrSWAP, CntrPar, LocalVar, objInst) END IF - - CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) - + + IF ( CntrPar%LoggingLevel > 0 ) THEN + CALL Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, SIZE(avcOUTNAME)) + END IF + END IF From 2ab7de67c7f6f869d5001a8dcee9bf40180371d3 Mon Sep 17 00:00:00 2001 From: Nikhar Abbas Date: Wed, 22 Dec 2021 12:48:06 -0700 Subject: [PATCH 40/40] add fl_pitcom and pc_minpit to debugvars --- ROSCO/rosco_registry/rosco_types.yaml | 6 ++++++ ROSCO/src/Controllers.f90 | 3 ++- ROSCO/src/ROSCO_IO.f90 | 10 +++++++--- ROSCO/src/ROSCO_Types.f90 | 2 ++ 4 files changed, 17 insertions(+), 4 deletions(-) diff --git a/ROSCO/rosco_registry/rosco_types.yaml b/ROSCO/rosco_registry/rosco_types.yaml index 400ea83c..2bd1e4f0 100644 --- a/ROSCO/rosco_registry/rosco_types.yaml +++ b/ROSCO/rosco_registry/rosco_types.yaml @@ -939,6 +939,12 @@ DebugVariables: FA_AccF: <<: *real description: Filtered FA_Acc [m/s] + Fl_PitCom: + <<: *real + description: Floating contribution to the pitch command [rad] + PC_MinPit: + <<: *real + description: Minimum blade pitch angle [rad] ErrorVariables: diff --git a/ROSCO/src/Controllers.f90 b/ROSCO/src/Controllers.f90 index 05b99692..21e5fce8 100644 --- a/ROSCO/src/Controllers.f90 +++ b/ROSCO/src/Controllers.f90 @@ -80,11 +80,12 @@ SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst, DebugVar, ErrVar) ELSE LocalVar%PC_MinPit = CntrPar%PC_FinePit ENDIF - + DebugVar%PC_MinPit = LocalVar%PC_MinPit ! FloatingFeedback IF (CntrPar%Fl_Mode > 0) THEN LocalVar%Fl_PitCom = FloatingFeedback(LocalVar, CntrPar, objInst) + DebugVar%FL_PitCom = LocalVar%Fl_PitCom LocalVar%PC_PitComT = LocalVar%PC_PitComT + LocalVar%Fl_PitCom ENDIF diff --git a/ROSCO/src/ROSCO_IO.f90 b/ROSCO/src/ROSCO_IO.f90 index ce7ab4d5..10dd30ab 100644 --- a/ROSCO/src/ROSCO_IO.f90 +++ b/ROSCO/src/ROSCO_IO.f90 @@ -379,7 +379,7 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME CHARACTER(15), ALLOCATABLE :: LocalVarOutStrings(:) REAL(DbKi), ALLOCATABLE :: LocalVarOutData(:) - nDebugOuts = 13 + nDebugOuts = 15 Allocate(DebugOutData(nDebugOuts)) Allocate(DebugOutStrings(nDebugOuts)) Allocate(DebugOutUnits(nDebugOuts)) @@ -396,12 +396,16 @@ SUBROUTINE Debug(LocalVar, CntrPar, DebugVar, avrSWAP, RootName, size_avcOUTNAME DebugOutData(11) = DebugVar%RotSpeedF DebugOutData(12) = DebugVar%NacIMU_FA_AccF DebugOutData(13) = DebugVar%FA_AccF + DebugOutData(14) = DebugVar%Fl_PitCom + DebugOutData(15) = DebugVar%PC_MinPit DebugOutStrings = [CHARACTER(15) :: 'WE_Cp', 'WE_b', 'WE_w', 'WE_t', 'WE_Vm', & 'WE_Vt', 'WE_Vw', 'WE_lambda', 'PC_PICommand', 'GenSpeedF', & - 'RotSpeedF', 'NacIMU_FA_AccF', 'FA_AccF'] + 'RotSpeedF', 'NacIMU_FA_AccF', 'FA_AccF', 'Fl_PitCom', 'PC_MinPit' & + ] DebugOutUnits = [CHARACTER(15) :: '[-]', '[-]', '[-]', '[-]', '[m/s]', & '[m/s]', '[m/s]', '[rad]', '[rad]', '[rad/s]', & - '[rad/s]', '[rad/s]', '[m/s]'] + '[rad/s]', '[rad/s]', '[m/s]', '[rad]', '[rad]' & + ] nLocalVars = 69 Allocate(LocalVarOutData(nLocalVars)) Allocate(LocalVarOutStrings(nLocalVars)) diff --git a/ROSCO/src/ROSCO_Types.f90 b/ROSCO/src/ROSCO_Types.f90 index 2cb3c088..857091ba 100644 --- a/ROSCO/src/ROSCO_Types.f90 +++ b/ROSCO/src/ROSCO_Types.f90 @@ -280,6 +280,8 @@ MODULE ROSCO_Types REAL(DbKi) :: RotSpeedF ! Filtered rotor speed [rad/s] REAL(DbKi) :: NacIMU_FA_AccF ! Filtered NacIMU_FA_Acc [rad/s] REAL(DbKi) :: FA_AccF ! Filtered FA_Acc [m/s] + REAL(DbKi) :: Fl_PitCom ! Floating contribution to the pitch command [rad] + REAL(DbKi) :: PC_MinPit ! Minimum blade pitch angle [rad] END TYPE DebugVariables TYPE, PUBLIC :: ErrorVariables