13 KiB
13 KiB
SUBROUTINE CalculatePositions( 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(R8Ki) :: rQ (3) ! Position vector from inertial frame origin to apex of rotation (point Q).
INTEGER(IntKi) :: J ! Counter for elements
INTEGER(IntKi) :: K ! Counter for blades
!-------------------------------------------------------------------------------------------------
! Positions
!-------------------------------------------------------------------------------------------------
! Define the position vectors between the various points on the wind turbine
! that are not dependent on the distributed tower or blade parameters:
RtHSdat%rZ = x%QT(DOF_Sg)* CoordSys%z1 + x%QT(DOF_Hv)* CoordSys%z2 - x%QT(DOF_Sw)* CoordSys%z3 ! Position vector from inertia frame origin to platform reference (point Z).
RtHSdat%rZY = p%rZYzt* CoordSys%a2 + p%PtfmCMxt*CoordSys%a1 - p%PtfmCMyt*CoordSys%a3 ! Position vector from platform reference (point Z) to platform mass center (point Y).
RtHSdat%rZT0 = p%rZT0zt* CoordSys%a2 ! Position vector from platform reference (point Z) to tower base (point T(0))
RtHSdat%rZO = ( x%QT(DOF_TFA1) + x%QT(DOF_TFA2) )*CoordSys%a1 & ! Position vector from platform reference (point Z) to tower-top / base plate (point O).
+ ( p%RefTwrHt - 0.5*( p%AxRedTFA(1,1,p%TTopNode)*x%QT(DOF_TFA1)*x%QT(DOF_TFA1) &
+ p%AxRedTFA(2,2,p%TTopNode)*x%QT(DOF_TFA2)*x%QT(DOF_TFA2) &
+ 2.0*p%AxRedTFA(1,2,p%TTopNode)*x%QT(DOF_TFA1)*x%QT(DOF_TFA2) &
+ p%AxRedTSS(1,1,p%TTopNode)*x%QT(DOF_TSS1)*x%QT(DOF_TSS1) &
+ p%AxRedTSS(2,2,p%TTopNode)*x%QT(DOF_TSS2)*x%QT(DOF_TSS2) &
+ 2.0*p%AxRedTSS(1,2,p%TTopNode)*x%QT(DOF_TSS1)*x%QT(DOF_TSS2) ) )*CoordSys%a2 &
+ ( x%QT(DOF_TSS1) + x%QT(DOF_TSS2) )*CoordSys%a3
RtHSdat%rOU = p%NacCMxn*CoordSys%d1 + p%NacCMzn *CoordSys%d2 - p%NacCMyn *CoordSys%d3 ! Position vector from tower-top / base plate (point O) to nacelle center of mass (point U).
RtHSdat%rOV = p%RFrlPnt_n(1)*CoordSys%d1 + p%RFrlPnt_n(3)*CoordSys%d2 - p%RFrlPnt_n(2)*CoordSys%d3 ! Position vector from tower-top / base plate (point O) to specified point on rotor-furl axis (point V).
RtHSdat%rVIMU = p%rVIMUxn*CoordSys%rf1 + p%rVIMUzn *CoordSys%rf2 - p%rVIMUyn *CoordSys%rf3 ! Position vector from specified point on rotor-furl axis (point V) to nacelle IMU (point IMU).
RtHSdat%rVD = p%rVDxn*CoordSys%rf1 + p%rVDzn *CoordSys%rf2 - p%rVDyn *CoordSys%rf3 ! Position vector from specified point on rotor-furl axis (point V) to center of mass of structure that furls with the rotor (not including rotor) (point D).
RtHSdat%rVP = p%rVPxn*CoordSys%rf1 + p%rVPzn *CoordSys%rf2 - p%rVPyn *CoordSys%rf3 + p%OverHang*CoordSys%c1 ! Position vector from specified point on rotor-furl axis (point V) to teeter pin (point P).
RtHSdat%rPQ = -p%UndSling*CoordSys%g1 ! Position vector from teeter pin (point P) to apex of rotation (point Q).
RtHSdat%rQC = p%HubCM*CoordSys%g1 ! Position vector from apex of rotation (point Q) to hub center of mass (point C).
RtHSdat%rOW = p%TFrlPnt_n(1)*CoordSys%d1 + p%TFrlPnt_n(3) *CoordSys%d2 - p%TFrlPnt_n(2)*CoordSys%d3 ! Position vector from tower-top / base plate (point O) to specified point on tail-furl axis (point W).
RtHSdat%rWI = p%rWIxn*CoordSys%tf1 + p%rWIzn*CoordSys%tf2 - p%rWIyn*CoordSys%tf3 ! Position vector from specified point on tail-furl axis (point W) to tail boom center of mass (point I).
RtHSdat%rWJ = p%rWJxn*CoordSys%tf1 + p%rWJzn*CoordSys%tf2 - p%rWJyn*CoordSys%tf3 ! Position vector from specified point on tail-furl axis (point W) to tail fin center of mass (point J).
RtHSdat%rPC = RtHSdat%rPQ + RtHSdat%rQC ! Position vector from teeter pin (point P) to hub center of mass (point C).
RtHSdat%rT0O = RtHSdat%rZO - RtHSdat%rZT0 ! Position vector from the tower base (point T(0)) to tower-top / base plate (point O).
RtHSdat%rO = RtHSdat%rZ + RtHSdat%rZO ! Position vector from inertial frame origin to tower-top / base plate (point O).
RtHSdat%rV = RtHSdat%rO + RtHSdat%rOV ! Position vector from inertial frame origin to specified point on rotor-furl axis (point V)
!RtHSdat%rP = RtHSdat%rO + RtHSdat%rOV + RtHSdat%rVP ! Position vector from inertial frame origin to teeter pin (point P).
RtHSdat%rP = RtHSdat%rV + RtHSdat%rVP ! Position vector from inertial frame origin to teeter pin (point P).
RtHSdat%rQ = RtHSdat%rP + RtHSdat%rPQ ! Position vector from inertial frame origin to apex of rotation (point Q).
RtHSdat%rJ = RtHSdat%rO + RtHSdat%rOW + RtHSdat%rWJ ! Position vector from inertial frame origin to tail fin center of mass (point J).
DO K = 1,p%NumBl ! Loop through all blades
! Calculate the position vector of the tip:
RtHSdat%rS0S(:,K,p%TipNode) = ( p%TwistedSF(K,1,1,p%TipNode,0)*x%QT( DOF_BF(K,1) ) & ! Position vector from the blade root (point S(0)) to the blade tip (point S(p%BldFlexL)).
+ p%TwistedSF(K,1,2,p%TipNode,0)*x%QT( DOF_BF(K,2) ) &
+ p%TwistedSF(K,1,3,p%TipNode,0)*x%QT( DOF_BE(K,1) ) )*CoordSys%j1(K,:) &
+ ( p%TwistedSF(K,2,1,p%TipNode,0)*x%QT( DOF_BF(K,1) ) &
+ p%TwistedSF(K,2,2,p%TipNode,0)*x%QT( DOF_BF(K,2) ) &
+ p%TwistedSF(K,2,3,p%TipNode,0)*x%QT( DOF_BE(K,1) ) )*CoordSys%j2(K,:) &
+ ( p%BldFlexL - 0.5* &
( p%AxRedBld(K,1,1,p%TipNode)*x%QT( DOF_BF(K,1) )*x%QT( DOF_BF(K,1) ) &
+ p%AxRedBld(K,2,2,p%TipNode)*x%QT( DOF_BF(K,2) )*x%QT( DOF_BF(K,2) ) &
+ p%AxRedBld(K,3,3,p%TipNode)*x%QT( DOF_BE(K,1) )*x%QT( DOF_BE(K,1) ) &
+ 2.*p%AxRedBld(K,1,2,p%TipNode)*x%QT( DOF_BF(K,1) )*x%QT( DOF_BF(K,2) ) &
+ 2.*p%AxRedBld(K,2,3,p%TipNode)*x%QT( DOF_BF(K,2) )*x%QT( DOF_BE(K,1) ) &
+ 2.*p%AxRedBld(K,1,3,p%TipNode)*x%QT( DOF_BF(K,1) )*x%QT( DOF_BE(K,1) ) ) )*CoordSys%j3(K,:)
RtHSdat%rQS (:,K,p%TipNode) = RtHSdat%rS0S(:,K,p%TipNode) + p%HubRad*CoordSys%j3(K,:) ! Position vector from apex of rotation (point Q) to the blade tip (point S(p%BldFlexL)).
RtHSdat%rS (:,K,p%TipNode) = RtHSdat%rQS (:,K,p%TipNode) + RtHSdat%rQ ! Position vector from inertial frame origin to the blade tip (point S(p%BldFlexL)).
! position vectors for blade root node:
RtHSdat%rQS (:,K,0) = p%HubRad*CoordSys%j3(K,:)
RtHSdat%rS (:,K,0) = p%HubRad*CoordSys%j3(K,:) + RtHSdat%rQ
! Calculate the position vector from the teeter pin to the blade root:
RtHSdat%rPS0(:,K) = RtHSdat%rPQ + p%HubRad*CoordSys%j3(K,:) ! Position vector from teeter pin (point P) to blade root (point S(0)).
DO J = 1,p%BldNodes ! Loop through the blade nodes / elements
! Calculate the position vector of the current node:
RtHSdat%rS0S(:,K,J) = ( p%TwistedSF(K,1,1,J,0)*x%QT( DOF_BF(K,1) ) & ! Position vector from the blade root (point S(0)) to the current node (point S(RNodes(J)).
+ p%TwistedSF(K,1,2,J,0)*x%QT( DOF_BF(K,2) ) &
+ p%TwistedSF(K,1,3,J,0)*x%QT( DOF_BE(K,1) ) )*CoordSys%j1(K,:) &
+ ( p%TwistedSF(K,2,1,J,0)*x%QT( DOF_BF(K,1) ) &
+ p%TwistedSF(K,2,2,J,0)*x%QT( DOF_BF(K,2) ) &
+ p%TwistedSF(K,2,3,J,0)*x%QT( DOF_BE(K,1) ) )*CoordSys%j2(K,:) &
+ ( p%RNodes(J) - 0.5* &
( p%AxRedBld(K,1,1,J)*x%QT( DOF_BF(K,1) )*x%QT( DOF_BF(K,1) ) &
+ p%AxRedBld(K,2,2,J)*x%QT( DOF_BF(K,2) )*x%QT( DOF_BF(K,2) ) &
+ p%AxRedBld(K,3,3,J)*x%QT( DOF_BE(K,1) )*x%QT( DOF_BE(K,1) ) &
+ 2.0*p%AxRedBld(K,1,2,J)*x%QT( DOF_BF(K,1) )*x%QT( DOF_BF(K,2) ) &
+ 2.0*p%AxRedBld(K,2,3,J)*x%QT( DOF_BF(K,2) )*x%QT( DOF_BE(K,1) ) &
+ 2.0*p%AxRedBld(K,1,3,J)*x%QT( DOF_BF(K,1) )*x%QT( DOF_BE(K,1) ) ) )*CoordSys%j3(K,:)
RtHSdat%rQS (:,K,J) = RtHSdat%rS0S(:,K,J) + p%HubRad*CoordSys%j3(K,:) ! Position vector from apex of rotation (point Q) to the current node (point S(RNodes(J)).
RtHSdat%rS (:,K,J) = RtHSdat%rQS (:,K,J) + RtHSdat%rQ ! Position vector from inertial frame origin to the current node (point S(RNodes(J)).
END DO !J = 1,p%BldNodes ! Loop through the blade nodes / elements
END DO !K = 1,p%NumBl
!----------------------------------------------------------------------------------------------------
! Get the tower element positions
!----------------------------------------------------------------------------------------------------
RtHSdat%rZT (:,0) = RtHSdat%rZT0
DO J = 1,p%TwrNodes ! Loop through the tower nodes / elements
! Calculate the position vector of the current node:
RtHSdat%rT0T(:,J) = ( p%TwrFASF(1,J,0)*x%QT(DOF_TFA1) + p%TwrFASF(2,J,0)*x%QT(DOF_TFA2) )*CoordSys%a1 & ! Position vector from base of flexible portion of tower (point T(0)) to current node (point T(J)).
+ ( p%HNodes(J) - 0.5*( p%AxRedTFA(1,1,J)*x%QT(DOF_TFA1)*x%QT(DOF_TFA1) &
+ p%AxRedTFA(2,2,J)*x%QT(DOF_TFA2)*x%QT(DOF_TFA2) &
+ 2.0*p%AxRedTFA(1,2,J)*x%QT(DOF_TFA1)*x%QT(DOF_TFA2) &
+ p%AxRedTSS(1,1,J)*x%QT(DOF_TSS1)*x%QT(DOF_TSS1) &
+ p%AxRedTSS(2,2,J)*x%QT(DOF_TSS2)*x%QT(DOF_TSS2) &
+ 2.0*p%AxRedTSS(1,2,J)*x%QT(DOF_TSS1)*x%QT(DOF_TSS2) ) )*CoordSys%a2 &
+ ( p%TwrSSSF(1,J,0)*x%QT(DOF_TSS1) + p%TwrSSSF(2,J,0)*x%QT(DOF_TSS2) )*CoordSys%a3
RtHSdat%rZT (:,J) = RtHSdat%rZT0 + RtHSdat%rT0T(:,J) ! Position vector from platform reference (point Z) to the current node (point T(HNodes(J)).
RtHSdat%rT(:,J) = RtHSdat%rZ + RtHSdat%rZT (:,J) ! Position vector from inertial frame origin to the current node (point T(HNodes(J)).
END DO
END SUBROUTINE CalculatePositions