222 lines
15 KiB
Markdown
222 lines
15 KiB
Markdown
|
|
|
|
```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
|
|
``` |