38 KiB
38 KiB
SUBROUTINE FillAugMat( p, x, CoordSys, u, HSSBrTrq, RtHSdat, AugMat )
!..................................................................................................................................
! Passed variables
TYPE(ED_ParameterType), INTENT(IN ) :: p !< Parameters
TYPE(ED_ContinuousStateType), INTENT(IN ) :: x !< Continuous states at Time
TYPE(ED_CoordSys), INTENT(IN ) :: CoordSys !< The coordinate systems that have been set for these states/time
TYPE(ED_InputType), INTENT(IN ) :: u !< The aero blade forces/moments
TYPE(ED_RtHndSide), INTENT(IN ) :: RtHSdat !< data from the RtHndSid module (contains positions to be set)
REAL(ReKi), INTENT(IN ) :: HSSBrTrq !< SIGN( u%HSSBrTrqC, x%QDT(DOF_GeAz) ) or corrected value from FixHSS
REAL(R8Ki), INTENT(OUT) :: AugMat(:,:) !< the return matrix
! Local variables
REAL(ReKi) :: TmpVec (3) ! A temporary vector used in various computations.
REAL(ReKi) :: TmpVec1 (3) ! A temporary vector used in various computations.
REAL(ReKi) :: TmpVec3 (3) ! A temporary vector used in various computations.
REAL(ReKi) :: GBoxTrq ! Gearbox torque on the LSS side in N-m (calculated from inputs and parameters).
REAL(ReKi) :: GBoxEffFac2 ! A second gearbox efficiency factor = ( 1 / GBoxEff^SgnPrvLSTQ - 1 )
INTEGER(IntKi) :: I ! Loops through some or all of the DOFs
INTEGER(IntKi) :: J ! Counter for elements
INTEGER(IntKi) :: K ! Counter for blades
INTEGER(IntKi) :: L ! Generic index
! Initialize the matrix:
AugMat = 0.0
GBoxTrq = ( u%GenTrq + HSSBrTrq )*ABS(p%GBRatio) ! bjj: do we use HSSBrTrqC or HSSBrTrq?
DO K = 1,p%NumBl ! Loop through all blades
! Initialize the portions of the mass matrix on and below the diagonal associated with purely blade DOFs (these portions can't
! be calculated using partial loads) using the tip mass effects.
! Also, initialize the portions of the forcing vector associated with purely blade DOFs (these portions can't be calculated
! using partial loads) using the tip mass effects:
! NOTE: The vector subscript array, PSBE(), used in the following loops must be sorted from smallest to largest DOF index in
! order for the loops to work to enter values only on and below the diagonal of AugMat():
DO L = 1,p%DOFs%NPSBE(K) ! Loop through all active (enabled) blade DOFs that contribute to the QD2T-related linear accelerations of the tip of blade K (point S(p%BldFlexL))
DO I = L,p%DOFs%NPSBE(K) ! Loop through all active (enabled) blade DOFs greater than or equal to L
AugMat(p%DOFs%PSBE(K,I),p%DOFs%PSBE(K,L)) = p%TipMass(K)*&
DOT_PRODUCT( RtHSdat%PLinVelES(K, p%TipNode, p%DOFs%PSBE(K,I),0,:), & ! [C(q,t)]B
RtHSdat%PLinVelES(K, p%TipNode, p%DOFs%PSBE(K,L),0,:) )
ENDDO ! I - All active (enabled) blade DOFs greater than or equal to L
ENDDO ! L - All active (enabled) blade DOFs that contribute to the QD2T-related linear accelerations of the tip of blade K (point S(p%BldFlexL))
TmpVec1 = RtHSdat%FSTipDrag(:,K) - p%TipMass(K)*( p%Gravity*CoordSys%z2 + RtHSdat%LinAccESt(:,K,p%TipNode) ) ! The portion of FrcS0Bt associated with the tip brake
DO I = 1,p%DOFs%NPSBE(K) ! Loop through all active (enabled) blade DOFs that contribute to the QD2T-related linear accelerations of the tip of blade K (point S(p%BldFlexL))
AugMat(p%DOFs%PSBE(K,I), p%NAug) = DOT_PRODUCT( RtHSdat%PLinVelES(K,p%TipNode,p%DOFs%PSBE(K,I),0,:), & ! {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB
TmpVec1 ) ! NOTE: TmpVec1 is still the portion of FrcS0Bt associated with the tip brake
ENDDO ! I - All active (enabled) blade DOFs that contribute to the QD2T-related linear accelerations of the tip of blade K (point S(p%BldFlexL))
DO J = 1,p%BldNodes ! Loop through the blade nodes / elements
! Integrate to find the portions of the mass matrix on and below the diagonal associated with purely blade DOFs (these portions
! can't be calculated using partial loads). Also, integrate to find the portions of the forcing vector associated with
! purely blade DOFs (these portions can't be calculated using partial loads):
! NOTE: The vector subscript array, PSBE(), used in the following loops must
! be sorted from smallest to largest DOF index in order for the loops
! to work to enter values only on and below the diagonal of AugMat():
DO L = 1,p%DOFs%NPSBE(K) ! Loop through all active (enabled) blade DOFs that contribute to the QD2T-related linear accelerations of the blade
DO I = L,p%DOFs%NPSBE(K) ! Loop through all active (enabled) blade DOFs greater than or equal to L
AugMat(p%DOFs%PSBE(K,I),p%DOFs%PSBE(K,L)) = AugMat(p%DOFs%PSBE(K,I),p%DOFs%PSBE(K,L)) + p%BElmntMass(J,K)*&
DOT_PRODUCT( RtHSdat%PLinVelES(K,J,p%DOFs%PSBE(K,I),0,:), & ! [C(q,t)]B
RtHSdat%PLinVelES(K,J,p%DOFs%PSBE(K,L),0,:) )
ENDDO ! I - All active (enabled) blade DOFs greater than or equal to L
ENDDO ! L - All active (enabled) blade DOFs that contribute to the QD2T-related linear accelerations of the blade
TmpVec1 = RtHSdat%FSAero(:,K,J)*p%DRNodes(J) - p%BElmntMass(J,K)*( p%Gravity*CoordSys%z2 + RtHSdat%LinAccESt(:,K,J) ) ! The portion of FrcS0Bt associated with blade element J
TmpVec3 = RtHSdat%MMAero(:,K,J)*p%DRNodes(J) ! The total external moment applied to blade element J
DO I = 1,p%DOFs%NPSBE(K) ! Loop through all active (enabled) blade DOFs that contribute to the QD2T-related linear accelerations of the blade
AugMat(p%DOFs%PSBE(K,I), p%NAug) = AugMat(p%DOFs%PSBE(K,I), p%NAug) & ! {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB
+ DOT_PRODUCT( RtHSdat%PLinVelES(K,J,p%DOFs%PSBE(K,I),0,:), TmpVec1 ) & ! NOTE: TmpVec1 is still the portion of FrcS0Bt associated with blade element J
+ DOT_PRODUCT( RtHSdat%PAngVelEM(K,J,p%DOFs%PSBE(K,I),0,:), TmpVec3 ) ! and TmpVec3 is still the total external moment applied to blade element J
ENDDO ! I - All active (enabled) blade DOFs that contribute to the QD2T-related linear accelerations of the blade
ENDDO ! J - Blade nodes / elements
! Initialize the portions of the mass matrix below the diagonal associated
! with the teeter and pure blade DOFs using the partial loads at the teeter pin; only do this if necessary:
IF ( ( p%NumBl == 2 ) ) THEN
IF ( p%DOF_Flag(DOF_Teet) ) THEN ! NOTE: two "ifs" since DOF_Teet might be out of bound
DO L = 1,p%DOFs%NPSBE(K) ! Loop through all active (enabled) blade DOFs that contribute to the QD2T-related linear accelerations of the blade
AugMat(DOF_Teet,p%DOFs%PSBE(K,L)) = -DOT_PRODUCT( RtHSdat%PAngVelEH(DOF_Teet,0,:), &
RtHSdat%PMomLPRot(:,p%DOFs%PSBE(K,L)) ) ! [C(q,t)]B
ENDDO ! L - All active (enabled) blade DOFs that contribute to the QD2T-related linear accelerations of the blade
ENDIF
ENDIF
! If the associated DOFs are enabled, add the blade elasticity and damping
! forces to the forcing vector (these portions can't be calculated using
! partial loads):
IF ( p%DOF_Flag(DOF_BF(K,1)) ) THEN
AugMat( DOF_BF(K,1),p%NAug) = AugMat(DOF_BF(K,1),p%NAug) & !
- p%KBF(K,1,1)*x%QT( DOF_BF(K,1)) &
- p%KBF(K,1,2)*x%QT( DOF_BF(K,2)) &
- p%CBF(K,1,1)*x%QDT(DOF_BF(K,1)) &
- p%CBF(K,1,2)*x%QDT(DOF_BF(K,2))
ENDIF
IF ( p%DOF_Flag(DOF_BF(K,2)) ) THEN
AugMat( DOF_BF(K,2),p%NAug) = AugMat(DOF_BF(K,2),p%NAug) & ! {-f(qd,q,t)}ElasticB + {-f(qd,q,t)}DampB
- p%KBF(K,2,1)*x%QT( DOF_BF(K,1)) &
- p%KBF(K,2,2)*x%QT( DOF_BF(K,2)) &
- p%CBF(K,2,1)*x%QDT(DOF_BF(K,1)) &
- p%CBF(K,2,2)*x%QDT(DOF_BF(K,2))
ENDIF
IF ( p%DOF_Flag(DOF_BE(K,1)) ) THEN
AugMat( DOF_BE(K,1),p%NAug) = AugMat(DOF_BE(K,1),p%NAug) & !
- p%KBE(K,1,1)*x%QT( DOF_BE(K,1)) &
- p%CBE(K,1,1)*x%QDT(DOF_BE(K,1))
ENDIF
END DO !k
! Initialize the portions of the mass matrix on and below the diagonal
! associated with purely tower DOFs (these portions can't be calculated
! using partial loads) using the yaw bearing mass effects.
! Also, initialize the portions of the forcing vector associated with
! purely blade DOFs (these portions can't be calculated using partial
! loads) using the yaw bearing mass effects:
! NOTE: The vector subscript array, PTTE(), used in the following loops must
! be sorted from smallest to largest DOF index in order for the loops
! to work to enter values only on and below the diagonal of AugMat():
DO L = 1,p%DOFs%NPTTE ! Loop through all active (enabled) tower DOFs that contribute to the QD2T-related linear accelerations of the yaw bearing (point O)
DO I = L,p%DOFs%NPTTE ! Loop through all active (enabled) tower DOFs greater than or equal to L
AugMat(p%DOFs%PTTE(I),p%DOFs%PTTE(L)) = p%YawBrMass*DOT_PRODUCT( RtHSdat%PLinVelEO(p%DOFs%PTTE(I),0,:), & ! [C(q,t)]T of YawBrMass
RtHSdat%PLinVelEO(p%DOFs%PTTE(L),0,:) )
ENDDO ! I - All active (enabled) tower DOFs greater than or equal to L
ENDDO ! L - All active (enabled) tower DOFs that contribute to the QD2T-related linear accelerations of the yaw bearing (point O)
TmpVec1 = -p%YawBrMass*( p%Gravity*CoordSys%z2 + RtHSdat%LinAccEOt ) ! The portion of FrcT0Trbt associated with the YawBrMass
DO I = 1,p%DOFs%NPTTE ! Loop through all active (enabled) tower DOFs that contribute to the QD2T-related linear accelerations of the yaw bearing (point O)
AugMat(p%DOFs%PTTE(I), p%NAug) = DOT_PRODUCT( RtHSdat%PLinVelEO(p%DOFs%PTTE(I),0,:), & ! {-f(qd,q,t)}T + {-f(qd,q,t)}GravT of YawBrMass
TmpVec1 ) ! NOTE: TmpVec1 is still the portion of FrcT0Trbt associated with YawBrMass
ENDDO ! I - All active (enabled) tower DOFs that contribute to the QD2T-related linear accelerations of the yaw bearing (point O)
DO J = 1,p%TwrNodes
!..................................................................................................................................
! Integrate to find the portions of the mass matrix on and below the diagonal associated with purely tower DOFs (these portions
! can't be calculated using partial loads). Also, integrate to find the portions of the forcing vector associated with purely
! tower DOFs (these portions can't be calculated using partial loads).
! NOTE: The vector subscript array, PTTE(), used in the following loops must be sorted from smallest to largest DOF index in order
! for the loops to work to enter values only on and below the diagonal of AugMat():
!..................................................................................................................................
DO L = 1,p%DOFs%NPTTE ! Loop through all active (enabled) tower DOFs that contribute to the QD2T-related linear accelerations of the tower
DO I = L,p%DOFs%NPTTE ! Loop through all active (enabled) tower DOFs greater than or equal to L
AugMat(p%DOFs%PTTE(I),p%DOFs%PTTE(L)) = AugMat(p%DOFs%PTTE(I),p%DOFs%PTTE(L)) &
+ p%TElmntMass(J) *DOT_PRODUCT( RtHSdat%PLinVelET(J,p%DOFs%PTTE(I),0,:), &
RtHSdat%PLinVelET(J,p%DOFs%PTTE(L),0,:) ) & ! [C(q,t)]T + [C(q,t)]HydroT
- abs(p%DHNodes(J))*DOT_PRODUCT( RtHSdat%PLinVelET(J,p%DOFs%PTTE(I),0,:), &
RtHSdat%PFTHydro (:,J,p%DOFs%PTTE(L) ) ) &
- abs(p%DHNodes(J))*DOT_PRODUCT( RtHSdat%PAngVelEF(J,p%DOFs%PTTE(I),0,:), &
RtHSdat%PMFHydro (:,J,p%DOFs%PTTE(L) ) )
ENDDO ! I - All active (enabled) tower DOFs greater than or equal to L
ENDDO ! L - All active (enabled) tower DOFs that contribute to the QD2T-related linear accelerations of the tower
TmpVec1 = ( RtHSdat%FTHydrot(:,J) )*abs(p%DHNodes(J)) &
- p%TElmntMass(J)*( p%Gravity*CoordSys%z2 + RtHSdat%LinAccETt(:,J) ) ! The portion of FrcT0Trbt associated with tower element J
TmpVec3 = ( RtHSdat%MFHydrot(:,J) )*abs(p%DHNodes(J)) ! The external moment applied to tower element J
DO I = 1,p%DOFs%NPTTE ! Loop through all active (enabled) tower DOFs that contribute to the QD2T-related linear accelerations of the tower
AugMat(p%DOFs%PTTE(I), p%NAug) = AugMat(p%DOFs%PTTE(I), p%NAug) & ! {-f(qd,q,t)}T + {-f(qd,q,t)}GravT + {-f(qd,q,t)}AeroT + {-f(qd,q,t)}HydroT
+ DOT_PRODUCT( RtHSdat%PLinVelET(J,p%DOFs%PTTE(I),0,:), TmpVec1 ) & ! NOTE: TmpVec1 is still the portion of FrcT0Trbt associated with tower element J
+ DOT_PRODUCT( RtHSdat%PAngVelEF(J,p%DOFs%PTTE(I),0,:), TmpVec3 ) ! and TmpVec3 is still the total external moment to tower element J
ENDDO ! I - All active (enabled) tower DOFs that contribute to the QD2T-related linear accelerations of the tower
ENDDO ! J - Tower nodes / elements
!..................................................................................................................................
! If the associated DOFs are enabled, add the tower elasticity and damping forces to the forcing vector (these portions can't be
! calculated using partial loads):
!..................................................................................................................................
IF ( p%DOF_Flag(DOF_TFA1) ) THEN
AugMat( DOF_TFA1,p%NAug) = AugMat(DOF_TFA1,p%NAug) &
- p%KTFA(1,1)*x%QT( DOF_TFA1) - p%KTFA(1,2)*x%QT( DOF_TFA2) & !
- p%CTFA(1,1)*x%QDT(DOF_TFA1) - p%CTFA(1,2)*x%QDT(DOF_TFA2)
ENDIF
IF ( p%DOF_Flag(DOF_TSS1) ) THEN
AugMat( DOF_TSS1,p%NAug) = AugMat(DOF_TSS1,p%NAug) &
- p%KTSS(1,1)*x%QT( DOF_TSS1) - p%KTSS(1,2)*x%QT( DOF_TSS2) & ! {-f(qd,q,t)}ElasticT + {-f(qd,q,t)}DampT
- p%CTSS(1,1)*x%QDT(DOF_TSS1) - p%CTSS(1,2)*x%QDT(DOF_TSS2)
ENDIF
IF ( p%DOF_Flag(DOF_TFA2) ) THEN
AugMat( DOF_TFA2,p%NAug) = AugMat(DOF_TFA2,p%NAug) &
- p%KTFA(2,1)*x%QT( DOF_TFA1) - p%KTFA(2,2)*x%QT( DOF_TFA2) & !
- p%CTFA(2,1)*x%QDT(DOF_TFA1) - p%CTFA(2,2)*x%QDT(DOF_TFA2)
ENDIF
IF ( p%DOF_Flag(DOF_TSS2) ) THEN
AugMat( DOF_TSS2,p%NAug) = AugMat(DOF_TSS2,p%NAug) &
- p%KTSS(2,1)*x%QT( DOF_TSS1) - p%KTSS(2,2)*x%QT( DOF_TSS2) & !
- p%CTSS(2,1)*x%QDT(DOF_TSS1) - p%CTSS(2,2)*x%QDT(DOF_TSS2)
ENDIF
!..................................................................................................................................
! Now that all of the partial loads have been found, let's fill in the portions of the mass matrix on and below the diagonal that
! may be calculated with the help of the partial loads.
! Also, let's fill in the portions of the forcing vector that may be calculated with the help of the partial loads.
! Also let's add in additional terms to the forcing function that can't be added with the help of the partial loads.
!
! NOTE: The vector subscript array, SrtPS(), used in the following loops must be sorted from smallest to largest DOF index in order
! for the loops to work to enter values only on and below the diagonal of AugMat():
!..................................................................................................................................
IF ( p%DOF_Flag (DOF_Sg ) ) THEN
DO I = p%DOFs%Diag(DOF_Sg ),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_Sg ) = -1.*DOT_PRODUCT( RtHSdat%PLinVelEZ(DOF_Sg ,0,:), RtHSdat%PFrcZAll (:,p%DOFs%SrtPS(I)) ) ! [C(q,t)]X + [C(q,t)]HydroX + [C(q,t)]T + [C(q,t)]HydroT + [C(q,t)]N + [C(q,t)]R + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_Sg , p%NAug) = DOT_PRODUCT( RtHSdat%PLinVelEZ(DOF_Sg ,0,:), RtHSdat%FrcZAllt ) ! {-f(qd,q,t)}X + {-f(qd,q,t)}HydroX + {-f(qd,q,t)}T + {-f(qd,q,t)}AeroT + {-f(qd,q,t)}HydroT + {-f(qd,q,t)}N + {-f(qd,q,t)}R + {-f(qd,q,t)}H + {-f(qd,q,t)}B + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}AeroA
ENDIF
IF ( p%DOF_Flag (DOF_Sw ) ) THEN
DO I = p%DOFs%Diag(DOF_Sw ),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_Sw ) = -1.*DOT_PRODUCT( RtHSdat%PLinVelEZ(DOF_Sw ,0,:), RtHSdat%PFrcZAll (:,p%DOFs%SrtPS(I)) ) ! [C(q,t)]X + [C(q,t)]HydroX + [C(q,t)]T + [C(q,t)]HydroT + [C(q,t)]N + [C(q,t)]R + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_Sw , p%NAug) = DOT_PRODUCT( RtHSdat%PLinVelEZ(DOF_Sw ,0,:), RtHSdat%FrcZAllt ) ! {-f(qd,q,t)}X + {-f(qd,q,t)}HydroX + {-f(qd,q,t)}T + {-f(qd,q,t)}AeroT + {-f(qd,q,t)}HydroT + {-f(qd,q,t)}N + {-f(qd,q,t)}R + {-f(qd,q,t)}H + {-f(qd,q,t)}B + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}AeroA
ENDIF
IF ( p%DOF_Flag (DOF_Hv ) ) THEN
DO I = p%DOFs%Diag(DOF_Hv ),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_Hv ) = -1.*DOT_PRODUCT( RtHSdat%PLinVelEZ(DOF_Hv ,0,:), RtHSdat%PFrcZAll (:,p%DOFs%SrtPS(I)) ) ! [C(q,t)]X + [C(q,t)]HydroX + [C(q,t)]T + [C(q,t)]HydroT + [C(q,t)]N + [C(q,t)]R + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_Hv , p%NAug) = DOT_PRODUCT( RtHSdat%PLinVelEZ(DOF_Hv ,0,:), RtHSdat%FrcZAllt ) ! {-f(qd,q,t)}X + {-f(qd,q,t)}GravX + {-f(qd,q,t)}HydroX + {-f(qd,q,t)}T + {-f(qd,q,t)}GravT + {-f(qd,q,t)}AeroT + {-f(qd,q,t)}HydroT + {-f(qd,q,t)}N + {-f(qd,q,t)}GravN + {-f(qd,q,t)}R + {-f(qd,q,t)}GravR + {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}GravA + {-f(qd,q,t)}AeroA
ENDIF
IF ( p%DOF_Flag (DOF_R ) ) THEN
DO I = p%DOFs%Diag(DOF_R ),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_R ) = -1.*DOT_PRODUCT( RtHSdat%PAngVelEX(DOF_R ,0,:), RtHSdat%PMomXAll (:,p%DOFs%SrtPS(I)) ) ! [C(q,t)]X + [C(q,t)]HydroX + [C(q,t)]T + [C(q,t)]HydroT + [C(q,t)]N + [C(q,t)]R + [C(q,t)]G + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_R , p%NAug) = DOT_PRODUCT( RtHSdat%PAngVelEX(DOF_R ,0,:), RtHSdat%MomXAllt ) ! {-f(qd,q,t)}X + {-f(qd,q,t)}GravX + {-f(qd,q,t)}HydroX + {-f(qd,q,t)}T + {-f(qd,q,t)}GravT + {-f(qd,q,t)}AeroT + {-f(qd,q,t)}HydroT + {-f(qd,q,t)}N + {-f(qd,q,t)}GravN + {-f(qd,q,t)}R + {-f(qd,q,t)}GravR + {-f(qd,q,t)}G + {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}GravA + {-f(qd,q,t)}AeroA
ENDIF
IF ( p%DOF_Flag (DOF_P ) ) THEN
DO I = p%DOFs%Diag(DOF_P ),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_P ) = -1.*DOT_PRODUCT( RtHSdat%PAngVelEX(DOF_P ,0,:), RtHSdat%PMomXAll (:,p%DOFs%SrtPS(I)) ) ! [C(q,t)]X + [C(q,t)]HydroX + [C(q,t)]T + [C(q,t)]HydroT + [C(q,t)]N + [C(q,t)]R + [C(q,t)]G + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_P ,p%NAug) = DOT_PRODUCT( RtHSdat%PAngVelEX(DOF_P ,0,:), RtHSdat%MomXAllt ) ! {-f(qd,q,t)}X + {-f(qd,q,t)}GravX + {-f(qd,q,t)}HydroX + {-f(qd,q,t)}T + {-f(qd,q,t)}GravT + {-f(qd,q,t)}AeroT + {-f(qd,q,t)}HydroT + {-f(qd,q,t)}N + {-f(qd,q,t)}GravN + {-f(qd,q,t)}R + {-f(qd,q,t)}GravR + {-f(qd,q,t)}G + {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}GravA + {-f(qd,q,t)}AeroA
END IF
IF ( p%DOF_Flag (DOF_Y ) ) THEN
DO I = p%DOFs%Diag(DOF_Y ),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_Y ) = -1.*DOT_PRODUCT( RtHSdat%PAngVelEX(DOF_Y ,0,:), RtHSdat%PMomXAll (:,p%DOFs%SrtPS(I)) ) ! [C(q,t)]X + [C(q,t)]HydroX + [C(q,t)]T + [C(q,t)]HydroT + [C(q,t)]N + [C(q,t)]R + [C(q,t)]G + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_Y , p%NAug) = DOT_PRODUCT( RtHSdat%PAngVelEX(DOF_Y ,0,:), RtHSdat%MomXAllt ) ! {-f(qd,q,t)}X + {-f(qd,q,t)}GravX + {-f(qd,q,t)}HydroX + {-f(qd,q,t)}T + {-f(qd,q,t)}GravT + {-f(qd,q,t)}AeroT + {-f(qd,q,t)}HydroT + {-f(qd,q,t)}N + {-f(qd,q,t)}GravN + {-f(qd,q,t)}R + {-f(qd,q,t)}GravR + {-f(qd,q,t)}G + {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}GravA + {-f(qd,q,t)}AeroA
ENDIF
IF ( p%DOF_Flag (DOF_TFA1) ) THEN
DO I = p%DOFs%Diag(DOF_TFA1),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_TFA1) = AugMat(p%DOFs%SrtPS(I),DOF_TFA1) &
- DOT_PRODUCT( RtHSdat%PLinVelEO(DOF_TFA1,0,:), &
RtHSdat%PFrcONcRt(:,p%DOFs%SrtPS(I)) ) & ! [C(q,t)]N + [C(q,t)]R + [C(q,t)]G + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
- DOT_PRODUCT( RtHSdat%PAngVelEB(DOF_TFA1,0,:), &
RtHSdat%PMomBNcRt(:,p%DOFs%SrtPS(I)) )
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_TFA1, p%NAug) = AugMat(DOF_TFA1, p%NAug) &
+ DOT_PRODUCT( RtHSdat%PLinVelEO(DOF_TFA1,0,:), RtHSdat%FrcONcRtt ) & ! {-f(qd,q,t)}N + {-f(qd,q,t)}GravN + {-f(qd,q,t)}R + {-f(qd,q,t)}GravR + {-f(qd,q,t)}G + {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}GravA + {-f(qd,q,t)}AeroA
+ DOT_PRODUCT( RtHSdat%PAngVelEB(DOF_TFA1,0,:), RtHSdat%MomBNcRtt )
ENDIF
IF ( p%DOF_Flag (DOF_TSS1) ) THEN
DO I = p%DOFs%Diag(DOF_TSS1),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_TSS1) = AugMat(p%DOFs%SrtPS(I),DOF_TSS1) &
- DOT_PRODUCT( RtHSdat%PLinVelEO(DOF_TSS1,0,:), &
RtHSdat%PFrcONcRt(:,p%DOFs%SrtPS(I)) ) & ! [C(q,t)]N + [C(q,t)]R + [C(q,t)]G + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
- DOT_PRODUCT( RtHSdat%PAngVelEB(DOF_TSS1,0,:), &
RtHSdat%PMomBNcRt(:,p%DOFs%SrtPS(I)) )
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_TSS1, p%NAug) = AugMat(DOF_TSS1, p%NAug) &
+ DOT_PRODUCT( RtHSdat%PLinVelEO(DOF_TSS1,0,:), RtHSdat%FrcONcRtt ) & ! {-f(qd,q,t)}N + {-f(qd,q,t)}GravN + {-f(qd,q,t)}R + {-f(qd,q,t)}GravR + {-f(qd,q,t)}G + {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}GravA + {-f(qd,q,t)}AeroA
+ DOT_PRODUCT( RtHSdat%PAngVelEB(DOF_TSS1,0,:), RtHSdat%MomBNcRtt )
ENDIF
IF ( p%DOF_Flag (DOF_TFA2) ) THEN
DO I = p%DOFs%Diag(DOF_TFA2),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_TFA2) = AugMat(p%DOFs%SrtPS(I),DOF_TFA2) &
- DOT_PRODUCT( RtHSdat%PLinVelEO(DOF_TFA2,0,:), &
RtHSdat%PFrcONcRt(:,p%DOFs%SrtPS(I)) ) & ! [C(q,t)]N + [C(q,t)]R + [C(q,t)]G + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
- DOT_PRODUCT( RtHSdat%PAngVelEB(DOF_TFA2,0,:), &
RtHSdat%PMomBNcRt(:,p%DOFs%SrtPS(I)) )
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_TFA2, p%NAug) = AugMat(DOF_TFA2, p%NAug) &
+ DOT_PRODUCT( RtHSdat%PLinVelEO(DOF_TFA2,0,:), RtHSdat%FrcONcRtt ) & ! {-f(qd,q,t)}N + {-f(qd,q,t)}GravN + {-f(qd,q,t)}R + {-f(qd,q,t)}GravR + {-f(qd,q,t)}G + {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}GravA + {-f(qd,q,t)}AeroA
+ DOT_PRODUCT( RtHSdat%PAngVelEB(DOF_TFA2,0,:), RtHSdat%MomBNcRtt )
ENDIF
IF ( p%DOF_Flag (DOF_TSS2) ) THEN
DO I = p%DOFs%Diag(DOF_TSS2),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_TSS2) = AugMat(p%DOFs%SrtPS(I),DOF_TSS2) &
- DOT_PRODUCT( RtHSdat%PLinVelEO(DOF_TSS2,0,:), &
RtHSdat%PFrcONcRt(:,p%DOFs%SrtPS(I)) ) & ! [C(q,t)]N + [C(q,t)]R + [C(q,t)]G + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
- DOT_PRODUCT( RtHSdat%PAngVelEB(DOF_TSS2,0,:), &
RtHSdat%PMomBNcRt(:,p%DOFs%SrtPS(I)) )
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_TSS2, p%NAug) = AugMat(DOF_TSS2, p%NAug) &
+ DOT_PRODUCT( RtHSdat%PLinVelEO(DOF_TSS2,0,:), RtHSdat%FrcONcRtt ) & ! {-f(qd,q,t)}N + {-f(qd,q,t)}GravN + {-f(qd,q,t)}R + {-f(qd,q,t)}GravR + {-f(qd,q,t)}G + {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}GravA + {-f(qd,q,t)}AeroA
+ DOT_PRODUCT( RtHSdat%PAngVelEB(DOF_TSS2,0,:), RtHSdat%MomBNcRtt )
ENDIF
IF ( p%DOF_Flag (DOF_Yaw ) ) THEN
DO I = p%DOFs%Diag(DOF_Yaw ),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_Yaw ) = -DOT_PRODUCT( RtHSdat%PAngVelEN(DOF_Yaw ,0,:), RtHSdat%PMomBNcRt(:,p%DOFs%SrtPS(I)) ) ! [C(q,t)]N + [C(q,t)]R + [C(q,t)]G + [C(q,t)]H + [C(q,t)]B + [C(q,t)]A
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_Yaw , p%NAug) = DOT_PRODUCT( RtHSdat%PAngVelEN(DOF_Yaw ,0,:), RtHSdat%MomBNcRtt ) & ! {-f(qd,q,t)}N + {-f(qd,q,t)}GravN + {-f(qd,q,t)}R + {-f(qd,q,t)}GravR + {-f(qd,q,t)}G + {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB + {-f(qd,q,t)}A + {-f(qd,q,t)}GravA + {-f(qd,q,t)}AeroA
+ u%YawMom ! + {-f(qd,q,t)}SpringYaw + {-f(qd,q,t)}DampYaw; NOTE: The neutral yaw rate, YawRateNeut, defaults to zero. It is only used for yaw control.
!PRINT '(A, F30.20)', 'AugMat[11, 25] = ', AugMat(DOF_Yaw ,p%NAug)
ENDIF
IF ( p%DOF_Flag (DOF_RFrl) ) THEN
DO I = p%DOFs%Diag(DOF_RFrl),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_RFrl) = -DOT_PRODUCT( RtHSdat%PAngVelER(DOF_RFrl,0,:), &
RtHSdat%PMomNGnRt(:,p%DOFs%SrtPS(I)) ) ! [C(q,t)]R + [C(q,t)]G + [C(q,t)]H + [C(q,t)]B
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_RFrl, p%NAug) = DOT_PRODUCT( RtHSdat%PAngVelER(DOF_RFrl,0,:), RtHSdat%MomNGnRtt ) & ! {-f(qd,q,t)}R + {-f(qd,q,t)}GravR + {-f(qd,q,t)}G + {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB
+ RtHSdat%RFrlMom ! + {-f(qd,q,t)}SpringRF + {-f(qd,q,t)}DampRF
ENDIF
TmpVec = p%GenIner*CoordSys%c1*DOT_PRODUCT( CoordSys%c1, RtHSdat%PAngVelEG(DOF_GeAz,0,:) ) ! = ( generator inertia dyadic ) Dot ( partial angular velocity of G in E for DOF_GeAz )
IF ( p%DOF_Flag (DOF_GeAz) ) THEN
DO I = p%DOFs%Diag(DOF_GeAz),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_GeAz) = -DOT_PRODUCT( RtHSdat%PAngVelEL(DOF_GeAz,0,:), RtHSdat%PMomLPRot(:,p%DOFs%SrtPS(I)) )! [C(q,t)]H + [C(q,t)]B
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_GeAz, p%NAug) = DOT_PRODUCT( RtHSdat%PAngVelEL(DOF_GeAz,0,:), RtHSdat%MomLPRott ) & ! {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB
- GBoxTrq ! + {-f(qd,q,t)}Gen + {-f(qd,q,t)}Brake
! The previous loop (DO I = p%DOFs%Diag(DOF_GeAz),p%DOFs%NActvDOF) misses the
! generator inertia-contribution to the mass matrix and forcing function.
! Thus, add these in as well:
AugMat(DOF_GeAz, DOF_GeAz) = AugMat(DOF_GeAz,DOF_GeAz) &
+ DOT_PRODUCT( RtHSdat%PAngVelEG(DOF_GeAz,0,:), TmpVec ) ! [C(q,t)]G
AugMat(DOF_GeAz, p%NAug) = AugMat(DOF_GeAz, p%NAug) &
- DOT_PRODUCT( RtHSdat%AngAccEGt , TmpVec ) ! {-f(qd,q,t)}G
ENDIF
IF ( p%DOF_Flag (DOF_DrTr) ) THEN
DO I = p%DOFs%Diag(DOF_DrTr),p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs on or below the diagonal
AugMat(p%DOFs%SrtPS(I),DOF_DrTr) = -DOT_PRODUCT( RtHSdat%PAngVelEL(DOF_DrTr,0,:), RtHSdat%PMomLPRot(:,p%DOFs%SrtPS(I)) ) ! [C(q,t)]H + [C(q,t)]B
ENDDO ! I - All active (enabled) DOFs on or below the diagonal
AugMat(DOF_DrTr, p%NAug) = DOT_PRODUCT( RtHSdat%PAngVelEL(DOF_DrTr,0,:), RtHSdat%MomLPRott ) & ! {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB
- p%DTTorSpr*x%QT (DOF_DrTr) & ! + {-f(qd,q,t)}ElasticDrive
- p%DTTorDmp*x%QDT(DOF_DrTr) ! + {-f(qd,q,t)}DampDrive
ENDIF
IF ( p%DOF_Flag (DOF_TFrl) ) THEN
! The tail-furl DOF does not affect any DOF index larger than DOF_TFrl. Therefore, there is no need to perform the loop: DO I = Diag(DOF_TFrl),NActvDOF
AugMat(DOF_TFrl, DOF_TFrl) = -DOT_PRODUCT( RtHSdat%PAngVelEA(DOF_TFrl,0,:), RtHSdat%PMomNTail(:,DOF_TFrl) ) ! [C(q,t)]A
AugMat(DOF_TFrl, p%NAug) = DOT_PRODUCT( RtHSdat%PAngVelEA(DOF_TFrl,0,:), RtHSdat%MomNTailt ) & ! {-f(qd,q,t)}A + {-f(qd,q,t)}GravA + {-f(qd,q,t)}AeroA
+ RtHSdat%TFrlMom ! + {-f(qd,q,t)}SpringTF + {-f(qd,q,t)}DampTF
ENDIF
IF ( ( p%NumBl == 2 ) ) THEN
IF ( p%DOF_Flag(DOF_Teet) ) THEN ! NOTE: two "ifs" since DOF_Teet may be out of bound
! The teeter DOF does not affect any DOF index larger than DOF_Teet. Therefore, there is no need to perform the loop: DO I = Diag(DOF_Teet),NActvDOF
AugMat(DOF_Teet, DOF_Teet) = -DOT_PRODUCT( RtHSdat%PAngVelEH(DOF_Teet,0,:), RtHSdat%PMomLPRot(:,DOF_Teet) ) ! [C(q,t)]H + [C(q,t)]B
AugMat(DOF_Teet, p%NAug) = DOT_PRODUCT( RtHSdat%PAngVelEH(DOF_Teet,0,:), RtHSdat%MomLPRott ) & ! {-f(qd,q,t)}H + {-f(qd,q,t)}GravH + {-f(qd,q,t)}B + {-f(qd,q,t)}GravB + {-f(qd,q,t)}AeroB
+ RtHSdat%TeetMom ! + {-f(qd,q,t)}SpringTeet + {-f(qd,q,t)}DampTeet
ENDIF
ENDIF
!..................................................................................................................................
! So far, we have only filled in the portions of the mass matrix on and below the diagonal. Because the mass matrix is symmetric
! up to this point, let's fill in the portion above the diagonal by mirroring the values from below:
! NOTE: The vector subscript array, SrtPS(), used in the following loops must be sorted from smallest to largest DOF index in order
! for the loops to work to enter values only on and below the diagonal of AugMat():
!..................................................................................................................................
DO L = 2,p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs above the diagonal (columns)
DO I = 1,L-1 ! Loop through all active (enabled) DOFs above the diagonal (rows)
AugMat(p%DOFs%SrtPS(I),p%DOFs%SrtPS(L)) = AugMat(p%DOFs%SrtPS(L),p%DOFs%SrtPS(I))
ENDDO ! I - All active (enabled) DOFs above the diagonal (rows)
ENDDO ! L - All active (enabled) DOFs above the diagonal (columns)
! Let's add the gearbox friction terms to the mass matrix and forcing
! function. These only effect the equation for the generator azimuth DOF.
! NOTE: the MASS MATRIX WILL NO LONGER BE SYMMETRIC after adding these
! terms, unless the gearbox efficiency, GBoxEff, was set to 100%:
GBoxEffFac2 = ( 1.0/RtHSdat%GBoxEffFac - 1.0 ) ! = ( 1 / GBoxEff^SgnPrvLSTQ - 1 )
!TmpVec = p%GenIner*CoordSys%c1*DOT_PRODUCT( CoordSys%c1, RtHSdat%PAngVelEG(DOF_GeAz,0,:) ) ! = ( generator inertia dyadic ) Dot ( partial angular velocity of G in E for DOF_GeAz )
DO I = 1,p%DOFs%NActvDOF ! Loop through all active (enabled) DOFs
AugMat(DOF_GeAz,p%DOFs%SrtPS(I)) = AugMat(DOF_GeAz,p%DOFs%SrtPS(I)) & ! NOTE: TmpVec is still = ( generator inertia dyadic ) Dot ( partial angular velocity of G in E for DOF_GeAz ) in the following equation
+ GBoxEffFac2* DOT_PRODUCT( RtHSdat%PAngVelEG(p%DOFs%SrtPS(I),0,:), TmpVec ) ! [C(q,t)]GBFric
ENDDO ! I - All active (enabled) DOFs
AugMat( DOF_GeAz, p%NAug) = AugMat(DOF_GeAz, p%NAug) & ! NOTE: TmpVec is still = ( generator inertia dyadic ) Dot ( partial angular velocity of G in E for DOF_GeAz ) in the following equation
- GBoxEffFac2*( DOT_PRODUCT( RtHSdat%AngAccEGt , TmpVec ) + GBoxTrq ) ! {-f(qd,q,t)}GBFric
END SUBROUTINE FillAugMat