```fortran 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 ```