15 KiB
15 KiB
SUBROUTINE CalculateAngularPosVelPAcc( p, x, CoordSys, RtHSdat )
!..................................................................................................................................
! 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_RtHndSide), INTENT(INOUT) :: RtHSdat !< data from the RtHndSid module (contains positions to be set)
!Local variables
REAL(ReKi) :: AngVelHM (3) ! Angular velocity of eleMent J of blade K (body M) in the hub (body H).
! REAL(ReKi) :: AngVelEN (3) ! Angular velocity of the nacelle (body N) in the inertia frame (body E for earth).
REAL(ReKi) :: AngAccELt (3) ! Portion of the angular acceleration of the low-speed shaft (body L) in the inertia frame (body E for earth) associated with everything but the QD2T()'s.
INTEGER(IntKi) :: J ! Counter for elements
INTEGER(IntKi) :: K ! Counter for blades
!-------------------------------------------------------------------------------------------------
! Angular and partial angular velocities
!-------------------------------------------------------------------------------------------------
! Define the angular and partial angular velocities of all of the rigid bodies in the inertia frame:
! NOTE: PAngVelEN(I,D,:) = the Dth-derivative of the partial angular velocity of DOF I for body N in body E.
RtHSdat%PAngVelEX( :,0,:) = 0.0
RtHSdat%PAngVelEX(DOF_R ,0,:) = CoordSys%z1
RtHSdat%PAngVelEX(DOF_P ,0,:) = -CoordSys%z3
RtHSdat%PAngVelEX(DOF_Y ,0,:) = CoordSys%z2
RtHSdat%AngVelEX = x%QDT(DOF_R )*RtHSdat%PAngVelEX(DOF_R ,0,:) &
+ x%QDT(DOF_P )*RtHSdat%PAngVelEX(DOF_P ,0,:) &
+ x%QDT(DOF_Y )*RtHSdat%PAngVelEX(DOF_Y ,0,:)
RtHSdat%AngPosEX = x%QT (DOF_R )*RtHSdat%PAngVelEX(DOF_R ,0,:) &
+ x%QT (DOF_P )*RtHSdat%PAngVelEX(DOF_P ,0,:) &
+ x%QT (DOF_Y )*RtHSdat%PAngVelEX(DOF_Y ,0,:)
RtHSdat%PAngVelEB( :,0,:) = RtHSdat%PAngVelEX(:,0,:)
RtHSdat%PAngVelEB(DOF_TFA1,0,:) = -p%TwrFASF(1,p%TTopNode,1)*CoordSys%a3
RtHSdat%PAngVelEB(DOF_TSS1,0,:) = p%TwrSSSF(1,p%TTopNode,1)*CoordSys%a1
RtHSdat%PAngVelEB(DOF_TFA2,0,:) = -p%TwrFASF(2,p%TTopNode,1)*CoordSys%a3
RtHSdat%PAngVelEB(DOF_TSS2,0,:) = p%TwrSSSF(2,p%TTopNode,1)*CoordSys%a1
RtHSdat%AngVelEB = RtHSdat%AngVelEX + x%QDT(DOF_TFA1)*RtHSdat%PAngVelEB(DOF_TFA1,0,:) &
+ x%QDT(DOF_TSS1)*RtHSdat%PAngVelEB(DOF_TSS1,0,:) &
+ x%QDT(DOF_TFA2)*RtHSdat%PAngVelEB(DOF_TFA2,0,:) &
+ x%QDT(DOF_TSS2)*RtHSdat%PAngVelEB(DOF_TSS2,0,:)
RtHSdat%AngPosXB = x%QT (DOF_TFA1)*RtHSdat%PAngVelEB(DOF_TFA1,0,:) &
+ x%QT (DOF_TSS1)*RtHSdat%PAngVelEB(DOF_TSS1,0,:) &
+ x%QT (DOF_TFA2)*RtHSdat%PAngVelEB(DOF_TFA2,0,:) &
+ x%QT (DOF_TSS2)*RtHSdat%PAngVelEB(DOF_TSS2,0,:)
RtHSdat%PAngVelEN( :,0,:)= RtHSdat%PAngVelEB(:,0,:)
RtHSdat%PAngVelEN(DOF_Yaw ,0,:)= CoordSys%d2
RtHSdat%AngVelEN = RtHSdat%AngVelEB + x%QDT(DOF_Yaw )*RtHSdat%PAngVelEN(DOF_Yaw ,0,:)
RtHSdat%PAngVelER( :,0,:)= RtHSdat%PAngVelEN(:,0,:)
RtHSdat%PAngVelER(DOF_RFrl,0,:)= CoordSys%rfa
RtHSdat%AngVelER = RtHSdat%AngVelEN + x%QDT(DOF_RFrl)*RtHSdat%PAngVelER(DOF_RFrl,0,:)
RtHSdat%PAngVelEL( :,0,:)= RtHSdat%PAngVelER(:,0,:)
RtHSdat%PAngVelEL(DOF_GeAz,0,:)= CoordSys%c1
RtHSdat%PAngVelEL(DOF_DrTr,0,:)= CoordSys%c1
RtHSdat%AngVelEL = RtHSdat%AngVelER + x%QDT(DOF_GeAz)*RtHSdat%PAngVelEL(DOF_GeAz,0,:) &
+ x%QDT(DOF_DrTr)*RtHSdat%PAngVelEL(DOF_DrTr,0,:)
RtHSdat%PAngVelEH( :,0,:)= RtHSdat%PAngVelEL(:,0,:)
RtHSdat%AngVelEH = RtHSdat%AngVelEL
IF ( p%NumBl == 2 ) THEN ! 2-blader
RtHSdat%PAngVelEH(DOF_Teet,0,:)= CoordSys%f2
RtHSdat%AngVelEH = RtHSdat%AngVelEH + x%QDT(DOF_Teet)*RtHSdat%PAngVelEH(DOF_Teet,0,:)
ENDIF
RtHSdat%PAngVelEG( :,0,:) = RtHSdat%PAngVelER(:,0,:)
RtHSdat%PAngVelEG(DOF_GeAz,0,:) = p%GBRatio*CoordSys%c1
RtHSdat%AngVelEG = RtHSdat%AngVelER + x%QDT(DOF_GeAz)*RtHSdat%PAngVelEG(DOF_GeAz,0,:)
RtHSdat%PAngVelEA( :,0,:) = RtHSdat%PAngVelEN(:,0,:)
RtHSdat%PAngVelEA(DOF_TFrl,0,:) = CoordSys%tfa
RtHSdat%AngVelEA = RtHSdat%AngVelEN + x%QDT(DOF_TFrl)*RtHSdat%PAngVelEA(DOF_TFrl,0,:)
! Define the 1st derivatives of the partial angular velocities of all
! of the rigid bodies in the inertia frame and the portion of the angular
! acceleration of the rigid bodies in the inertia frame associated with
! everything but the QD2T()'s:
RtHSdat%PAngVelEX( :,1,:) = 0.0
RtHSdat%AngAccEXt = 0.0
RtHSdat%PAngVelEB( :,1,:) = RtHSdat%PAngVelEX(:,1,:)
RtHSdat%PAngVelEB(DOF_TFA1,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEX, RtHSdat%PAngVelEB(DOF_TFA1,0,:) )
RtHSdat%PAngVelEB(DOF_TSS1,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEX, RtHSdat%PAngVelEB(DOF_TSS1,0,:) )
RtHSdat%PAngVelEB(DOF_TFA2,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEX, RtHSdat%PAngVelEB(DOF_TFA2,0,:) )
RtHSdat%PAngVelEB(DOF_TSS2,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEX, RtHSdat%PAngVelEB(DOF_TSS2,0,:) )
RtHSdat%AngAccEBt = RtHSdat%AngAccEXt + x%QDT(DOF_TFA1)*RtHSdat%PAngVelEB(DOF_TFA1,1,:) &
+ x%QDT(DOF_TSS1)*RtHSdat%PAngVelEB(DOF_TSS1,1,:) &
+ x%QDT(DOF_TFA2)*RtHSdat%PAngVelEB(DOF_TFA2,1,:) &
+ x%QDT(DOF_TSS2)*RtHSdat%PAngVelEB(DOF_TSS2,1,:)
RtHSdat%PAngVelEN( :,1,:) = RtHSdat%PAngVelEB(:,1,:)
RtHSdat%PAngVelEN(DOF_Yaw ,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEB, RtHSdat%PAngVelEN(DOF_Yaw ,0,:) )
RtHSdat%AngAccENt = RtHSdat%AngAccEBt + x%QDT(DOF_Yaw )*RtHSdat%PAngVelEN(DOF_Yaw ,1,:)
RtHSdat%PAngVelER( :,1,:) = RtHSdat%PAngVelEN(:,1,:)
RtHSdat%PAngVelER(DOF_RFrl,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEN, RtHSdat%PAngVelER(DOF_RFrl,0,:) )
RtHSdat%AngAccERt = RtHSdat%AngAccENt + x%QDT(DOF_RFrl)*RtHSdat%PAngVelER(DOF_RFrl,1,:)
RtHSdat%PAngVelEL( :,1,:) = RtHSdat%PAngVelER(:,1,:)
RtHSdat%PAngVelEL(DOF_GeAz,1,:) = CROSS_PRODUCT( RtHSdat%AngVelER, RtHSdat%PAngVelEL(DOF_GeAz,0,:) )
RtHSdat%PAngVelEL(DOF_DrTr,1,:) = CROSS_PRODUCT( RtHSdat%AngVelER, RtHSdat%PAngVelEL(DOF_DrTr,0,:) )
AngAccELt = RtHSdat%AngAccERt + x%QDT(DOF_GeAz)*RtHSdat%PAngVelEL(DOF_GeAz,1,:) &
+ x%QDT(DOF_DrTr)*RtHSdat%PAngVelEL(DOF_DrTr,1,:)
RtHSdat%PAngVelEH( :,1,:) = RtHSdat%PAngVelEL(:,1,:)
RtHSdat%AngAccEHt = AngAccELt
IF ( p%NumBl == 2 ) THEN ! 2-blader
RtHSdat%PAngVelEH(DOF_Teet,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEH, RtHSdat%PAngVelEH(DOF_Teet,0,:) )
RtHSdat%AngAccEHt = RtHSdat%AngAccEHt + x%QDT(DOF_Teet)*RtHSdat%PAngVelEH(DOF_Teet,1,:)
ENDIF
RtHSdat%PAngVelEG( :,1,:) = RtHSdat%PAngVelER(:,1,:)
RtHSdat%PAngVelEG(DOF_GeAz,1,:) = CROSS_PRODUCT( RtHSdat%AngVelER, RtHSdat%PAngVelEG(DOF_GeAz,0,:) )
RtHSdat%AngAccEGt = RtHSdat%AngAccERt + x%QDT(DOF_GeAz)*RtHSdat%PAngVelEG(DOF_GeAz,1,:)
RtHSdat%PAngVelEA( :,1,:) = RtHSdat%PAngVelEN(:,1,:)
RtHSdat%PAngVelEA(DOF_TFrl,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEN, RtHSdat%PAngVelEA(DOF_TFrl,0,:) )
RtHSdat%AngAccEAt = RtHSdat%AngAccENt + x%QDT(DOF_TFrl)*RtHSdat%PAngVelEA(DOF_TFrl,1,:)
DO K = 1,p%NumBl ! Loop through all blades
DO J = 0,p%TipNode ! Loop through the blade nodes / elements
! Define the partial angular velocities of the current node (body M(RNodes(J))) in the inertia frame:
! NOTE: PAngVelEM(K,J,I,D,:) = the Dth-derivative of the partial angular velocity
! of DOF I for body M of blade K, element J in body E.
RtHSdat%PAngVelEM(K,J, :,0,:) = RtHSdat%PAngVelEH(:,0,:)
RtHSdat%PAngVelEM(K,J,DOF_BF(K,1),0,:) = - p%TwistedSF(K,2,1,J,1)*CoordSys%j1(K,:) &
+ p%TwistedSF(K,1,1,J,1)*CoordSys%j2(K,:)
RtHSdat%PAngVelEM(K,J,DOF_BF(K,2),0,:) = - p%TwistedSF(K,2,2,J,1)*CoordSys%j1(K,:) &
+ p%TwistedSF(K,1,2,J,1)*CoordSys%j2(K,:)
RtHSdat%PAngVelEM(K,J,DOF_BE(K,1),0,:) = - p%TwistedSF(K,2,3,J,1)*CoordSys%j1(K,:) &
+ p%TwistedSF(K,1,3,J,1)*CoordSys%j2(K,:)
AngVelHM = x%QDT(DOF_BF(K,1))*RtHSdat%PAngVelEM(K,J,DOF_BF(K,1),0,:) &
+ x%QDT(DOF_BF(K,2))*RtHSdat%PAngVelEM(K,J,DOF_BF(K,2),0,:) &
+ x%QDT(DOF_BE(K,1))*RtHSdat%PAngVelEM(K,J,DOF_BE(K,1),0,:)
RtHSdat%AngVelEM(:,J,K ) = RtHSdat%AngVelEH + AngVelHM
RtHSdat%AngPosHM(:,K,J ) = x%QT (DOF_BF(K,1))*RtHSdat%PAngVelEM(K,J,DOF_BF(K,1),0,:) &
+ x%QT (DOF_BF(K,2))*RtHSdat%PAngVelEM(K,J,DOF_BF(K,2),0,:) &
+ x%QT (DOF_BE(K,1))*RtHSdat%PAngVelEM(K,J,DOF_BE(K,1),0,:)
RtHSdat%AngAccEKt(:,J ,K) = RtHSdat%AngAccEHt + x%QDT(DOF_BF(K,1))*RtHSdat%PAngVelEM(K,J,DOF_BF(K,1),1,:) &
+ x%QDT(DOF_BF(K,2))*RtHSdat%PAngVelEM(K,J,DOF_BF(K,2),1,:) &
+ x%QDT(DOF_BE(K,1))*RtHSdat%PAngVelEM(K,J,DOF_BE(K,1),1,:)
! Define the 1st derivatives of the partial angular velocities of the current node (body M(RNodes(J))) in the inertia frame:
! NOTE: These are currently unused by the code, therefore, they need not
! be calculated. Thus, they are currently commented out. If it
! turns out that they are ever needed (i.e., if inertias of the
! blade elements are ever added, etc...) simply uncomment out these computations:
! RtHSdat%PAngVelEM(K,J, :,1,:) = RtHSdat%PAngVelEH(:,1,:)
! RtHSdat%PAngVelEM(K,J,DOF_BF(K,1),1,:) = CROSS_PRODUCT( RtHSdat%AngVelEH, PAngVelEM(K,J,DOF_BF(K,1),0,:) )
! RtHSdat%PAngVelEM(K,J,DOF_BF(K,2),1,:) = CROSS_PRODUCT( RtHSdat%AngVelEH, PAngVelEM(K,J,DOF_BF(K,2),0,:) )
! RtHSdat%PAngVelEM(K,J,DOF_BE(K,1),1,:) = CROSS_PRODUCT( RtHSdat%AngVelEH, PAngVelEM(K,J,DOF_BE(K,1),0,:) )
END DO !J = 1,p%BldNodes ! Loop through the blade nodes / elements
END DO !K = 1,p%NumBl
!...............
! tower values:
!...............
DO J = 0,p%TwrNodes ! Loop through the tower nodes / elements
! Define the partial angular velocities (and their 1st derivatives) of the
! current node (body F(HNodes(J)) in the inertia frame.
! Also define the overall angular velocity of the current node in the inertia frame.
! Also, define the portion of the angular acceleration of the current node
! in the inertia frame associated with everything but the QD2T()'s:
! NOTE: PAngVelEF(J,I,D,:) = the Dth-derivative of the partial angular velocity
! of DOF I for body F of element J in body E.
RtHSdat%PAngVelEF (J, :,0,:) = RtHSdat%PAngVelEX(:,0,:)
RtHSdat%PAngVelEF (J,DOF_TFA1,0,:) = -p%TwrFASF(1,J,1)*CoordSys%a3
RtHSdat%PAngVelEF (J,DOF_TSS1,0,:) = p%TwrSSSF(1,J,1)*CoordSys%a1
RtHSdat%PAngVelEF (J,DOF_TFA2,0,:) = -p%TwrFASF(2,J,1)*CoordSys%a3
RtHSdat%PAngVelEF (J,DOF_TSS2,0,:) = p%TwrSSSF(2,J,1)*CoordSys%a1
RtHSdat%PAngVelEF (J, :,1,:) = RtHSdat%PAngVelEX(:,1,:)
RtHSdat%PAngVelEF (J,DOF_TFA1,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEX , RtHSdat%PAngVelEF(J,DOF_TFA1,0,:) )
RtHSdat%PAngVelEF (J,DOF_TSS1,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEX , RtHSdat%PAngVelEF(J,DOF_TSS1,0,:) )
RtHSdat%PAngVelEF (J,DOF_TFA2,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEX , RtHSdat%PAngVelEF(J,DOF_TFA2,0,:) )
RtHSdat%PAngVelEF (J,DOF_TSS2,1,:) = CROSS_PRODUCT( RtHSdat%AngVelEX , RtHSdat%PAngVelEF(J,DOF_TSS2,0,:) )
RtHSdat%AngVelEF (:,J) = RtHSdat%AngVelEX + x%QDT(DOF_TFA1)*RtHSdat%PAngVelEF(J,DOF_TFA1,0,:) &
+ x%QDT(DOF_TSS1)*RtHSdat%PAngVelEF(J,DOF_TSS1,0,:) &
+ x%QDT(DOF_TFA2)*RtHSdat%PAngVelEF(J,DOF_TFA2,0,:) &
+ x%QDT(DOF_TSS2)*RtHSdat%PAngVelEF(J,DOF_TSS2,0,:)
RtHSdat%AngPosXF (:,J) = x%QT (DOF_TFA1)*RtHSdat%PAngVelEF(J,DOF_TFA1,0,:) &
+ x%QT (DOF_TSS1)*RtHSdat%PAngVelEF(J,DOF_TSS1,0,:) &
+ x%QT (DOF_TFA2)*RtHSdat%PAngVelEF(J,DOF_TFA2,0,:) &
+ x%QT (DOF_TSS2)*RtHSdat%PAngVelEF(J,DOF_TSS2,0,:)
RtHSdat%AngPosEF (:,J) = RtHSdat%AngPosEX + RtHSdat%AngPosXF(:,J)
RtHSdat%AngAccEFt(:,J) = RtHSdat%AngAccEXt + x%QDT(DOF_TFA1)*RtHSdat%PAngVelEF(J,DOF_TFA1,1,:) &
+ x%QDT(DOF_TSS1)*RtHSdat%PAngVelEF(J,DOF_TSS1,1,:) &
+ x%QDT(DOF_TFA2)*RtHSdat%PAngVelEF(J,DOF_TFA2,1,:) &
+ x%QDT(DOF_TSS2)*RtHSdat%PAngVelEF(J,DOF_TSS2,1,:)
END DO ! J
END SUBROUTINE CalculateAngularPosVelPAcc