Skip to content

Commit

Permalink
Filter Nacelle vane signal and use to calculate aligned wind speed
Browse files Browse the repository at this point in the history
  • Loading branch information
@dzalkind
dzalkind committed Oct 18, 2024
1 parent ed3c6a4 commit 04de072
Show file tree
Hide file tree
Showing 8 changed files with 325 additions and 310 deletions.
3 changes: 3 additions & 0 deletions rosco/controller/rosco_registry/rosco_types.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1012,6 +1012,9 @@ LocalVariables:
NacVane:
<<: *real
description: Nacelle vane angle [deg]
NacVaneF:
<<: *real
description: Filtered nacelle vane angle [deg]
HorWindV:
<<: *real
description: Hub height wind speed m/s
Expand Down
2 changes: 1 addition & 1 deletion rosco/controller/src/ControllerBlocks.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -342,7 +342,7 @@ SUBROUTINE WindSpeedEstimator(LocalVar, CntrPar, objInst, PerfData, DebugVar, Er

! Filter the wind speed at hub height regardless, only use if WE_Mode = 0 or WE_Op = 0
! Re-initialize at WE_Vw if leaving operational wind, WE_Vw is initialized at HorWindV
LocalVar%HorWindV_F = LPFilter(LocalVar%HorWindV, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%FP, LocalVar%RestartWSE, LocalVar%restart, objInst%instLPF, LocalVar%WE_Vw)
LocalVar%HorWindV_F = cos(LocalVar%NacVaneF*D2R) * LPFilter(LocalVar%HorWindV, LocalVar%DT, CntrPar%F_WECornerFreq, LocalVar%FP, LocalVar%RestartWSE, LocalVar%restart, objInst%instLPF, LocalVar%WE_Vw)

! ---- Debug Inputs ------
DebugVar%WE_b = WE_Inp_Pitch
Expand Down
147 changes: 147 additions & 0 deletions rosco/controller/src/Controllers.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -870,5 +870,152 @@ SUBROUTINE StructuralControl(avrSWAP, CntrPar, LocalVar, objInst, ErrVar)
ENDIF

END SUBROUTINE StructuralControl
!-------------------------------------------------------------------------------------------------------------------------------
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(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(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
piP%ITerm(inst) = I0
piP%ITermLast(inst) = I0

PIController = I0
ELSE
PTerm = kp*error
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)

piP%ITermLast(inst) = piP%ITerm(inst)
END IF
inst = inst + 1

END FUNCTION PIController


!-------------------------------------------------------------------------------------------------------------------------------
REAL(DbKi) FUNCTION PIDController(error, kp, ki, kd, tf, minValue, maxValue, DT, I0, piP, reset, objInst, LocalVar)
USE ROSCO_Types, ONLY : piParams, LocalVariables, ObjectInstances

! 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) :: kd
REAL(DbKi), INTENT(IN) :: tf
REAL(DbKi), INTENT(IN) :: minValue
REAL(DbKi), INTENT(IN) :: maxValue
REAL(DbKi), INTENT(IN) :: DT
TYPE(ObjectInstances), INTENT(INOUT) :: objInst ! all object instances (PI, filters used here)
TYPE(LocalVariables), INTENT(INOUT) :: LocalVar

REAL(DbKi), 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, DTerm ! Proportional, deriv. terms
REAL(DbKi) :: EFilt ! Filtered error for derivative

! Always filter error
EFilt = LPFilter(error, DT, tf, LocalVar%FP, LocalVar%iStatus, reset, objInst%instLPF)

! Initialize persistent variables/arrays, and set inital condition for integrator term
IF (reset) THEN
piP%ITerm(objInst%instPI) = I0
piP%ITermLast(objInst%instPI) = I0
piP%ELast(objInst%instPI) = 0.0_DbKi
PIDController = I0
ELSE
! Proportional
PTerm = kp*error

! Integrate and saturate
piP%ITerm(objInst%instPI) = piP%ITerm(objInst%instPI) + DT*ki*error
piP%ITerm(objInst%instPI) = saturate(piP%ITerm(objInst%instPI), minValue, maxValue)

! Derivative (filtered)
DTerm = kd * (EFilt - piP%ELast(objInst%instPI)) / DT

! Saturate all
PIDController = saturate(PTerm + piP%ITerm(objInst%instPI) + DTerm, minValue, maxValue)

! Save lasts
piP%ITermLast(objInst%instPI) = piP%ITerm(objInst%instPI)
piP%ELast(objInst%instPI) = EFilt
END IF
objInst%instPI = objInst%instPI + 1

END FUNCTION PIDController

!-------------------------------------------------------------------------------------------------------------------------------
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(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(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
piP%ITerm(inst) = I0
piP%ITermLast(inst) = I0
piP%ITerm2(inst) = I0
piP%ITermLast2(inst) = I0

PIIController = I0
ELSE
PTerm = kp*error
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)

piP%ITermLast(inst) = piP%ITerm(inst)
END IF
inst = inst + 1

END FUNCTION PIIController

!-------------------------------------------------------------------------------------------------------------------------------
END MODULE Controllers
8 changes: 8 additions & 0 deletions rosco/controller/src/Filters.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
MODULE Filters
!...............................................................................................................................
USE Constants
USE Functions
IMPLICIT NONE

CONTAINS
Expand Down Expand Up @@ -333,6 +334,8 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, DebugVar, objInst, ErrVar
TYPE(ErrorVariables), INTENT(INOUT) :: ErrVar
INTEGER(IntKi) :: K ! Integer used to loop through turbine blades
INTEGER(IntKi) :: n ! Integer used to loop through notch filters
REAL(DbKi) :: NacVaneCosF ! Time-filtered x-component of NacVane (deg)
REAL(DbKi) :: NacVaneSinF ! Time-filtered y-component of NacVane (deg)

! If there's an error, don't even try to run
IF (ErrVar%aviFAIL < 0) THEN
Expand Down Expand Up @@ -421,6 +424,11 @@ SUBROUTINE PreFilterMeasuredSignals(CntrPar, LocalVar, DebugVar, 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)

! Wind vane signal
NacVaneCosF = LPFilter(cos(LocalVar%NacVane*D2R), LocalVar%DT, CntrPar%F_YawErr, LocalVar%FP, LocalVar%iStatus, .FALSE., objInst%instLPF) ! (-)
NacVaneSinF = LPFilter(sin(LocalVar%NacVane*D2R), LocalVar%DT, CntrPar%F_YawErr, LocalVar%FP, LocalVar%iStatus, .FALSE., objInst%instLPF) ! (-)
LocalVar%NacVaneF = wrap_180(atan2(NacVaneSinF, NacVaneCosF) * R2D) ! (deg)

! Debug Variables
DebugVar%GenSpeedF = LocalVar%GenSpeedF
DebugVar%RotSpeedF = LocalVar%RotSpeedF
Expand Down
148 changes: 0 additions & 148 deletions rosco/controller/src/Functions.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,6 @@
MODULE Functions

USE Constants
USE Filters

IMPLICIT NONE

Expand Down Expand Up @@ -91,154 +90,7 @@ REAL(DbKi) FUNCTION ratelimit(inputSignal, minRate, maxRate, DT, reset, rlP, ins

END FUNCTION ratelimit

!-------------------------------------------------------------------------------------------------------------------------------
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(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(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
piP%ITerm(inst) = I0
piP%ITermLast(inst) = I0

PIController = I0
ELSE
PTerm = kp*error
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)

piP%ITermLast(inst) = piP%ITerm(inst)
END IF
inst = inst + 1

END FUNCTION PIController


!-------------------------------------------------------------------------------------------------------------------------------
REAL(DbKi) FUNCTION PIDController(error, kp, ki, kd, tf, minValue, maxValue, DT, I0, piP, reset, objInst, LocalVar)
USE ROSCO_Types, ONLY : piParams, LocalVariables, ObjectInstances

! 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) :: kd
REAL(DbKi), INTENT(IN) :: tf
REAL(DbKi), INTENT(IN) :: minValue
REAL(DbKi), INTENT(IN) :: maxValue
REAL(DbKi), INTENT(IN) :: DT
TYPE(ObjectInstances), INTENT(INOUT) :: objInst ! all object instances (PI, filters used here)
TYPE(LocalVariables), INTENT(INOUT) :: LocalVar

REAL(DbKi), 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, DTerm ! Proportional, deriv. terms
REAL(DbKi) :: EFilt ! Filtered error for derivative

! Always filter error
EFilt = LPFilter(error, DT, tf, LocalVar%FP, LocalVar%iStatus, reset, objInst%instLPF)

! Initialize persistent variables/arrays, and set inital condition for integrator term
IF (reset) THEN
piP%ITerm(objInst%instPI) = I0
piP%ITermLast(objInst%instPI) = I0
piP%ELast(objInst%instPI) = 0.0_DbKi
PIDController = I0
ELSE
! Proportional
PTerm = kp*error

! Integrate and saturate
piP%ITerm(objInst%instPI) = piP%ITerm(objInst%instPI) + DT*ki*error
piP%ITerm(objInst%instPI) = saturate(piP%ITerm(objInst%instPI), minValue, maxValue)

! Derivative (filtered)
DTerm = kd * (EFilt - piP%ELast(objInst%instPI)) / DT

! Saturate all
PIDController = saturate(PTerm + piP%ITerm(objInst%instPI) + DTerm, minValue, maxValue)

! Save lasts
piP%ITermLast(objInst%instPI) = piP%ITerm(objInst%instPI)
piP%ELast(objInst%instPI) = EFilt
END IF
objInst%instPI = objInst%instPI + 1

END FUNCTION PIDController

!-------------------------------------------------------------------------------------------------------------------------------
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(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(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
piP%ITerm(inst) = I0
piP%ITermLast(inst) = I0
piP%ITerm2(inst) = I0
piP%ITermLast2(inst) = I0

PIIController = I0
ELSE
PTerm = kp*error
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)

piP%ITermLast(inst) = piP%ITerm(inst)
END IF
inst = inst + 1

END FUNCTION PIIController

!-------------------------------------------------------------------------------------------------------------------------------
REAL(DbKi) FUNCTION interp1d(xData, yData, xq, ErrVar)
! interp1d 1-D interpolation (table lookup), xData should be strictly increasing

Expand Down
Loading

0 comments on commit 04de072

Please sign in to comment.