From 5a0f935e14ba7ebd1e1ec808fa86e75e53a9c71f Mon Sep 17 00:00:00 2001 From: Emmanuel Branlard Date: Fri, 25 Aug 2023 22:22:10 -0600 Subject: [PATCH] LD: implemented state integration --- modules/aerodyn/src/UA_Dvr_Subs.f90 | 16 +- modules/aerodyn/src/UnsteadyAero_Driver.f90 | 70 +- modules/lindyn/src/LinDyn.f90 | 1096 ++++++++----------- modules/lindyn/src/LinDyn_Registry.txt | 19 +- modules/lindyn/src/LinDyn_Types.f90 | 598 ++++++++-- 5 files changed, 1079 insertions(+), 720 deletions(-) diff --git a/modules/aerodyn/src/UA_Dvr_Subs.f90 b/modules/aerodyn/src/UA_Dvr_Subs.f90 index 51d9d3bd8b..be9f61eabe 100644 --- a/modules/aerodyn/src/UA_Dvr_Subs.f90 +++ b/modules/aerodyn/src/UA_Dvr_Subs.f90 @@ -115,18 +115,20 @@ subroutine ReadDriverInputFile( FileName, InitInp, ErrStat, ErrMsg ) ! --- ELASTIC SECTION section if (InitInp%SimMod==3) then ! Temporary to avoid changing r-test for now call ParseCom(FI, iLine, Line , errStat2, errMsg2, UnEcho); if(Failed()) return - call ParseAry(FI, iLine, 'ActiveDOFs' , InitInp%activeDOFs, 3, errStat2, errMsg2, UnEcho); if(Failed()) return + call ParseVar(FI, iLine, 'TMax' , InitInp%Tmax , errStat2, errMsg2, UnEcho); if(Failed()) return + call ParseVar(FI, iLine, 'DT' , InitInp%dt , errStat2, errMsg2, UnEcho); if(Failed()) return + call ParseAry(FI, iLine, 'activeDOFs' , InitInp%activeDOFs, 3, errStat2, errMsg2, UnEcho); if(Failed()) return call ParseAry(FI, iLine, 'initPos' , InitInp%initPos , 3, errStat2, errMsg2, UnEcho); if(Failed()) return call ParseAry(FI, iLine, 'initVel' , InitInp%initVel , 3, errStat2, errMsg2, UnEcho); if(Failed()) return call ParseAry(FI, iLine, 'MassMatrix1' , InitInp%MM(1,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return - call ParseAry(FI, iLine, 'MassMatrix2' , InitInp%MM(1,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return - call ParseAry(FI, iLine, 'MassMatrix3' , InitInp%MM(1,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return + call ParseAry(FI, iLine, 'MassMatrix2' , InitInp%MM(2,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return + call ParseAry(FI, iLine, 'MassMatrix3' , InitInp%MM(3,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return call ParseAry(FI, iLine, 'DampMatrix1' , InitInp%CC(1,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return - call ParseAry(FI, iLine, 'DampMatrix2' , InitInp%CC(1,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return - call ParseAry(FI, iLine, 'DampMatrix3' , InitInp%CC(1,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return + call ParseAry(FI, iLine, 'DampMatrix2' , InitInp%CC(2,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return + call ParseAry(FI, iLine, 'DampMatrix3' , InitInp%CC(3,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return call ParseAry(FI, iLine, 'StifMatrix1' , InitInp%KK(1,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return - call ParseAry(FI, iLine, 'StifMatrix2' , InitInp%KK(1,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return - call ParseAry(FI, iLine, 'StifMatrix3' , InitInp%KK(1,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return + call ParseAry(FI, iLine, 'StifMatrix2' , InitInp%KK(2,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return + call ParseAry(FI, iLine, 'StifMatrix3' , InitInp%KK(3,:) , 3, errStat2, errMsg2, UnEcho); if(Failed()) return endif ! --- OUTPUT section diff --git a/modules/aerodyn/src/UnsteadyAero_Driver.f90 b/modules/aerodyn/src/UnsteadyAero_Driver.f90 index 30bcb9e416..36312bc4b3 100644 --- a/modules/aerodyn/src/UnsteadyAero_Driver.f90 +++ b/modules/aerodyn/src/UnsteadyAero_Driver.f90 @@ -53,6 +53,7 @@ program UnsteadyAero_Driver type(LD_ContinuousStateType) :: LD_x ! Continuous states type(LD_DiscreteStateType) :: LD_xd ! Discrete states type(LD_OtherStateType) :: LD_OtherState ! Other/optimization states + type(LD_ConstraintStateType) :: LD_z ! Constraint states type(LD_MiscVarType) :: LD_m ! Misc/optimization variables type(LD_ParameterType) :: LD_p ! Parameters type(LD_InputType) :: LD_u(NumInp) ! System inputs @@ -118,10 +119,34 @@ program UnsteadyAero_Driver simTime = dvrInitInp%TMax dt = dvrInitInp%dt nSimSteps = int(simTime/dt) ! TODO - print*,'nSimSteps',nSimSteps + print*,'nSimSteps',nSimSteps, simTime, dt ! --- Initialize Elastic Section - !call LD_Init(LD_InitInData, LD_u(1), LD_p, LD_x, LD_xd, LD_O, LD_y, LD_m, LD_InitOutData, errStat, errMsg); call checkError() + call LD_InitInputData(3, LD_InitInData, errStat, errMsg); call checkError() + LD_InitInData%dt = dt + LD_InitInData%IntMethod = 1 ! TODO + LD_InitInData%prefix = '' ! TODO for output channel names + LD_InitInData%MM = dvrInitInp%MM + LD_InitInData%CC = dvrInitInp%CC + LD_InitInData%KK = dvrInitInp%KK + LD_InitInData%x0 = dvrInitInp%initPos + LD_InitInData%xd0 = dvrInitInp%initVel + LD_InitInData%activeDOFs = dvrInitInp%activeDOFs + call LD_Init(LD_InitInData, LD_u(1), LD_p, LD_x, LD_xd, LD_z, LD_OtherState, LD_y, LD_m, LD_InitOutData, errStat, errMsg); call checkError() + + ! set inputs: + !u(1) = time at n=1 (t= 0) + !u(2) = time at n=0 (t= -dt) + !u(3) = time at n=-1 (t= -2dt) if NumInp > 2 + ! t = (n-1)*dt + do iu = 1, NumInp !u(NumInp) is overwritten in time-sim loop, so no need to init here + uTimes(iu) = (2-iu-1)*dt + enddo + ! Allocs + do iu = 2,NumInp + call AllocAry(LD_u(iu)%Fext, LD_p%nx, 'Fext', errStat, errMsg); call checkError() + enddo + end if ! --- Init UA input data based on driver inputs @@ -136,21 +161,54 @@ program UnsteadyAero_Driver end if + if ( dvrInitInp%SimMod == 3 ) then + + LD_u(1)%Fext=0.0_ReKi ! TODO TODO + LD_u(2)%Fext=0.0_ReKi ! TODO TODO + + ! --- time marching loop + do n = 1, nSimSteps + ! set inputs: + DO iu = NumInp-1, 1, -1 + LD_u( iu+1) = LD_u( iu) + uTimes(iu+1) = uTimes(iu) + END DO +! ! first value of uTimes/u contain inputs at t+dt +! call setUAinputs(n+1, u(1), uTimes(1), dt, dvrInitInp, timeArr, AOAarr, Uarr, OmegaArr, errStat, errMsg); call checkError() + uTimes(1) = (n+1-1)*dt + + t = uTimes(2) + ! Use existing states to compute the outputs + call LD_CalcOutput(t, LD_u(2), LD_p, LD_x, LD_xd, LD_z, LD_OtherState, LD_y, LD_m, errStat, errMsg); call checkError() + !! Use existing states to compute the outputs + !call UA_CalcOutput(i, j, t, u(2), p, x, xd, OtherState, AFI_Params(AFIndx(i,j)), y, m, errStat, errMsg ); call checkError() + print*,'t',t, LD_x%q +! ! Generate file outputs +! call UA_WriteOutputToFile(t, p, y) + ! Prepare states for next time step + call LD_UpdateStates(t, n, LD_u, uTimes, LD_p, LD_x, LD_xd, LD_z, LD_OtherState, LD_m, errStat, errMsg); call checkError() +! ! Prepare states for next time step +! call UA_UpdateStates(i, j, t, n, u, uTimes, p, x, xd, OtherState, AFI_Params(AFIndx(i,j)), m, errStat, errMsg ); call checkError() + end do + + print*,'STOPPING FOR NOW' + call cleanUp() + call NormStop() + endif ! set inputs: !u(1) = time at n=1 (t= 0) !u(2) = time at n=0 (t= -dt) !u(3) = time at n=-1 (t= -2dt) if NumInp > 2 - DO iu = 1, NumInp-1 !u(NumInp) is overwritten in time-sim loop, so no need to init here call setUAinputs(2-iu, u(iu), uTimes(iu), dt, dvrInitInp, timeArr, AOAarr, Uarr, OmegaArr, errStat, errMsg); call checkError() END DO ! --- time marching loop - do n = 1, nSimSteps + i = 1 ! nodes per blade + j = 1 ! number of blades - i = 1 ! nodes per blade - j = 1 ! number of blades + do n = 1, nSimSteps ! set inputs: DO iu = NumInp-1, 1, -1 diff --git a/modules/lindyn/src/LinDyn.f90 b/modules/lindyn/src/LinDyn.f90 index dda9c588c9..039e9ce787 100644 --- a/modules/lindyn/src/LinDyn.f90 +++ b/modules/lindyn/src/LinDyn.f90 @@ -45,7 +45,7 @@ module LinDyn ! contains -subroutine LD_Init(InitInp, u, p, x, xd, z, OtherState, y, m, dt_gluecode, InitOut, errStat, errMsg) +subroutine LD_Init(InitInp, u, p, x, xd, z, OtherState, y, m, InitOut, errStat, errMsg) type(LD_InitInputType), intent(in ) :: InitInp !< Input data for initialization routine type(LD_InputType), intent(out) :: u !< An initial guess for the input; input mesh must be defined type(LD_ParameterType), intent(out) :: p !< Parameters @@ -55,13 +55,11 @@ subroutine LD_Init(InitInp, u, p, x, xd, z, OtherState, y, m, dt_gluecode, InitO type(LD_OtherStateType), intent(out) :: OtherState !< Initial other states (logical, etc) type(LD_OutputType), intent(out) :: y !< Initial system outputs (outputs are not calculated; type(LD_MiscVarType), intent(out) :: m !< Misc variables for optimization (not copied in glue code) - real(DbKi), intent(inout) :: dt_gluecode !< Coupling interval in seconds: the rate that type(LD_InitOutputType), intent(out) :: InitOut !< Output for initialization routine integer(IntKi), intent(out) :: errStat !< Error status of the operation character(*), intent(out) :: errMsg !< Error message if errStat /= ErrID_None integer(IntKi) :: errStat2 ! Status of error message character(1024) :: errMsg2 ! Error message if ErrStat /= ErrID_None - integer(IntKi) :: i, n ! Loop counter ! Misc Init errStat = ErrID_None errMsg = "" @@ -69,11 +67,12 @@ subroutine LD_Init(InitInp, u, p, x, xd, z, OtherState, y, m, dt_gluecode, InitO call DispNVD( LD_Ver ) ! Display the module information ! --- Setting Params from InitInp - n = size(p%MM,1) - call AllocAry(p%MM , n, n, 'MM', errStat2, errMsg2); if(Failed()) return - call AllocAry(p%CC , n, n, 'CC', errStat2, errMsg2); if(Failed()) return - call AllocAry(p%KK , n, n, 'KK', errStat2, errMsg2); if(Failed()) return - call AllocAry(p%activeDOFs, n , 'activeDOFs', errStat2, errMsg2); if(Failed()) return + p%nx = size(InitInp%MM,1) + p%nq = 2*p%nx + call AllocAry(p%MM , p%nx, p%nx, 'MM', errStat2, errMsg2); if(Failed()) return + call AllocAry(p%CC , p%nx, p%nx, 'CC', errStat2, errMsg2); if(Failed()) return + call AllocAry(p%KK , p%nx, p%nx, 'KK', errStat2, errMsg2); if(Failed()) return + call AllocAry(p%activeDOFs, p%nx , 'activeDOFs', errStat2, errMsg2); if(Failed()) return p%dt = InitInp%dt p%IntMethod = InitInp%IntMethod p%MM = InitInp%MM @@ -81,74 +80,74 @@ subroutine LD_Init(InitInp, u, p, x, xd, z, OtherState, y, m, dt_gluecode, InitO p%KK = InitInp%KK p%activeDOFs = InitInp%activeDOFs -! INTERFACE LAPACK_getri -! SUBROUTINE LAPACK_DGETRI( N, A, IPIV, WORK, LWORK, ErrStat, ErrMsg ) + print*,'' + print*,'M',p%MM(1,:) + print*,'M',p%MM(2,:) + print*,'M',p%MM(3,:) + print*,'' + print*,'C',p%CC(1,:) + print*,'C',p%CC(2,:) + print*,'C',p%CC(3,:) + print*,'' + print*,'K',p%KK(1,:) + print*,'K',p%KK(2,:) + print*,'K',p%KK(3,:) + print*,'' + call StateMatrices(p%MM, p%CC, p%KK, p%AA, p%BB, errStat2, errMsg2); if(Failed()) return + print*,'' + print*,'A',p%AA(1,:) + print*,'A',p%AA(2,:) + print*,'A',p%AA(3,:) + print*,'A',p%AA(4,:) + print*,'A',p%AA(5,:) + print*,'A',p%AA(6,:) + print*,'' + print*,'B',p%BB(1,:) + print*,'B',p%BB(2,:) + print*,'B',p%BB(3,:) + print*,'B',p%BB(4,:) + print*,'B',p%BB(5,:) + print*,'B',p%BB(6,:) + + ! --- Allocate STates + call AllocAry( x%q , p%nq,'DOFs' , errStat,errMsg); if(Failed()) return + x%q( 1:p%nx) = InitInp%x0 + x%q(p%nx+1:p%nq) = InitInp%xd0 + + ! allocate OtherState%xdot if using multi-step method; initialize n + if ( ( p%IntMethod .eq. 2) .OR. ( p%IntMethod .eq. 3)) THEN + allocate( OtherState%xdot(4), STAT=errStat2); errMsg2='Error allocating OtherState%xdot' + if(Failed()) return + endif + + ! --- Initialize Misc Variables: + +! ! Define initial guess (set up mesh first) for the system inputs here: +! call Init_meshes(u, y, InitInp, errStat, errMsg); if(Failed()) return + ! --- Guess inputs + call AllocAry(u%Fext, p%nx, 'Fext', errStat2, errMsg2); if(Failed()) return + u%Fext=0.0_ReKi + + ! --- Outputs + call AllocAry(y%qd, p%nx, 'qd', errStat2, errMsg2); if(Failed()) return + y%qd = 0.0_ReKi + y%qd(1:p%nx) = InitInp%xd0 + + ! --- Write Outputs + p%NumOuts = 0 ! ! Setting p%OutParam from OutList ! call SetOutParam(InputFileData%OutList, InputFileData%NumOuts, p, errStat, errMsg); if(Failed()) return -! ! Set the constant state matrices A,B,C,D -! call SetStateMatrices(p, errStat, errMsg) ! -! ! --- Allocate and init continuous states -! call AllocAry( x%qm , p%nCB,'CB DOF positions' , errStat,errMsg); if(Failed()) return -! call AllocAry( x%qmdot , p%nCB,'CB DOF velocities', errStat,errMsg); if(Failed()) return -! if (allocated(InputFileData%InitPosList)) then -! if (size(InputFileData%InitPosList)/=p%nCB) then -! call SeterrStat(ErrID_Fatal, 'The number of elements of `InitPosList` ('//trim(Num2LStr(size(InputFileData%InitPosList)))//') does not match the number of CB modes: '//trim(Num2LStr(p%nCB)), errStat, errMsg, 'LD_Init'); -! return -! endif -! do I=1,p%nCB; -! x%qm(I)=InputFileData%InitPosList(I); -! end do -! else -! do I=1,p%nCB; x%qm (I)=0; end do -! endif -! if (allocated(InputFileData%InitVelList)) then -! if (size(InputFileData%InitVelList)/=p%nCB) then -! call SeterrStat(ErrID_Fatal, 'The number of elements of `InitVelList` ('//trim(Num2LStr(size(InputFileData%InitVelList)))//') does not match the number of CB modes: '//trim(Num2LStr(p%nCB)), errStat, errMsg, 'LD_Init'); -! return -! endif -! do I=1,p%nCB; -! x%qmdot(I)=InputFileData%InitVelList(I); -! enddo -! else -! do I=1,p%nCB; x%qmdot(I)=0; end do -! endif -! -! ! Other states -! xd%DummyDiscState = 0.0_ReKi -! z%DummyConstrState = 0.0_ReKi -! ! allocate OtherState%xdot if using multi-step method; initialize n -! if ( ( p%IntMethod .eq. 2) .OR. ( p%IntMethod .eq. 3)) THEN -! allocate( OtherState%xdot(4), STAT=errStat ) -! errMsg='Error allocating OtherState%xdot' -! if(Failed()) return -! endif -! -! ! Initialize Misc Variables: -! !m%EquilStart = InputFileData%EquilStart -! m%EquilStart = .False. ! Feature not yet implemented -! -! m%Indx = 1 ! used to optimize interpolation of loads in time -! call AllocAry( m%F_at_t, p%nTot,'Loads at t', errStat,errMsg); if(Failed()) return -! do I=1,p%nTot; m%F_at_t(I)=0; end do -! call AllocAry( m%xFlat, 2*p%nCB,'xFlat', errStat,errMsg); if(Failed()) return -! do I=1,2*p%nCB; m%xFlat(I)=0; end do -! do I=1,N_inPUTS; m%uFlat(I)=0; end do -! -! ! Define initial guess (set up mesh first) for the system inputs here: -! call Init_meshes(u, y, InitInp, errStat, errMsg); if(Failed()) return -! -! ! --- Outputs -! call AllocAry( m%AllOuts, ID_QStart+3*p%nCBFull-1, "LinDyn AllOut", errStat,errMsg ); if(Failed()) return -! m%AllOuts(1:ID_QStart+3*p%nCBFull-1) = 0.0 -! call AllocAry( y%WriteOutput, p%NumOuts,'WriteOutput', errStat,errMsg); if(Failed()) return -! call AllocAry(InitOut%WriteOutputHdr,p%NumOuts,'WriteOutputHdr',errStat,errMsg); if(Failed()) return -! call AllocAry(InitOut%WriteOutputUnt,p%NumOuts,'WriteOutputUnt',errStat,errMsg); if(Failed()) return -! y%WriteOutput(1:p%NumOuts) = 0.0 -! InitOut%WriteOutputHdr(1:p%NumOuts) = p%OutParam(1:p%NumOuts)%Name -! InitOut%WriteOutputUnt(1:p%NumOuts) = p%OutParam(1:p%NumOuts)%Units + call AllocAry( m%AllOuts, p%NumOuts, "LinDyn AllOut", errStat,errMsg ); if(Failed()) return + m%AllOuts(:) = 0.0_ReKi + call AllocAry( y%WriteOutput, p%NumOuts,'WriteOutput', errStat,errMsg); if(Failed()) return + call AllocAry(InitOut%WriteOutputHdr,p%NumOuts,'WriteOutputHdr',errStat,errMsg); if(Failed()) return + call AllocAry(InitOut%WriteOutputUnt,p%NumOuts,'WriteOutputUnt',errStat,errMsg); if(Failed()) return + y%WriteOutput(1:p%NumOuts) = 0.0 + !InitOut%WriteOutputHdr(1:p%NumOuts) = p%OutParam(1:p%NumOuts)%Name + !InitOut%WriteOutputUnt(1:p%NumOuts) = p%OutParam(1:p%NumOuts)%Units InitOut%Ver = LD_Ver ! ! if (InitInp%Linearize) then @@ -187,7 +186,6 @@ subroutine LD_Init(InitInp, u, p, x, xd, z, OtherState, y, m, dt_gluecode, InitO ! InitOut%LinNames_x(I) = 'Mode '//trim(Num2LStr(p%ActiveCBDOF(I)))//' displacement, -'; ! InitOut%LinNames_x(I+p%nCB) = 'Mode '//trim(Num2LStr(p%ActiveCBDOF(I)))//' velocity, -'; ! enddo -! ! ! InitOut%RotFrame_x = .false. ! note that meshes are in the global, not rotating frame ! InitOut%RotFrame_y = .false. ! note that meshes are in the global, not rotating frame ! InitOut%RotFrame_u = .false. ! note that meshes are in the global, not rotating frame @@ -200,99 +198,118 @@ subroutine LD_Init(InitInp, u, p, x, xd, z, OtherState, y, m, dt_gluecode, InitO ! endif ! contains - logical function Failed() - call SeterrStatSimple(errStat, errMsg, 'LD_Init') - Failed = errStat >= AbortErrLev - end function Failed + logical function Failed() + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'LD_Init' ) + Failed = ErrStat >= AbortErrLev + if (Failed) call CleanUp() + end function Failed + subroutine CleanUp() + end subroutine CleanUp end subroutine LD_Init -! -! !---------------------------------------------------------------------------------------------------------------------------------- -! subroutine SetStateMatrices( p, errStat, errMsg) -! subroutine SetStateMatrices( p, errStat, errMsg) -! !.................................................................................................................................. -! type(LD_ParameterType), intent(inout) :: p !< All the parameter matrices stored in this input file -! integer(IntKi), intent(out) :: errStat !< Error status -! character(*), intent(out) :: errMsg !< Error message -! ! Local variables: -! integer(IntKi) :: I ! loop counter -! integer(IntKi) :: nX ! Number of states -! integer(IntKi) :: nU ! Number of inputs -! integer(IntKi) :: nY ! Number of ouputs -! integer(IntKi) :: n1 ! Number of interface DOF -! integer(IntKi) :: n2 ! Number of CB DOF -! real(ReKi), dimension(:,:), allocatable :: I22 -! ! Init -! nX = 2*p%nCB -! nU = 3*6 -! nY = 6 -! n1 = 6 -! n2 = p%nCB -! if (allocated(p%AMat)) deallocate(p%AMat) -! if (allocated(p%BMat)) deallocate(p%BMat) -! if (allocated(p%CMat)) deallocate(p%CMat) -! if (allocated(p%DMat)) deallocate(p%DMat) -! if (allocated(p%M11)) deallocate(p%M11) -! if (allocated(p%M12)) deallocate(p%M12) -! if (allocated(p%M22)) deallocate(p%M22) -! if (allocated(p%M21)) deallocate(p%M21) -! if (allocated(p%C11)) deallocate(p%C11) -! if (allocated(p%C12)) deallocate(p%C12) -! if (allocated(p%C22)) deallocate(p%C22) -! if (allocated(p%C21)) deallocate(p%C21) -! if (allocated(p%K11)) deallocate(p%C11) -! if (allocated(p%K22)) deallocate(p%C22) -! ! Allocation -! call allocAry(p%AMat, nX, nX, 'p%AMat', errStat, errMsg); if(Failed()) return ; p%AMat(1:nX,1:nX) =0 -! call allocAry(p%BMat, nX, nU, 'p%BMat', errStat, errMsg); if(Failed()) return ; p%BMat(1:nX,1:nU) =0 -! call allocAry(p%FX , nX, 'p%FX' , errStat, errMsg); if(Failed()) return ; p%Fx (1:nX) =0 -! call allocAry(p%CMat, nY, nX, 'p%CMat', errStat, errMsg); if(Failed()) return ; p%CMat(1:nY,1:nX) =0 -! call allocAry(p%DMat, nY, nU, 'p%DMat', errStat, errMsg); if(Failed()) return ; p%DMat(1:nY,1:nU) =0 -! call allocAry(p%FY , nY, 'p%FY' , errStat, errMsg); if(Failed()) return ; p%FY (1:nY) =0 -! call allocAry(p%M11 , n1, n1, 'p%M11' , errStat, errMsg); if(Failed()) return ; p%M11 (1:n1,1:n1) =0 -! call allocAry(p%K11 , n1, n1, 'p%K11' , errStat, errMsg); if(Failed()) return ; p%K11 (1:n1,1:n1) =0 -! call allocAry(p%C11 , n1, n1, 'p%C11' , errStat, errMsg); if(Failed()) return ; p%C11 (1:n1,1:n1) =0 -! call allocAry(p%M22 , n2, n2, 'p%M22' , errStat, errMsg); if(Failed()) return ; p%M22 (1:n2,1:n2) =0 -! call allocAry(p%K22 , n2, n2, 'p%K22' , errStat, errMsg); if(Failed()) return ; p%K22 (1:n2,1:n2) =0 -! call allocAry(p%C22 , n2, n2, 'p%C22' , errStat, errMsg); if(Failed()) return ; p%C22 (1:n2,1:n2) =0 -! call allocAry(p%M12 , n1, n2, 'p%M12' , errStat, errMsg); if(Failed()) return ; p%M12 (1:n1,1:n2) =0 -! call allocAry(p%C12 , n1, n2, 'p%C12' , errStat, errMsg); if(Failed()) return ; p%C12 (1:n1,1:n2) =0 -! call allocAry(p%M21 , n2, n1, 'p%M21' , errStat, errMsg); if(Failed()) return ; p%M21 (1:n2,1:n1) =0 -! call allocAry(p%C21 , n2, n1, 'p%C21' , errStat, errMsg); if(Failed()) return ; p%C21 (1:n2,1:n1) =0 -! call allocAry( I22 , n2, n2, ' I22' , errStat, errMsg); if(Failed()) return ; I22 (1:n2,1:n2) =0 -! do I=1,n2 ; I22(I,I)=1; enddo ! Identity matrix -! ! Submatrices -! p%M11(1:n1,1:n1) = p%Mass(1:n1 ,1:n1 ) -! p%C11(1:n1,1:n1) = p%Damp(1:n1 ,1:n1 ) -! p%K11(1:n1,1:n1) = p%Stff(1:n1 ,1:n1 ) -! p%M12(1:n1,1:n2) = p%Mass(1:n1 ,n1+1:n1+n2) -! p%C12(1:n1,1:n2) = p%Damp(1:n1 ,n1+1:n1+n2) -! p%M21(1:n2,1:n1) = p%Mass(n1+1:n1+n2,1:n1 ) -! p%C21(1:n2,1:n1) = p%Damp(n1+1:n1+n2,1:n1 ) -! p%M22(1:n2,1:n2) = p%Mass(n1+1:n1+n2,n1+1:n1+n2) -! p%C22(1:n2,1:n2) = p%Damp(n1+1:n1+n2,n1+1:n1+n2) -! p%K22(1:n2,1:n2) = p%Stff(n1+1:n1+n2,n1+1:n1+n2) -! ! A matrix -! p%AMat(1:n2 ,n2+1:nX) = I22 (1:n2,1:n2) -! p%AMat(n2+1:nX,1:n2 ) = -p%K22(1:n2,1:n2) -! p%AMat(n2+1:nX,n2+1:nX) = -p%C22(1:n2,1:n2) -! ! B matrix -! p%BMat(n2+1:nX,7 :12 ) = -p%C21(1:n2,1:6) -! p%BMat(n2+1:nX,13:18 ) = -p%M21(1:n2,1:6) -! ! C matrix -! p%CMat(1:nY,1:n2 ) = matmul(p%M12,p%K22) -! p%CMat(1:nY,n2+1:nX) = matmul(p%M12,p%C22) - p%C12 -! ! D matrix -! p%DMat(1:nY,1:6 ) = -p%K11 -! p%DMat(1:nY,7:12 ) = -p%C11 + matmul(p%M12,p%C21) -! p%DMat(1:nY,13:18 ) = -p%M11 + matmul(p%M12,p%M21) -! CONTAinS -! logical function Failed() -! call SeterrStatSimple(errStat, errMsg, 'LD_SetStateMatrices') -! Failed = errStat >= AbortErrLev -! end function Failed -! end subroutine SetStateMatrices -! !---------------------------------------------------------------------------------------------------------------------------------- +!> Allocate init input data for module based on number of degrees of freedom +subroutine LD_InitInputData(nx, InitInp, errStat, errMsg) + integer(IntKi), intent(in ) :: nx !< Number of degrees of freedom + type(LD_InitInputType), intent(out) :: InitInp !< Input data for initialization routine + integer(IntKi), intent(out) :: errStat !< Error status of the operation + character(*), intent(out) :: errMsg !< Error message if errStat /= ErrID_None + integer(IntKi) :: errStat2 ! Status of error message + character(1024) :: errMsg2 ! Error message if ErrStat /= ErrID_None + ! Initialize errStat + errStat = ErrID_None ! no error has occurred + errMsg = "" + call AllocAry(InitInp%MM , nx, nx, 'MM' , errStat2, errMsg2); if(Failed()) return + call AllocAry(InitInp%CC , nx, nx, 'CC' , errStat2, errMsg2); if(Failed()) return + call AllocAry(InitInp%KK , nx, nx, 'KK' , errStat2, errMsg2); if(Failed()) return + call AllocAry(InitInp%x0 , nx , 'x0' , errStat2, errMsg2); if(Failed()) return + call AllocAry(InitInp%xd0 , nx , 'xd0', errStat2, errMsg2); if(Failed()) return + call AllocAry(InitInp%activeDOFs, nx , 'activeDOFs', errStat2, errMsg2); if(Failed()) return + InitInp%MM = 0.0_ReKi + InitInp%CC = 0.0_ReKi + InitInp%KK = 0.0_ReKi + InitInp%x0 = 0.0_ReKi + InitInp%xd0 = 0.0_ReKi + InitInp%activeDOFs = .True. +contains + logical function Failed() + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'LD_Init' ) + Failed = ErrStat >= AbortErrLev + end function Failed +end subroutine LD_InitInputData +!---------------------------------------------------------------------------------------------------------------------------------- +!> Compute A and B state matrices for a linear mechanical system +!! NOTE: Generic function (no derived types), keep it that way +!! A = [ 0 I ] B = [0 ] +!! [-M^{-1}K -M^{-1}C ] = [-M^{-1}] +subroutine StateMatrices(MM, CC, KK, AA, BB, errStat, errMsg) + real(ReKi), intent(in ) :: MM(:,:) + real(ReKi), intent(in ) :: CC(:,:) + real(ReKi), intent(in ) :: KK(:,:) + real(ReKi), allocatable, intent(out) :: AA(:,:) + real(ReKi), allocatable, intent(out) :: BB(:,:) + integer(IntKi), intent(out) :: errStat !< Error status of the operation + character(*), intent(out) :: errMsg !< Error message if errStat /= ErrID_None + integer(IntKi) :: errStat2 ! Status of error message + character(1024) :: errMsg2 ! Error message if ErrStat /= ErrID_None + integer :: nx, nq, i + real(ReKi), dimension(:,:), allocatable :: MLU ! LU factorization of M matrix + real(ReKi), dimension(:,:), allocatable :: MinvX ! Tmp array to store either: M^{-1} C, M^{-1} K , or M^{-1} + real(ReKi), dimension(:) , allocatable :: WORK ! LAPACK variable + integer, allocatable :: IPIV(:) ! LAPACK variable + integer :: LWORK ! LAPACK variable + ! Initialize errStat + errStat = ErrID_None + errMsg = "" + + ! --- Init A and B matrix + nx = size(MM,1) + nq = 2*nx + call AllocAry(AA, nq, nq, 'AA', errStat2, errMsg2); if(Failed()) return + call AllocAry(BB, nq, nx, 'BB', errStat2, errMsg2); if(Failed()) return + AA(:,:) = 0.0_ReKi + BB(:,:) = 0.0_ReKi + do i=1,nx ; AA(i,i+nx)=1; enddo ! Identity matrix for upper right block + + ! --- Compute misc inverse of M and put in A and B matrices + call AllocAry(IPIV , nx , 'IPIV' , errStat2, errMsg2); if(Failed()) return + call AllocAry(MinvX , nx, nx, 'MinvX', errStat2, errMsg2); if(Failed()) return + call AllocAry(MLU , nx, nx, 'MLU' , errStat2, errMsg2); if(Failed()) return + + ! LU Factorization of M + MLU = MM ! temp copy + call LAPACK_getrf(nx, nx, MLU, IPIV, errStat2, errMsg2); if(Failed()) return + + ! M^-1 C + MinvX = CC + call LAPACK_getrs('n', nx, MLU, IPIV, MinvX, errStat2, errMsg2); if(Failed()) return + AA(nx+1:nq,nx+1:nq) = -MinvX + + ! M^-1 K + MinvX = KK + call LAPACK_getrs('n', nx, MLU, IPIV, MinvX, errStat2, errMsg2); if(Failed()) return + AA(nx+1:nq, 1:nx) = -MinvX + + ! Inverse of M + MinvX = MLU + LWORK=nx*nx ! Somehow LWORK = -1 does not work + allocate(WORK(LWORk)) + call LAPACK_getri(nx, MinvX, IPIV, WORK, LWORK, errStat2, errMsg2); if(Failed()) return + BB(nx+1:nq, : ) = -MinvX + +contains + logical function Failed() + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'LD_Init' ) + Failed = ErrStat >= AbortErrLev + if (Failed) call CleanUp() + end function Failed + subroutine CleanUp() + if (allocated(MLU)) deallocate(MLU) + if (allocated(IPIV)) deallocate(IPIV) + if (allocated(WORK )) deallocate(WORK) + if (allocated(MinvX)) deallocate(MinvX) + end subroutine CleanUp +end subroutine StateMatrices +!---------------------------------------------------------------------------------------------------------------------------------- ! subroutine Init_meshes(u, y, InitInp, errStat, errMsg) ! type(LD_InputType), intent(inout) :: u !< System inputs ! type(LD_OutputType), intent(inout) :: y !< System outputs @@ -328,351 +345,257 @@ end subroutine LD_Init ! Failed = errStat >= AbortErrLev ! end function Failed ! end subroutine Init_meshes -! !---------------------------------------------------------------------------------------------------------------------------------- -! !> This routine is called at the end of the simulation. -! subroutine LD_End( u, p, x, xd, z, OtherState, y, m, errStat, errMsg ) -! !.................................................................................................................................. -! type(LD_InputType), intent(inout) :: u !< System inputs -! type(LD_ParameterType), intent(inout) :: p !< Parameters -! type(LD_ContinuousStateType), intent(inout) :: x !< Continuous states -! type(LD_DiscreteStateType), intent(inout) :: xd !< Discrete states -! type(LD_ConstraintStateType), intent(inout) :: z !< Constraint states -! type(LD_OtherStateType), intent(inout) :: OtherState !< Other states -! type(LD_OutputType), intent(inout) :: y !< System outputs -! type(LD_MiscVarType), intent(inout) :: m !< Misc variables for optimization (not copied in glue code) -! integer(IntKi), intent( out) :: errStat !< Error status of the operation -! character(*), intent( out) :: errMsg !< Error message if errStat /= ErrID_None -! ! Place any last minute operations or calculations here: -! ! Close files here (but because of checkpoint-restart capability, it is not recommended to have files open during the simulation): -! ! Destroy the input data: -! call LD_DestroyInput( u, errStat, errMsg ); if(Failed()) return -! ! Destroy the parameter data: -! call LD_DestroyParam( p, errStat, errMsg ); if(Failed()) return -! ! Destroy the state data: -! call LD_DestroyContState( x, errStat,errMsg); if(Failed()) return -! call LD_DestroyDiscState( xd, errStat,errMsg); if(Failed()) return -! call LD_DestroyConstrState( z, errStat,errMsg); if(Failed()) return -! call LD_DestroyOtherState( OtherState, errStat,errMsg); if(Failed()) return -! ! Destroy the output data: -! call LD_DestroyOutput( y, errStat, errMsg ); if(Failed()) return -! ! Destroy the misc data: -! call LD_DestroyMisc( m, errStat, errMsg ); if(Failed()) return -! CONTAinS -! logical function Failed() -! call SeterrStatSimple(errStat, errMsg, 'LD_End') -! Failed = errStat >= AbortErrLev -! end function Failed -! end subroutine LD_End -! -! -! !---------------------------------------------------------------------------------------------------------------------------------- -! !> This subroutine implements the fourth-order Adams-Bashforth Method (RK4) for numerically integrating ordinary differential -! !! equations: -! !! Let f(t, x) = xdot denote the time (t) derivative of the continuous states (x). -! !! x(t+dt) = x(t) + (dt / 24.) * ( 55.*f(t,x) - 59.*f(t-dt,x) + 37.*f(t-2.*dt,x) - 9.*f(t-3.*dt,x) ) -! !! See, e.g., -! !! http://en.wikipedia.org/wiki/Linear_multistep_method -! !! K. E. Atkinson, "An Introduction to Numerical Analysis", 1989, John Wiley & Sons, Inc, Second Edition. -! subroutine LD_AB4( t, n, u, utimes, p, x, xd, z, OtherState, m, errStat, errMsg ) -! !.................................................................................................................................. -! real(DbKi), intent(in ) :: t !< Current simulation time in seconds -! integer(IntKi), intent(in ) :: n !< time step number -! type(LD_InputType), intent(inout) :: u(:) !< Inputs at t -! real(DbKi), intent(in ) :: utimes(:) !< times of input -! type(LD_ParameterType), intent(in ) :: p !< Parameters -! type(LD_ContinuousStateType), intent(inout) :: x !< Continuous states at t on input at t + dt on output -! type(LD_DiscreteStateType), intent(in ) :: xd !< Discrete states at t -! type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t (possibly a guess) -! type(LD_OtherStateType), intent(inout) :: OtherState !< Other states at t on input at t + dt on output -! type(LD_MiscVarType), intent(inout) :: m !< Misc/optimization variables -! integer(IntKi), intent( out) :: errStat !< Error status of the operation -! character(*), intent( out) :: errMsg !< Error message if errStat /= ErrID_None -! ! local variables -! type(LD_ContinuousStateType) :: xdot ! Continuous state derivs at t -! type(LD_InputType) :: u_interp -! ! Initialize errStat -! errStat = ErrID_None -! errMsg = "" -! -! ! need xdot at t -! call LD_CopyInput(u(1), u_interp, MESH_NEWCOPY, errStat, errMsg ) ! we need to allocate input arrays/meshes before calling ExtrapInterp... -! call LD_Input_ExtrapInterp(u, utimes, u_interp, t, errStat, errMsg) -! call LD_CalcContStateDeriv( t, u_interp, p, x, xd, z, OtherState, m, xdot, errStat, errMsg ) ! initializes xdot -! call LD_DestroyInput( u_interp, errStat, errMsg) ! we don't need this local copy anymore -! if (n .le. 2) then -! OtherState%n = n -! call LD_CopyContState(xdot, OtherState%xdot(3-n), MESH_UPDATECOPY, errStat, errMsg ) -! call LD_RK4(t, n, u, utimes, p, x, xd, z, OtherState, m, errStat, errMsg ) -! else -! if (OtherState%n .lt. n) then -! OtherState%n = n -! call LD_CopyContState(OtherState%xdot(3), OtherState%xdot(4), MESH_UPDATECOPY, errStat, errMsg ) -! call LD_CopyContState(OtherState%xdot(2), OtherState%xdot(3), MESH_UPDATECOPY, errStat, errMsg ) -! call LD_CopyContState(OtherState%xdot(1), OtherState%xdot(2), MESH_UPDATECOPY, errStat, errMsg ) -! elseif (OtherState%n .gt. n) then -! errStat = ErrID_Fatal -! errMsg = ' Backing up in time is not supported with a multistep method ' -! RETURN -! endif -! call LD_CopyContState( xdot, OtherState%xdot ( 1 ), MESH_UPDATECOPY, errStat, errMsg ) -! !OtherState%xdot ( 1 ) = xdot ! make sure this is most up to date -! x%qm = x%qm + (p%EP_DeltaT / 24.) * ( 55.*OtherState%xdot(1)%qm - 59.*OtherState%xdot(2)%qm + 37.*OtherState%xdot(3)%qm & -! - 9. * OtherState%xdot(4)%qm ) -! x%qmdot = x%qmdot + (p%EP_DeltaT / 24.) * ( 55.*OtherState%xdot(1)%qmdot - 59.*OtherState%xdot(2)%qmdot & -! + 37.*OtherState%xdot(3)%qmdot - 9.*OtherState%xdot(4)%qmdot ) -! endif -! call LD_DestroyContState(xdot, errStat, errMsg) -! call LD_DestroyInput(u_interp, errStat, errMsg) -! -! end subroutine LD_AB4 -! !---------------------------------------------------------------------------------------------------------------------------------- -! !> This subroutine implements the fourth-order Adams-Bashforth-Moulton Method (RK4) for numerically integrating ordinary -! !! differential equations: -! !! Let f(t, x) = xdot denote the time (t) derivative of the continuous states (x). -! !! Adams-Bashforth Predictor: -! !! x^p(t+dt) = x(t) + (dt / 24.) * ( 55.*f(t,x) - 59.*f(t-dt,x) + 37.*f(t-2.*dt,x) - 9.*f(t-3.*dt,x) ) -! !! Adams-Moulton Corrector: -! !! x(t+dt) = x(t) + (dt / 24.) * ( 9.*f(t+dt,x^p) + 19.*f(t,x) - 5.*f(t-dt,x) + 1.*f(t-2.*dt,x) ) -! !! See, e.g., -! !! http://en.wikipedia.org/wiki/Linear_multistep_method -! !! K. E. Atkinson, "An Introduction to Numerical Analysis", 1989, John Wiley & Sons, Inc, Second Edition. -! subroutine LD_ABM4( t, n, u, utimes, p, x, xd, z, OtherState, m, errStat, errMsg ) -! !.................................................................................................................................. -! real(DbKi), intent(in ) :: t !< Current simulation time in seconds -! integer(IntKi), intent(in ) :: n !< time step number -! type(LD_InputType), intent(inout) :: u(:) !< Inputs at t -! real(DbKi), intent(in ) :: utimes(:) !< times of input -! type(LD_ParameterType), intent(in ) :: p !< Parameters -! type(LD_ContinuousStateType), intent(inout) :: x !< Continuous states at t on input at t + dt on output ! TODO TODO TODO in -! type(LD_DiscreteStateType), intent(in ) :: xd !< Discrete states at t -! type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t (possibly a guess) -! type(LD_OtherStateType), intent(inout) :: OtherState !< Other states at t on input at t + dt on output -! type(LD_MiscVarType), intent(inout) :: m !< Misc/optimization variables -! integer(IntKi), intent( out) :: errStat !< Error status of the operation -! character(*), intent( out) :: errMsg !< Error message if errStat /= ErrID_None -! ! local variables -! type(LD_InputType) :: u_interp ! Continuous states at t -! type(LD_ContinuousStateType) :: x_pred ! Continuous states at t -! type(LD_ContinuousStateType) :: xdot_pred ! Continuous states at t -! -! ! Initialize errStat -! errStat = ErrID_None -! errMsg = "" -! -! call LD_CopyContState(x, x_pred, MESH_NEWCOPY, errStat, errMsg) !initialize x_pred -! call LD_AB4( t, n, u, utimes, p, x_pred, xd, z, OtherState, m, errStat, errMsg ) -! if (n .gt. 2) then -! call LD_CopyInput( u(1), u_interp, MESH_NEWCOPY, errStat, errMsg) ! make copy so that arrays/meshes get initialized/allocated for ExtrapInterp -! call LD_Input_ExtrapInterp(u, utimes, u_interp, t + p%EP_DeltaT, errStat, errMsg) -! call LD_CalcContStateDeriv(t + p%EP_DeltaT, u_interp, p, x_pred, xd, z, OtherState, m, xdot_pred, errStat, errMsg ) ! initializes xdot_pred -! call LD_DestroyInput( u_interp, errStat, errMsg) ! local copy no longer needed -! -! x%qm = x%qm + (p%EP_DeltaT / 24.) * ( 9. * xdot_pred%qm + 19. * OtherState%xdot(1)%qm - 5. * OtherState%xdot(2)%qm & -! + 1. * OtherState%xdot(3)%qm ) -! -! x%qmdot = x%qmdot + (p%EP_DeltaT / 24.) * ( 9. * xdot_pred%qmdot + 19. * OtherState%xdot(1)%qmdot - 5. * OtherState%xdot(2)%qmdot & -! + 1. * OtherState%xdot(3)%qmdot ) -! call LD_DestroyContState( xdot_pred, errStat, errMsg) ! local copy no longer needed -! else -! x%qm = x_pred%qm -! x%qmdot = x_pred%qmdot -! endif -! call LD_DestroyContState( x_pred, errStat, errMsg) ! local copy no longer needed -! end subroutine LD_ABM4 -! -! !---------------------------------------------------------------------------------------------------------------------------------- -! !> This subroutine implements the fourth-order Runge-Kutta Method (RK4) for numerically integrating ordinary differential equations: -! !! Let f(t, x) = xdot denote the time (t) derivative of the continuous states (x). -! !! Define constants k1, k2, k3, and k4 as -! !! k1 = dt * f(t , x_t ) -! !! k2 = dt * f(t + dt/2 , x_t + k1/2 ) -! !! k3 = dt * f(t + dt/2 , x_t + k2/2 ), and -! !! k4 = dt * f(t + dt , x_t + k3 ). -! !! Then the continuous states at t = t + dt are -! !! x_(t+dt) = x_t + k1/6 + k2/3 + k3/3 + k4/6 + O(dt^5) -! !! For details, see: -! !! Press, W. H.; Flannery, B. P.; Teukolsky, S. A.; and Vetterling, W. T. "Runge-Kutta Method" and "Adaptive Step Size Control for -! !! Runge-Kutta." sections 16.1 and 16.2 in Numerical Recipes in FORTRAN: The Art of Scientific Computing, 2nd ed. Cambridge, England: -! !! Cambridge University Press, pp. 704-716, 1992. -! subroutine LD_RK4( t, n, u, utimes, p, x, xd, z, OtherState, m, errStat, errMsg ) -! !.................................................................................................................................. -! real(DbKi), intent(in ) :: t !< Current simulation time in seconds -! integer(IntKi), intent(in ) :: n !< time step number -! type(LD_InputType), intent(inout) :: u(:) !< Inputs at t -! real(DbKi), intent(in ) :: utimes(:) !< times of input -! type(LD_ParameterType), intent(in ) :: p !< Parameters -! type(LD_ContinuousStateType), intent(inout) :: x !< Continuous states at t on input at t + dt on output -! type(LD_DiscreteStateType), intent(in ) :: xd !< Discrete states at t -! type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t (possibly a guess) -! type(LD_OtherStateType), intent(inout) :: OtherState !< Other states at t on input at t + dt on output -! type(LD_MiscVarType), intent(inout) :: m !< Misc/optimization variables -! integer(IntKi), intent( out) :: errStat !< Error status of the operation -! character(*), intent( out) :: errMsg !< Error message if errStat /= ErrID_None -! ! local variables -! type(LD_ContinuousStateType) :: xdot ! time derivatives of continuous states -! type(LD_ContinuousStateType) :: k1 ! RK4 constant; see above -! type(LD_ContinuousStateType) :: k2 ! RK4 constant; see above -! type(LD_ContinuousStateType) :: k3 ! RK4 constant; see above -! type(LD_ContinuousStateType) :: k4 ! RK4 constant; see above -! type(LD_ContinuousStateType) :: x_tmp ! Holds temporary modification to x -! type(LD_InputType) :: u_interp ! interpolated value of inputs -! ! Initialize errStat -! errStat = ErrID_None -! errMsg = "" -! -! ! Initialize interim vars -! !bjj: the state type contains allocatable arrays, so we must first allocate space: -! call LD_CopyContState( x, k1, MESH_NEWCOPY, errStat, errMsg ) -! call LD_CopyContState( x, k2, MESH_NEWCOPY, errStat, errMsg ) -! call LD_CopyContState( x, k3, MESH_NEWCOPY, errStat, errMsg ) -! call LD_CopyContState( x, k4, MESH_NEWCOPY, errStat, errMsg ) -! call LD_CopyContState( x, x_tmp, MESH_NEWCOPY, errStat, errMsg ) -! -! ! interpolate u to find u_interp = u(t) -! call LD_CopyInput(u(1), u_interp, MESH_NEWCOPY, errStat, errMsg ) ! we need to allocate input arrays/meshes before calling ExtrapInterp... -! call LD_Input_ExtrapInterp( u, utimes, u_interp, t, errStat, errMsg ) -! -! ! find xdot at t -! call LD_CalcContStateDeriv( t, u_interp, p, x, xd, z, OtherState, m, xdot, errStat, errMsg ) !initializes xdot -! -! k1%qm = p%EP_DeltaT * xdot%qm -! k1%qmdot = p%EP_DeltaT * xdot%qmdot -! x_tmp%qm = x%qm + 0.5 * k1%qm -! x_tmp%qmdot = x%qmdot + 0.5 * k1%qmdot -! -! ! interpolate u to find u_interp = u(t + dt/2) -! call LD_Input_ExtrapInterp(u, utimes, u_interp, t+0.5*p%EP_DeltaT, errStat, errMsg) -! -! ! find xdot at t + dt/2 -! call LD_CalcContStateDeriv( t + 0.5*p%EP_DeltaT, u_interp, p, x_tmp, xd, z, OtherState, m, xdot, errStat, errMsg ) -! -! k2%qm = p%EP_DeltaT * xdot%qm -! k2%qmdot = p%EP_DeltaT * xdot%qmdot -! x_tmp%qm = x%qm + 0.5 * k2%qm -! x_tmp%qmdot = x%qmdot + 0.5 * k2%qmdot -! -! ! find xdot at t + dt/2 -! call LD_CalcContStateDeriv( t + 0.5*p%EP_DeltaT, u_interp, p, x_tmp, xd, z, OtherState, m, xdot, errStat, errMsg ) -! -! k3%qm = p%EP_DeltaT * xdot%qm -! k3%qmdot = p%EP_DeltaT * xdot%qmdot -! x_tmp%qm = x%qm + k3%qm -! x_tmp%qmdot = x%qmdot + k3%qmdot -! -! ! interpolate u to find u_interp = u(t + dt) -! call LD_Input_ExtrapInterp(u, utimes, u_interp, t + p%EP_DeltaT, errStat, errMsg) -! -! ! find xdot at t + dt -! call LD_CalcContStateDeriv( t + p%EP_DeltaT, u_interp, p, x_tmp, xd, z, OtherState, m, xdot, errStat, errMsg ) -! -! k4%qm = p%EP_DeltaT * xdot%qm -! k4%qmdot = p%EP_DeltaT * xdot%qmdot -! x%qm = x%qm + ( k1%qm + 2. * k2%qm + 2. * k3%qm + k4%qm ) / 6. -! x%qmdot = x%qmdot + ( k1%qmdot + 2. * k2%qmdot + 2. * k3%qmdot + k4%qmdot ) / 6. -! call ExitThisRoutine() -! CONTAinS -! !............................................................................................................................... -! subroutine ExitThisRoutine() -! ! This subroutine destroys all the local variables -! integer(IntKi) :: errStat3 ! The error identifier (errStat) -! character(1024) :: errMsg3 ! The error message (errMsg) -! call LD_DestroyContState( xdot, errStat3, errMsg3 ) -! call LD_DestroyContState( k1, errStat3, errMsg3 ) -! call LD_DestroyContState( k2, errStat3, errMsg3 ) -! call LD_DestroyContState( k3, errStat3, errMsg3 ) -! call LD_DestroyContState( k4, errStat3, errMsg3 ) -! call LD_DestroyContState( x_tmp, errStat3, errMsg3 ) -! call LD_DestroyInput( u_interp, errStat3, errMsg3 ) -! end subroutine ExitThisRoutine -! -! end subroutine LD_RK4 -! -! -! !---------------------------------------------------------------------------------------------------------------------------------- -! !> This is a loose coupling routine for solving constraint states, integrating continuous states, and updating discrete and other -! !! states. Continuous, constraint, discrete, and other states are updated to values at t + Interval. -! subroutine LD_UpdateStates( t, n, Inputs, InputTimes, p, x, xd, z, OtherState, m, errStat, errMsg ) -! !.................................................................................................................................. -! real(DbKi), intent(in ) :: t !< Current simulation time in seconds -! integer(IntKi), intent(in ) :: n !< Current step of the simulation: t = n*Interval -! type(LD_InputType), intent(inout) :: Inputs(:) !< Inputs at InputTimes (output from this routine only -! !! because of record keeping in routines that copy meshes) -! real(DbKi), intent(in ) :: InputTimes(:) !< Times in seconds associated with Inputs -! type(LD_ParameterType), intent(in ) :: p !< Parameters -! type(LD_ContinuousStateType), intent(inout) :: x !< Input: Continuous states at t; -! !! Output: Continuous states at t + Interval -! type(LD_DiscreteStateType), intent(inout) :: xd !< Input: Discrete states at t; -! !! Output: Discrete states at t + Interval -! type(LD_ConstraintStateType), intent(inout) :: z !< Input: Constraint states at t; -! !! Output: Constraint states at t + Interval -! type(LD_OtherStateType), intent(inout) :: OtherState !< Other states: Other states at t; -! !! Output: Other states at t + Interval -! type(LD_MiscVarType), intent(inout) :: m !< Misc variables for optimization (not copied in glue code) -! integer(IntKi), intent( out) :: errStat !< Error status of the operation -! character(*), intent( out) :: errMsg !< Error message if errStat /= ErrID_None -! ! Initialize variables -! errStat = ErrID_None ! no error has occurred -! errMsg = "" -! if ( p%nCB == 0) return ! no modes = no states -! if (p%IntMethod .eq. 1) then -! call LD_RK4( t, n, Inputs, InputTimes, p, x, xd, z, OtherState, m, errStat, errMsg ) -! elseif (p%IntMethod .eq. 2) then -! call LD_AB4( t, n, Inputs, InputTimes, p, x, xd, z, OtherState, m, errStat, errMsg ) -! elseif (p%IntMethod .eq. 3) then -! call LD_ABM4( t, n, Inputs, InputTimes, p, x, xd, z, OtherState, m, errStat, errMsg ) -! else -! call SeterrStat(ErrID_Fatal,'Invalid time integration method:'//Num2LStr(p%IntMethod),errStat,errMsg,'LD_UpdateState') -! end IF -! end subroutine LD_UpdateStates -! !---------------------------------------------------------------------------------------------------------------------------------- -! !> This is a routine for computing outputs, used in both loose and tight coupling. -! subroutine LD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, errStat, errMsg ) -! real(DbKi), intent(in ) :: t !< Current simulation time in seconds -! type(LD_InputType), intent(in ) :: u !< Inputs at t -! type(LD_ParameterType), intent(in ) :: p !< Parameters -! type(LD_ContinuousStateType), intent(in ) :: x !< Continuous states at t -! type(LD_DiscreteStateType), intent(in ) :: xd !< Discrete states at t -! type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t -! type(LD_OtherStateType), intent(in ) :: OtherState !< Other states at t -! type(LD_MiscVarType), intent(inout) :: m !< Misc variables for optimization (not copied in glue code) -! type(LD_OutputType), intent(inout) :: y !< Outputs computed at t (Input only so that mesh con- -! !! nectivity information does not have to be recalculated) -! integer(IntKi), intent( out) :: errStat !< Error status of the operation -! character(*), intent( out) :: errMsg !< Error message if errStat /= ErrID_None -! ! Local variables +!---------------------------------------------------------------------------------------------------------------------------------- +!> This routine is called at the end of the simulation. +subroutine LD_End( u, p, x, xd, z, OtherState, y, m, errStat, errMsg ) + type(LD_InputType), intent(inout) :: u !< System inputs + type(LD_ParameterType), intent(inout) :: p !< Parameters + type(LD_ContinuousStateType), intent(inout) :: x !< Continuous states + type(LD_DiscreteStateType), intent(inout) :: xd !< Discrete states + type(LD_ConstraintStateType), intent(inout) :: z !< Constraint states + type(LD_OtherStateType), intent(inout) :: OtherState !< Other states + type(LD_OutputType), intent(inout) :: y !< System outputs + type(LD_MiscVarType), intent(inout) :: m !< Misc variables for optimization (not copied in glue code) + integer(IntKi), intent(out) :: errStat !< Error status of the operation + character(*), intent(out) :: errMsg !< Error message if errStat /= ErrID_None + ! Initialize errStat + errStat = ErrID_None ! no error has occurred + errMsg = "" + call LD_DestroyInput (u ,errStat,errMsg) + call LD_DestroyParam (p ,errStat,errMsg) + call LD_DestroyContState (x ,errStat,errMsg) + call LD_DestroyDiscState (xd ,errStat,errMsg) + call LD_DestroyConstrState(z ,errStat,errMsg) + call LD_DestroyOtherState (OtherState,errStat,errMsg) + call LD_DestroyOutput (y ,errStat,errMsg) + call LD_DestroyMisc (m ,errStat,errMsg) +end subroutine LD_End +!---------------------------------------------------------------------------------------------------------------------------------- +!> Fourth-order Adams-Bashforth Method (RK4) for numerically integration (see ElastoDyn.f9) +subroutine LD_AB4( t, n, u, utimes, p, x, xd, z, OtherState, m, errStat, errMsg ) + real(DbKi), intent(in ) :: t !< Current simulation time in seconds + integer(IntKi), intent(in ) :: n !< time step number + type(LD_InputType), intent(inout) :: u(:) !< Inputs at t + real(DbKi), intent(in ) :: utimes(:) !< times of input + type(LD_ParameterType), intent(in ) :: p !< Parameters + type(LD_ContinuousStateType), intent(inout) :: x !< Continuous states at t on input at t + dt on output + type(LD_DiscreteStateType), intent(in ) :: xd !< Discrete states at t + type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t (possibly a guess) + type(LD_OtherStateType), intent(inout) :: OtherState !< Other states at t on input at t + dt on output + type(LD_MiscVarType), intent(inout) :: m !< Misc/optimization variables + integer(IntKi), intent(out) :: errStat !< Error status of the operation + character(*), intent(out) :: errMsg !< Error message if errStat /= ErrID_None + ! local variables + type(LD_ContinuousStateType) :: xdot ! Continuous state derivs at t + type(LD_InputType) :: u_interp + ! Initialize errStat + errStat = ErrID_None + errMsg = "" + + ! need xdot at t + call LD_CopyInput(u(1), u_interp, MESH_NEWCOPY, errStat, errMsg ) ! we need to allocate input arrays/meshes before calling ExtrapInterp... + call LD_Input_ExtrapInterp(u, utimes, u_interp, t, errStat, errMsg) + call LD_CalcContStateDeriv( t, u_interp, p, x, xd, z, OtherState, m, xdot, errStat, errMsg ) ! initializes xdot + call LD_DestroyInput( u_interp, errStat, errMsg) ! we don't need this local copy anymore + if (n .le. 2) then + OtherState%n = n + call LD_CopyContState(xdot, OtherState%xdot(3-n), MESH_UPDATECOPY, errStat, errMsg ) + call LD_RK4(t, n, u, utimes, p, x, xd, z, OtherState, m, errStat, errMsg ) + else + if (OtherState%n .lt. n) then + OtherState%n = n + call LD_CopyContState(OtherState%xdot(3), OtherState%xdot(4), MESH_UPDATECOPY, errStat, errMsg ) + call LD_CopyContState(OtherState%xdot(2), OtherState%xdot(3), MESH_UPDATECOPY, errStat, errMsg ) + call LD_CopyContState(OtherState%xdot(1), OtherState%xdot(2), MESH_UPDATECOPY, errStat, errMsg ) + elseif (OtherState%n .gt. n) then + errStat = ErrID_Fatal + errMsg = ' Backing up in time is not supported with a multistep method ' + return + endif + call LD_CopyContState( xdot, OtherState%xdot ( 1 ), MESH_UPDATECOPY, errStat, errMsg ) + !OtherState%xdot ( 1 ) = xdot ! make sure this is most up to date + x%q = x%q + (p%dt / 24._ReKi) * (55._ReKi*OtherState%xdot(1)%q - 59._ReKi*OtherState%xdot(2)%q + 37._ReKi*OtherState%xdot(3)%q - 9._ReKi * OtherState%xdot(4)%q) + endif + call LD_DestroyContState(xdot, errStat, errMsg) + call LD_DestroyInput(u_interp, errStat, errMsg) +end subroutine LD_AB4 +!---------------------------------------------------------------------------------------------------------------------------------- +!> Fourth-order Adams-Bashforth-Moulton Method (RK4) for numerically integrating (see ElastoDyn.f90) +subroutine LD_ABM4( t, n, u, utimes, p, x, xd, z, OtherState, m, errStat, errMsg ) + real(DbKi), intent(in ) :: t !< Current simulation time in seconds + integer(IntKi), intent(in ) :: n !< time step number + type(LD_InputType), intent(inout) :: u(:) !< Inputs at t + real(DbKi), intent(in ) :: utimes(:) !< times of input + type(LD_ParameterType), intent(in ) :: p !< Parameters + type(LD_ContinuousStateType), intent(inout) :: x !< Continuous states at t on input at t + dt on output ! TODO TODO TODO in + type(LD_DiscreteStateType), intent(in ) :: xd !< Discrete states at t + type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t (possibly a guess) + type(LD_OtherStateType), intent(inout) :: OtherState !< Other states at t on input at t + dt on output + type(LD_MiscVarType), intent(inout) :: m !< Misc/optimization variables + integer(IntKi), intent(out) :: errStat !< Error status of the operation + character(*), intent(out) :: errMsg !< Error message if errStat /= ErrID_None + ! local variables + type(LD_InputType) :: u_interp ! Continuous states at t + type(LD_ContinuousStateType) :: x_pred ! Continuous states at t + type(LD_ContinuousStateType) :: xdot_pred ! Continuous states at t + ! Initialize errStat + errStat = ErrID_None + errMsg = "" + call LD_CopyContState(x, x_pred, MESH_NEWCOPY, errStat, errMsg) !initialize x_pred + call LD_AB4( t, n, u, utimes, p, x_pred, xd, z, OtherState, m, errStat, errMsg ) + if (n .gt. 2) then + call LD_CopyInput( u(1), u_interp, MESH_NEWCOPY, errStat, errMsg) ! make copy so that arrays/meshes get initialized/allocated for ExtrapInterp + call LD_Input_ExtrapInterp(u, utimes, u_interp, t + p%dt, errStat, errMsg) + call LD_CalcContStateDeriv(t + p%dt, u_interp, p, x_pred, xd, z, OtherState, m, xdot_pred, errStat, errMsg ) ! initializes xdot_pred + call LD_DestroyInput( u_interp, errStat, errMsg) ! local copy no longer needed + + x%q = x%q + (p%dt / 24.) * ( 9. * xdot_pred%q + 19. * OtherState%xdot(1)%q - 5. * OtherState%xdot(2)%q + 1. * OtherState%xdot(3)%q ) + call LD_DestroyContState( xdot_pred, errStat, errMsg) ! local copy no longer needed + else + x%q = x_pred%q + endif + call LD_DestroyContState( x_pred, errStat, errMsg) ! local copy no longer needed +end subroutine LD_ABM4 +!---------------------------------------------------------------------------------------------------------------------------------- +!> Fourth-order Runge-Kutta Method (RK4) for numerically integration (see ElastoDyn.f90) +subroutine LD_RK4( t, n, u, utimes, p, x, xd, z, OtherState, m, errStat, errMsg ) + real(DbKi), intent(in ) :: t !< Current simulation time in seconds + integer(IntKi), intent(in ) :: n !< time step number + type(LD_InputType), intent(inout) :: u(:) !< Inputs at t + real(DbKi), intent(in ) :: utimes(:) !< times of input + type(LD_ParameterType), intent(in ) :: p !< Parameters + type(LD_ContinuousStateType), intent(inout) :: x !< Continuous states at t on input at t + dt on output + type(LD_DiscreteStateType), intent(in ) :: xd !< Discrete states at t + type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t (possibly a guess) + type(LD_OtherStateType), intent(inout) :: OtherState !< Other states at t on input at t + dt on output + type(LD_MiscVarType), intent(inout) :: m !< Misc/optimization variables + integer(IntKi), intent(out) :: errStat !< Error status of the operation + character(*), intent(out) :: errMsg !< Error message if errStat /= ErrID_None + ! local variables + type(LD_ContinuousStateType) :: xdot ! time derivatives of continuous states + type(LD_ContinuousStateType) :: k1 ! RK4 constant; see above + type(LD_ContinuousStateType) :: k2 ! RK4 constant; see above + type(LD_ContinuousStateType) :: k3 ! RK4 constant; see above + type(LD_ContinuousStateType) :: k4 ! RK4 constant; see above + type(LD_ContinuousStateType) :: x_tmp ! Holds temporary modification to x + type(LD_InputType) :: u_interp ! interpolated value of inputs + ! Initialize errStat + errStat = ErrID_None + errMsg = "" + + ! Initialize interim vars + call LD_CopyContState( x, k1, MESH_NEWCOPY, errStat, errMsg ) + call LD_CopyContState( x, k2, MESH_NEWCOPY, errStat, errMsg ) + call LD_CopyContState( x, k3, MESH_NEWCOPY, errStat, errMsg ) + call LD_CopyContState( x, k4, MESH_NEWCOPY, errStat, errMsg ) + call LD_CopyContState( x, x_tmp, MESH_NEWCOPY, errStat, errMsg ) + + ! interpolate u to find u_interp = u(t) + call LD_CopyInput(u(1), u_interp, MESH_NEWCOPY, errStat, errMsg ) ! we need to allocate input arrays/meshes before calling ExtrapInterp... + call LD_Input_ExtrapInterp( u, utimes, u_interp, t, errStat, errMsg ) + + ! find xdot at t + call LD_CalcContStateDeriv( t, u_interp, p, x, xd, z, OtherState, m, xdot, errStat, errMsg ) !initializes xdot + + k1%q = p%dt * xdot%q + x_tmp%q = x%q + 0.5_ReKi * k1%q + + ! interpolate u to find u_interp = u(t + dt/2) + call LD_Input_ExtrapInterp(u, utimes, u_interp, t+0.5_ReKi*p%dt, errStat, errMsg) + + ! find xdot at t + dt/2 + call LD_CalcContStateDeriv( t + 0.5_ReKi*p%dt, u_interp, p, x_tmp, xd, z, OtherState, m, xdot, errStat, errMsg ) + + k2%q = p%dt * xdot%q + x_tmp%q = x%q + 0.5_ReKi * k2%q + + ! find xdot at t + dt/2 + call LD_CalcContStateDeriv( t + 0.5_ReKi*p%dt, u_interp, p, x_tmp, xd, z, OtherState, m, xdot, errStat, errMsg ) + + k3%q = p%dt * xdot%q + x_tmp%q = x%q + k3%q + + ! interpolate u to find u_interp = u(t + dt) + call LD_Input_ExtrapInterp(u, utimes, u_interp, t + p%dt, errStat, errMsg) + + ! find xdot at t + dt + call LD_CalcContStateDeriv( t + p%dt, u_interp, p, x_tmp, xd, z, OtherState, m, xdot, errStat, errMsg ) + k4%q = p%dt * xdot%q + x%q = x%q + ( k1%q + 2._ReKi * k2%q + 2._ReKi * k3%q + k4%q ) / 6._ReKi + call CleanUp() +contains + subroutine CleanUp() + integer(IntKi) :: errStat3 ! The error identifier (errStat) + character(1024) :: errMsg3 ! The error message (errMsg) + call LD_DestroyContState( xdot, errStat3, errMsg3 ) + call LD_DestroyContState( k1, errStat3, errMsg3 ) + call LD_DestroyContState( k2, errStat3, errMsg3 ) + call LD_DestroyContState( k3, errStat3, errMsg3 ) + call LD_DestroyContState( k4, errStat3, errMsg3 ) + call LD_DestroyContState( x_tmp, errStat3, errMsg3 ) + call LD_DestroyInput( u_interp, errStat3, errMsg3 ) + end subroutine CleanUp +end subroutine LD_RK4 +!---------------------------------------------------------------------------------------------------------------------------------- +!> Loose coupling routine for solving states at t+dt +subroutine LD_UpdateStates( t, n, Inputs, InputTimes, p, x, xd, z, OtherState, m, errStat, errMsg ) + real(DbKi), intent(in ) :: t !< Current simulation time in seconds + integer(IntKi), intent(in ) :: n !< Current step of the simulation: t = n*dt + type(LD_InputType), intent(inout) :: Inputs(:) !< Inputs at InputTimes (output from this routine only + real(DbKi), intent(in ) :: InputTimes(:) !< Times in seconds associated with Inputs + type(LD_ParameterType), intent(in ) :: p !< Parameters + type(LD_ContinuousStateType), intent(inout) :: x !< Input: Continuous states at t; Output: at t+dt + type(LD_DiscreteStateType), intent(inout) :: xd !< Input: Discrete states at t; Output: at t+dt + type(LD_ConstraintStateType), intent(inout) :: z !< Input: Constraint states at t; Output: at t+dt + type(LD_OtherStateType), intent(inout) :: OtherState !< Other states: Other states at t;Output: at t+dt + type(LD_MiscVarType), intent(inout) :: m !< Misc variables for optimization (not copied in glue code) + integer(IntKi), intent(out) :: errStat !< Error status of the operation + character(*), intent(out) :: errMsg !< Error message if errStat /= ErrID_None + ! Initialize variables + errStat = ErrID_None ! no error has occurred + errMsg = "" + if ( p%nq == 0) return + if (p%IntMethod .eq. 1) then + call LD_RK4( t, n, Inputs, InputTimes, p, x, xd, z, OtherState, m, errStat, errMsg ) + elseif (p%IntMethod .eq. 2) then + call LD_AB4( t, n, Inputs, InputTimes, p, x, xd, z, OtherState, m, errStat, errMsg ) + elseif (p%IntMethod .eq. 3) then + call LD_ABM4( t, n, Inputs, InputTimes, p, x, xd, z, OtherState, m, errStat, errMsg ) + else + call SeterrStat(ErrID_Fatal,'Invalid time integration method:'//Num2LStr(p%IntMethod),errStat,errMsg,'LD_UpdateState') + end if +end subroutine LD_UpdateStates +!---------------------------------------------------------------------------------------------------------------------------------- +!> This is a routine for computing outputs, used in both loose and tight coupling. +subroutine LD_CalcOutput( t, u, p, x, xd, z, OtherState, y, m, errStat, errMsg ) + real(DbKi), intent(in ) :: t !< Current simulation time in seconds + type(LD_InputType), intent(in ) :: u !< Inputs at t + type(LD_ParameterType), intent(in ) :: p !< Parameters + type(LD_ContinuousStateType), intent(in ) :: x !< Continuous states at t + type(LD_DiscreteStateType), intent(in ) :: xd !< Discrete states at t + type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t + type(LD_OtherStateType), intent(in ) :: OtherState !< Other states at t + type(LD_MiscVarType), intent(inout) :: m !< Misc variables for optimization (not copied in glue code) + type(LD_OutputType), intent(inout) :: y !< Outputs computed at t (Input only so that mesh con- + integer(IntKi), intent(out) :: errStat !< Error status of the operation + character(*), intent(out) :: errMsg !< Error message if errStat /= ErrID_None + ! Local variables + type(LD_ContinuousStateType) :: dxdt !< + integer(IntKi) :: errStat2 ! Status of error message + character(1024) :: errMsg2 ! Error message if ErrStat /= ErrID_None ! integer(IntKi) :: I !< Generic counters ! real(ReKi), dimension(6) :: Fc !< Output coupling force -! ! Compute the loads `fr1 fr2` at t (fr1 without added mass) by time interpolation of the inputs loads p%Forces -! call InterpStpMat(real(t,ReKi), p%times, p%Forces, m%Indx, p%nTimeSteps, m%F_at_t) -! -! ! --- Flatening vectors and using linear state formulation y=Cx+Du+Fy -! ! u flat (x1, \dot{x1}, \ddot{x1}) -! m%uFlat(1:3) = u%PtfmMesh%TranslationDisp(:,1) -! m%uFlat(4:6) = GetSmllRotAngs(u%PtfmMesh%Orientation(:,:,1), errStat, errMsg); call SeterrStatSimple(errStat, errMsg, 'LD_CalcOutput') -! m%uFlat(7:9 ) = u%PtfmMesh%TranslationVel(:,1) -! m%uFlat(10:12) = u%PtfmMesh%RotationVel (:,1) -! m%uFlat(13:15) = u%PtfmMesh%TranslationAcc(:,1) -! m%uFlat(16:18) = u%PtfmMesh%RotationAcc (:,1) -! -! !--- Computing output: y = Cx + Du + Fy -! ! -! if (p%nCB>0) then -! ! x flat -! m%xFlat( 1:p%nCB ) = x%qm (1:p%nCB) -! m%xFlat(p%nCB+1:2*p%nCB) = x%qmdot(1:p%nCB) + ! Initialize variables + errStat = ErrID_None ! no error has occurred + errMsg = "" ! -! ! >>> MATMUL implementation -! !Fc = matmul(p%CMat, m%xFlat) + matmul(p%DMat, m%uFlat) + m%F_at_t(1:6) - matmul(p%M12, m%F_at_t(6+1:6+p%nCB)) -! -! ! >>> LAPACK implementation -! Fc(1:6) = m%F_at_t(1:6) ! Fc = F1r + ... -! ! GEMV(TRS, M , N , alpha , A , LDA, X ,inCX, Beta , Y, IncY) -! call LAPACK_GEMV('n', 6 , 2*p%nCB, 1.0_ReKi, p%CMat, 6 , m%xFlat , 1, 1.0_ReKi, Fc, 1 ) ! = C*x + (F1r) -! call LAPACK_GEMV('n', 6 , 18 , 1.0_ReKi, p%DMat, 6 , m%uFlat , 1, 1.0_ReKi, Fc, 1 ) ! + D*u -! call LAPACK_GEMV('n', 6 , p%nCB , -1.0_ReKi, p%M12 , 6 , m%F_at_t(6+1:6+p%nCB), 1, 1.0_ReKi, Fc, 1 ) ! - M12*F2r -! else -! Fc = matmul(p%DMat, m%uFlat) + m%F_at_t(1:6) -! endif + ! --- Compute accelerations + call LD_CalcContStateDeriv(t, u, p, x, xd, z, OtherState, m, dxdt, errStat2, errMsg2) + y%qd = dxdt%q + + + + !--- Computing output: y = Cx + Du + Fy ! ! ! Update the output mesh ! do i=1,3 @@ -707,123 +630,56 @@ end subroutine LD_Init ! y%WriteOutput(I) = -9.9999e20 ! endif ! enddo -! end subroutine LD_CalcOutput -! !---------------------------------------------------------------------------------------------------------------------------------- -! -! -! !++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -! !> This is a tight coupling routine for computing derivatives of continuous states. -! subroutine LD_CalcContStateDeriv( t, u, p, x, xd, z, OtherState, m, dxdt, errStat, errMsg ) -! !.................................................................................................................................. -! real(DbKi), intent(in ) :: t !< Current simulation time in seconds -! type(LD_InputType), intent(in ) :: u !< Inputs at t -! type(LD_ParameterType), intent(in ) :: p !< Parameters -! type(LD_ContinuousStateType), intent(in ) :: x !< Continuous states at t -! type(LD_DiscreteStateType), intent(in ) :: xd !< Discrete states at t -! type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t -! type(LD_OtherStateType), intent(in ) :: OtherState !< Other states at t -! type(LD_MiscVarType), intent(inout) :: m !< Misc variables for optimization (not copied in glue code) -! type(LD_ContinuousStateType), intent( out) :: dxdt !< Continuous state derivatives at t -! integer(IntKi), intent( out) :: errStat !< Error status of the operation -! character(*), intent( out) :: errMsg !< Error message if errStat /= ErrID_None -! ! Local variables +contains + logical function Failed() + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'LD_CalcOutput' ) + Failed = errStat >= AbortErrLev + end function Failed +end subroutine LD_CalcOutput +!---------------------------------------------------------------------------------------------------------------------------------- +!> Tight coupling routine for computing derivatives of continuous states. +subroutine LD_CalcContStateDeriv( t, u, p, x, xd, z, OtherState, m, dxdt, errStat, errMsg ) + real(DbKi), intent(in ) :: t !< Current simulation time in seconds + type(LD_InputType), intent(in ) :: u !< Inputs at t + type(LD_ParameterType), intent(in ) :: p !< Parameters + type(LD_ContinuousStateType), intent(in ) :: x !< Continuous states at t + type(LD_DiscreteStateType), intent(in ) :: xd !< Discrete states at t + type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t + type(LD_OtherStateType), intent(in ) :: OtherState !< Other states at t + type(LD_MiscVarType), intent(inout) :: m !< Misc variables for optimization (not copied in glue code) + type(LD_ContinuousStateType), intent(out) :: dxdt !< Continuous state derivatives at t + integer(IntKi), intent(out) :: errStat !< Error status of the operation + character(*), intent(out) :: errMsg !< Error message if errStat /= ErrID_None + ! Local variables + integer(IntKi) :: errStat2 ! Status of error message + character(1024) :: errMsg2 ! Error message if ErrStat /= ErrID_None ! integer(IntKi) :: I -! ! Allocation of output dxdt (since intent(out)) -! call AllocAry(dxdt%qm, p%nCB, 'dxdt%qm', errStat, errMsg); if(Failed()) return -! call AllocAry(dxdt%qmdot, p%nCB, 'dxdt%qmdot', errStat, errMsg); if(Failed()) return -! if ( p%nCB == 0 ) return -! do I=1,p%nCB; dxdt%qm (I)=0; enddo -! do I=1,p%nCB; dxdt%qmdot(I)=0; enddo -! -! ! Compute the loads `fr1 fr2` at t (fr1 without added mass) by time interpolation of the inputs loads p%F -! call InterpStpMat(real(t,ReKi), p%times, p%Forces, m%Indx, p%nTimeSteps, m%F_at_t) -! -! ! u flat (x1, \dot{x1}, \ddot{x1}) -! m%uFlat(1:3) = u%PtfmMesh%TranslationDisp(:,1) -! m%uFlat(4:6) = GetSmllRotAngs(u%PtfmMesh%Orientation(:,:,1), errStat, errMsg); if(Failed()) return -! m%uFlat(7:9 ) = u%PtfmMesh%TranslationVel(:,1) -! m%uFlat(10:12) = u%PtfmMesh%RotationVel (:,1) -! m%uFlat(13:15) = u%PtfmMesh%TranslationAcc(:,1) -! m%uFlat(16:18) = u%PtfmMesh%RotationAcc (:,1) -! -! ! --- Computation of qm and qmdot -! ! >>> Latex formulae: -! ! \ddot{x2} = -K22 x2 - C22 \dot{x2} - C21 \dot{x1} - M21 \ddot{x1} + fr2 -! ! >>> MATMUL IMPLEMENTATION -! !dxdt%qm= x%qmdot -! !dxdt%qmdot = - matmul(p%K22,x%qm) - matmul(p%C22,x%qmdot) & -! ! - matmul(p%C21,m%uFlat(7:12)) - matmul(p%M21, m%uFlat(13:18)) + m%F_at_t(6+1:6+p%nCB) -! ! >>> BLAS IMPLEMENTATION -! ! COPY( N , X , inCX, Y , inCY) -! call LAPACK_COPY(p%nCB, x%qmdot , 1 , dxdt%qm , 1 ) ! qmdot=qmdot -! call LAPACK_COPY(p%nCB, m%F_at_t(6+1:6+p%nCB), 1 , dxdt%qmdot , 1 ) ! qmddot = fr2 -! ! GEMV(TRS, M , N , alpha , A , LDA , X ,inCX, Beta , Y , IncY) -! call LAPACK_GEMV('n', p%nCB, p%nCB , -1.0_ReKi, p%K22, p%nCB, x%qm , 1 , 1.0_ReKi, dxdt%qmdot, 1 ) ! - K22 x2 -! call LAPACK_GEMV('n', p%nCB, 6 , -1.0_ReKi, p%C21, p%nCB, m%uFlat(7:12) , 1 , 1.0_ReKi, dxdt%qmdot, 1 ) ! - C21 \dot{x1} -! call LAPACK_GEMV('n', p%nCB, p%nCB , -1.0_ReKi, p%C22, p%nCB, x%qmdot , 1 , 1.0_ReKi, dxdt%qmdot, 1 ) ! - C22 \dot{x2} -! call LAPACK_GEMV('n', p%nCB, 6 , -1.0_ReKi, p%M21, p%nCB, m%uFlat(13:18), 1 , 1.0_ReKi, dxdt%qmdot, 1 ) ! - M21 \ddot{x1} -! -! CONTAinS -! logical function Failed() -! call SeterrStatSimple(errStat, errMsg, 'LD_CalcContStateDeriv') -! Failed = errStat >= AbortErrLev -! end function Failed -! end subroutine LD_CalcContStateDeriv -! !---------------------------------------------------------------------------------------------------------------------------------- -! !> This is a tight coupling routine for updating discrete states. -! subroutine LD_UpdateDiscState( t, n, u, p, x, xd, z, OtherState, m, errStat, errMsg ) -! !.................................................................................................................................. -! real(DbKi), intent(in ) :: t !< Current simulation time in seconds -! integer(IntKi), intent(in ) :: n !< Current step of the simulation: t = n*Interval -! type(LD_InputType), intent(in ) :: u !< Inputs at t -! type(LD_ParameterType), intent(in ) :: p !< Parameters -! type(LD_ContinuousStateType), intent(in ) :: x !< Continuous states at t -! type(LD_DiscreteStateType), intent(inout) :: xd !< Input: Discrete states at t, Output: Discrete states at t + Interval -! type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t -! type(LD_OtherStateType), intent(in ) :: OtherState !< Other states at t -! type(LD_MiscVarType), intent(inout) :: m !< Misc variables for optimization (not copied in glue code) -! integer(IntKi), intent( out) :: errStat !< Error status of the operation -! character(*), intent( out) :: errMsg !< Error message if errStat /= ErrID_None -! ! Initialize errStat -! errStat = ErrID_None -! errMsg = "" -! ! Update discrete states here: -! xd%DummyDiscState = 0.0_Reki -! end subroutine LD_UpdateDiscState -! !---------------------------------------------------------------------------------------------------------------------------------- -! !> This is a tight coupling routine for solving for the residual of the constraint state functions. -! subroutine LD_CalcConstrStateResidual( t, u, p, x, xd, z, OtherState, m, Z_residual, errStat, errMsg ) -! !.................................................................................................................................. -! real(DbKi), intent(in ) :: t !< Current simulation time in seconds -! type(LD_InputType), intent(in ) :: u !< Inputs at t -! type(LD_ParameterType), intent(in ) :: p !< Parameters -! type(LD_ContinuousStateType), intent(in ) :: x !< Continuous states at t -! type(LD_DiscreteStateType), intent(in ) :: xd !< Discrete states at t -! type(LD_ConstraintStateType), intent(in ) :: z !< Constraint states at t (possibly a guess) -! type(LD_OtherStateType), intent(in ) :: OtherState !< Other states at t -! type(LD_MiscVarType), intent(inout) :: m !< Misc variables for optimization (not copied in glue code) -! type(LD_ConstraintStateType), intent( out) :: Z_residual !< Residual of the constraint state functions using -! !! the input values described above -! integer(IntKi), intent( out) :: errStat !< Error status of the operation -! character(*), intent( out) :: errMsg !< Error message if errStat /= ErrID_None -! ! Initialize errStat -! errStat = ErrID_None -! errMsg = "" -! ! Solve for the residual of the constraint state functions here: -! Z_residual%DummyConstrState = 0.0_ReKi -! -! end subroutine LD_CalcConstrStateResidual -! !---------------------------------------------------------------------------------------------------------------------------------- -! -! !++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -! ! ###### The following four routines are Jacobian routines for linearization capabilities ####### -! ! If the module does not implement them, set errStat = ErrID_Fatal in LD_Init() when InitInp%Linearize is .true. -! !---------------------------------------------------------------------------------------------------------------------------------- -! !> Routine to compute the Jacobians of the output (Y), continuous- (X), discrete- (Xd), and constraint-state (Z) functions -! !! with respect to the inputs (u). The partial derivatives dY/du, dX/du, dXd/du, and DZ/du are returned. -! + ! Initialize variables + errStat = ErrID_None ! no error has occurred + errMsg = "" + ! Allocation of output dxdt (since intent(out)) + call AllocAry(dxdt%q, p%nq, 'dxdt%q', errStat2, errMsg2); if(Failed()) return + if ( p%nq == 0 ) return + + ! --- Computation of dq + ! >>> MATMUL IMPLEMENTATION + dxdt%q = matmul(p%AA,x%q) + matmul(p%BB,u%Fext) + ! >>> BLAS IMPLEMENTATION + ! COPY( N , X , inCX, Y , inCY) + !call LAPACK_COPY(p%nCB, x%qmdot , 1 , dxdt%qm , 1 ) ! qmdot=qmdot + !! GEMV(TRS, M , N , alpha , A , LDA , X ,inCX, Beta , Y , IncY) + !call LAPACK_GEMV('n', p%nq, p%nq , 1.0_ReKi, p%AA, p%nq, x%q , 1 , 1.0_ReKi, dxdt%qmdot, 1 ) ! - K22 x2 + !call LAPACK_GEMV('n', p%nq, p%nx , 1.0_ReKi, p%BB, p%nq, u%Fext, 1 , 1.0_ReKi, dxdt%qmdot, 1 ) ! - M21 \ddot{x1} +! +contains + logical function Failed() + call SetErrStat( ErrStat2, ErrMsg2, ErrStat, ErrMsg, 'LD_CalcContStateDeriv' ) + Failed = errStat >= AbortErrLev + end function Failed +end subroutine LD_CalcContStateDeriv +!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +!> ! subroutine LD_JacobianPInput( t, u, p, x, xd, z, OtherState, y, m, errStat, errMsg, dYdu, dXdu, dXddu, dZdu) -! !.................................................................................................................................. ! real(DbKi), intent(in ) :: t !< Time in seconds at operating point ! type(LD_InputType), intent(in ) :: u !< Inputs at operating point (may change to inout if a mesh copy is required) ! type(LD_ParameterType), intent(in ) :: p !< Parameters diff --git a/modules/lindyn/src/LinDyn_Registry.txt b/modules/lindyn/src/LinDyn_Registry.txt index 4079acbf0f..37bdcc96ca 100644 --- a/modules/lindyn/src/LinDyn_Registry.txt +++ b/modules/lindyn/src/LinDyn_Registry.txt @@ -14,7 +14,7 @@ typedef ^ ^ ReKi typedef ^ ^ ReKi KK {:}{:} - - "Stiffness matrix" - typedef ^ ^ ReKi x0 {:} 0 - "Degrees of freedom initial conditions" - typedef ^ ^ ReKi xd0 {:} 0 - "Velocities initial conditions" - -typedef ^ ^ IntKi activeDOFs {:} .true. - "Degrees of freedom that are active" - +typedef ^ ^ logical activeDOFs {:} .true. - "Degrees of freedom that are active" - typedef ^ ^ character(8) prefix - "" - "Prefix for degrees of freedom" - # Initialization outputs @@ -33,27 +33,36 @@ typedef ^ DiscreteStateType Logical typedef ^ ConstraintStateType Logical Dummy - - - "" - # Other states: -typedef ^ OtherStateType Logical Dummy - - - "" - +typedef ^ OtherStateType LD_ContinuousStateType xdot {:} - - "Previous state derivs for m-step time integrator" +typedef ^ ^ IntKi n - - - "Tracks time step for which OtherState was updated last" # ..... Misc/Optimization variables................................................................................................. typedef ^ MiscVarType Logical Dummy - - - "" - +typedef ^ ^ ReKi AllOuts {:} - - "An array holding the value of all of the calculated (not only selected) output channels" "see OutListParameters.xlsx spreadsheet" # ..... Parameters ................................................................................................................ typedef ^ ParameterType DbKi dt - - - "time step" s typedef ^ ^ IntKi IntMethod - - - "Identifier for integration method (1 [RK4], 2 [AB4], or 3 [ABM4])" - +typedef ^ ^ IntKi nx - - - "Number of degrees of freedom (size of M)" - +typedef ^ ^ IntKi nq - - - "nq=2*nx" - typedef ^ ^ ReKi MM {:}{:} - - "Mass Matrix" - typedef ^ ^ ReKi CC {:}{:} - - "Damping Matrix" - typedef ^ ^ ReKi KK {:}{:} - - "Stiffness Matrix" - typedef ^ ^ ReKi Minv {:}{:} - - "Inverse of Mass matrix" - -typedef ^ ^ IntKi activeDOFs {:} - - "Degrees of freedom that are active" - -typedef ^ ^ IntKi AA {:}{:} - - "State matrix" - +typedef ^ ^ Logical activeDOFs {:} - - "Degrees of freedom that are active" - +typedef ^ ^ ReKi AA {:}{:} - - "State matrix A" - +typedef ^ ^ ReKi BB {:}{:} - - "State matrix B" - +typedef ^ ^ IntKi NumOuts - - - "Number of values in WriteOutput" - +typedef ^ ^ OutParmType OutParam {:} - - "Names and units (and other characteristics) of all requested output parameters" - +typedef ^ ^ IntKi OutParamLinIndx {:}{:} - - "Index into WriteOutput for linearization analysis" - + + # ..... Inputs .................................................................................................................... typedef ^ InputType ReKi Fext : - - "External loads" # ..... Outputs ................................................................................................................... typedef ^ OutputType ReKi qd {:} - "Time derivative of continuous states" - -typedef ^ OutputType ReKi qdd {:} - "Time derivative of continuous states" - typedef ^ ^ ReKi WriteOutput {:} - - "outputs to be written to a file" - diff --git a/modules/lindyn/src/LinDyn_Types.f90 b/modules/lindyn/src/LinDyn_Types.f90 index 7e366f37fb..c3cb1cbdd7 100644 --- a/modules/lindyn/src/LinDyn_Types.f90 +++ b/modules/lindyn/src/LinDyn_Types.f90 @@ -42,7 +42,7 @@ MODULE LinDyn_Types REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: KK !< Stiffness matrix [-] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: x0 !< Degrees of freedom initial conditions [-] REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: xd0 !< Velocities initial conditions [-] - INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: activeDOFs !< Degrees of freedom that are active [-] + LOGICAL , DIMENSION(:), ALLOCATABLE :: activeDOFs !< Degrees of freedom that are active [-] character(8) :: prefix !< Prefix for degrees of freedom [-] END TYPE LD_InitInputType ! ======================= @@ -70,24 +70,32 @@ MODULE LinDyn_Types ! ======================= ! ========= LD_OtherStateType ======= TYPE, PUBLIC :: LD_OtherStateType - LOGICAL :: Dummy !< [-] + TYPE(LD_ContinuousStateType) , DIMENSION(:), ALLOCATABLE :: xdot !< Previous state derivs for m-step time integrator [-] + INTEGER(IntKi) :: n !< Tracks time step for which OtherState was updated last [-] END TYPE LD_OtherStateType ! ======================= ! ========= LD_MiscVarType ======= TYPE, PUBLIC :: LD_MiscVarType LOGICAL :: Dummy !< [-] + REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: AllOuts !< An array holding the value of all of the calculated (not only selected) output channels [see OutListParameters.xlsx spreadsheet] END TYPE LD_MiscVarType ! ======================= ! ========= LD_ParameterType ======= TYPE, PUBLIC :: LD_ParameterType REAL(DbKi) :: dt !< time step [s] INTEGER(IntKi) :: IntMethod !< Identifier for integration method (1 [RK4], 2 [AB4], or 3 [ABM4]) [-] + INTEGER(IntKi) :: nx !< Number of degrees of freedom (size of M) [-] + INTEGER(IntKi) :: nq !< nq=2*nx [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: MM !< Mass Matrix [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: CC !< Damping Matrix [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: KK !< Stiffness Matrix [-] REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: Minv !< Inverse of Mass matrix [-] - INTEGER(IntKi) , DIMENSION(:), ALLOCATABLE :: activeDOFs !< Degrees of freedom that are active [-] - INTEGER(IntKi) , DIMENSION(:,:), ALLOCATABLE :: AA !< State matrix [-] + LOGICAL , DIMENSION(:), ALLOCATABLE :: activeDOFs !< Degrees of freedom that are active [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: AA !< State matrix A [-] + REAL(ReKi) , DIMENSION(:,:), ALLOCATABLE :: BB !< State matrix B [-] + INTEGER(IntKi) :: NumOuts !< Number of values in WriteOutput [-] + TYPE(OutParmType) , DIMENSION(:), ALLOCATABLE :: OutParam !< Names and units (and other characteristics) of all requested output parameters [-] + INTEGER(IntKi) , DIMENSION(:,:), ALLOCATABLE :: OutParamLinIndx !< Index into WriteOutput for linearization analysis [-] END TYPE LD_ParameterType ! ======================= ! ========= LD_InputType ======= @@ -98,7 +106,6 @@ MODULE LinDyn_Types ! ========= LD_OutputType ======= TYPE, PUBLIC :: LD_OutputType REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: qd - REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: qdd REAL(ReKi) , DIMENSION(:), ALLOCATABLE :: WriteOutput !< outputs to be written to a file [-] END TYPE LD_OutputType ! ======================= @@ -443,7 +450,7 @@ SUBROUTINE LD_PackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg Int_Xferred = Int_Xferred + 2 DO i1 = LBOUND(InData%activeDOFs,1), UBOUND(InData%activeDOFs,1) - IntKiBuf(Int_Xferred) = InData%activeDOFs(i1) + IntKiBuf(Int_Xferred) = TRANSFER(InData%activeDOFs(i1), IntKiBuf(1)) Int_Xferred = Int_Xferred + 1 END DO END IF @@ -604,7 +611,7 @@ SUBROUTINE LD_UnPackInitInput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Err RETURN END IF DO i1 = LBOUND(OutData%activeDOFs,1), UBOUND(OutData%activeDOFs,1) - OutData%activeDOFs(i1) = IntKiBuf(Int_Xferred) + OutData%activeDOFs(i1) = TRANSFER(IntKiBuf(Int_Xferred), OutData%activeDOFs(i1)) Int_Xferred = Int_Xferred + 1 END DO END IF @@ -1420,13 +1427,30 @@ SUBROUTINE LD_CopyOtherState( SrcOtherStateData, DstOtherStateData, CtrlCode, Er CHARACTER(*), INTENT( OUT) :: ErrMsg ! Local INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'LD_CopyOtherState' ! ErrStat = ErrID_None ErrMsg = "" - DstOtherStateData%Dummy = SrcOtherStateData%Dummy +IF (ALLOCATED(SrcOtherStateData%xdot)) THEN + i1_l = LBOUND(SrcOtherStateData%xdot,1) + i1_u = UBOUND(SrcOtherStateData%xdot,1) + IF (.NOT. ALLOCATED(DstOtherStateData%xdot)) THEN + ALLOCATE(DstOtherStateData%xdot(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstOtherStateData%xdot.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcOtherStateData%xdot,1), UBOUND(SrcOtherStateData%xdot,1) + CALL LD_CopyContState( SrcOtherStateData%xdot(i1), DstOtherStateData%xdot(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF + DstOtherStateData%n = SrcOtherStateData%n END SUBROUTINE LD_CopyOtherState SUBROUTINE LD_DestroyOtherState( OtherStateData, ErrStat, ErrMsg, DEALLOCATEpointers ) @@ -1450,6 +1474,13 @@ SUBROUTINE LD_DestroyOtherState( OtherStateData, ErrStat, ErrMsg, DEALLOCATEpoin DEALLOCATEpointers_local = .true. END IF +IF (ALLOCATED(OtherStateData%xdot)) THEN +DO i1 = LBOUND(OtherStateData%xdot,1), UBOUND(OtherStateData%xdot,1) + CALL LD_DestroyContState( OtherStateData%xdot(i1), ErrStat2, ErrMsg2, DEALLOCATEpointers_local ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) +ENDDO + DEALLOCATE(OtherStateData%xdot) +ENDIF END SUBROUTINE LD_DestroyOtherState SUBROUTINE LD_PackOtherState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) @@ -1487,7 +1518,31 @@ SUBROUTINE LD_PackOtherState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMs Re_BufSz = 0 Db_BufSz = 0 Int_BufSz = 0 - Int_BufSz = Int_BufSz + 1 ! Dummy + Int_BufSz = Int_BufSz + 1 ! xdot allocated yes/no + IF ( ALLOCATED(InData%xdot) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! xdot upper/lower bounds for each dimension + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i1 = LBOUND(InData%xdot,1), UBOUND(InData%xdot,1) + Int_BufSz = Int_BufSz + 3 ! xdot: size of buffers for each call to pack subtype + CALL LD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xdot(i1), ErrStat2, ErrMsg2, .TRUE. ) ! xdot + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! xdot + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! xdot + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! xdot + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! n IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -1515,7 +1570,48 @@ SUBROUTINE LD_PackOtherState( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMs Db_Xferred = 1 Int_Xferred = 1 - IntKiBuf(Int_Xferred) = TRANSFER(InData%Dummy, IntKiBuf(1)) + IF ( .NOT. ALLOCATED(InData%xdot) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%xdot,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%xdot,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%xdot,1), UBOUND(InData%xdot,1) + CALL LD_PackContState( Re_Buf, Db_Buf, Int_Buf, InData%xdot(i1), ErrStat2, ErrMsg2, OnlySize ) ! xdot + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + IntKiBuf(Int_Xferred) = InData%n Int_Xferred = Int_Xferred + 1 END SUBROUTINE LD_PackOtherState @@ -1532,6 +1628,7 @@ SUBROUTINE LD_UnPackOtherState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Er INTEGER(IntKi) :: Db_Xferred INTEGER(IntKi) :: Int_Xferred INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'LD_UnPackOtherState' @@ -1545,7 +1642,63 @@ SUBROUTINE LD_UnPackOtherState( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, Er Re_Xferred = 1 Db_Xferred = 1 Int_Xferred = 1 - OutData%Dummy = TRANSFER(IntKiBuf(Int_Xferred), OutData%Dummy) + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! xdot not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%xdot)) DEALLOCATE(OutData%xdot) + ALLOCATE(OutData%xdot(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%xdot.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%xdot,1), UBOUND(OutData%xdot,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL LD_UnpackContState( Re_Buf, Db_Buf, Int_Buf, OutData%xdot(i1), ErrStat2, ErrMsg2 ) ! xdot + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + OutData%n = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 END SUBROUTINE LD_UnPackOtherState @@ -1557,6 +1710,7 @@ SUBROUTINE LD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) CHARACTER(*), INTENT( OUT) :: ErrMsg ! Local INTEGER(IntKi) :: i,j,k + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'LD_CopyMisc' @@ -1564,6 +1718,18 @@ SUBROUTINE LD_CopyMisc( SrcMiscData, DstMiscData, CtrlCode, ErrStat, ErrMsg ) ErrStat = ErrID_None ErrMsg = "" DstMiscData%Dummy = SrcMiscData%Dummy +IF (ALLOCATED(SrcMiscData%AllOuts)) THEN + i1_l = LBOUND(SrcMiscData%AllOuts,1) + i1_u = UBOUND(SrcMiscData%AllOuts,1) + IF (.NOT. ALLOCATED(DstMiscData%AllOuts)) THEN + ALLOCATE(DstMiscData%AllOuts(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstMiscData%AllOuts.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstMiscData%AllOuts = SrcMiscData%AllOuts +ENDIF END SUBROUTINE LD_CopyMisc SUBROUTINE LD_DestroyMisc( MiscData, ErrStat, ErrMsg, DEALLOCATEpointers ) @@ -1587,6 +1753,9 @@ SUBROUTINE LD_DestroyMisc( MiscData, ErrStat, ErrMsg, DEALLOCATEpointers ) DEALLOCATEpointers_local = .true. END IF +IF (ALLOCATED(MiscData%AllOuts)) THEN + DEALLOCATE(MiscData%AllOuts) +ENDIF END SUBROUTINE LD_DestroyMisc SUBROUTINE LD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, SizeOnly ) @@ -1625,6 +1794,11 @@ SUBROUTINE LD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz Db_BufSz = 0 Int_BufSz = 0 Int_BufSz = Int_BufSz + 1 ! Dummy + Int_BufSz = Int_BufSz + 1 ! AllOuts allocated yes/no + IF ( ALLOCATED(InData%AllOuts) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! AllOuts upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%AllOuts) ! AllOuts + END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) IF (ErrStat2 /= 0) THEN @@ -1654,6 +1828,21 @@ SUBROUTINE LD_PackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Siz IntKiBuf(Int_Xferred) = TRANSFER(InData%Dummy, IntKiBuf(1)) Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%AllOuts) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%AllOuts,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%AllOuts,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%AllOuts,1), UBOUND(InData%AllOuts,1) + ReKiBuf(Re_Xferred) = InData%AllOuts(i1) + Re_Xferred = Re_Xferred + 1 + END DO + END IF END SUBROUTINE LD_PackMisc SUBROUTINE LD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) @@ -1669,6 +1858,7 @@ SUBROUTINE LD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) INTEGER(IntKi) :: Db_Xferred INTEGER(IntKi) :: Int_Xferred INTEGER(IntKi) :: i + INTEGER(IntKi) :: i1, i1_l, i1_u ! bounds (upper/lower) for an array dimension 1 INTEGER(IntKi) :: ErrStat2 CHARACTER(ErrMsgLen) :: ErrMsg2 CHARACTER(*), PARAMETER :: RoutineName = 'LD_UnPackMisc' @@ -1684,6 +1874,24 @@ SUBROUTINE LD_UnPackMisc( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg ) Int_Xferred = 1 OutData%Dummy = TRANSFER(IntKiBuf(Int_Xferred), OutData%Dummy) Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! AllOuts not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%AllOuts)) DEALLOCATE(OutData%AllOuts) + ALLOCATE(OutData%AllOuts(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%AllOuts.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%AllOuts,1), UBOUND(OutData%AllOuts,1) + OutData%AllOuts(i1) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END IF END SUBROUTINE LD_UnPackMisc SUBROUTINE LD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) @@ -1704,6 +1912,8 @@ SUBROUTINE LD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) ErrMsg = "" DstParamData%dt = SrcParamData%dt DstParamData%IntMethod = SrcParamData%IntMethod + DstParamData%nx = SrcParamData%nx + DstParamData%nq = SrcParamData%nq IF (ALLOCATED(SrcParamData%MM)) THEN i1_l = LBOUND(SrcParamData%MM,1) i1_u = UBOUND(SrcParamData%MM,1) @@ -1785,6 +1995,51 @@ SUBROUTINE LD_CopyParam( SrcParamData, DstParamData, CtrlCode, ErrStat, ErrMsg ) END IF END IF DstParamData%AA = SrcParamData%AA +ENDIF +IF (ALLOCATED(SrcParamData%BB)) THEN + i1_l = LBOUND(SrcParamData%BB,1) + i1_u = UBOUND(SrcParamData%BB,1) + i2_l = LBOUND(SrcParamData%BB,2) + i2_u = UBOUND(SrcParamData%BB,2) + IF (.NOT. ALLOCATED(DstParamData%BB)) THEN + ALLOCATE(DstParamData%BB(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%BB.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%BB = SrcParamData%BB +ENDIF + DstParamData%NumOuts = SrcParamData%NumOuts +IF (ALLOCATED(SrcParamData%OutParam)) THEN + i1_l = LBOUND(SrcParamData%OutParam,1) + i1_u = UBOUND(SrcParamData%OutParam,1) + IF (.NOT. ALLOCATED(DstParamData%OutParam)) THEN + ALLOCATE(DstParamData%OutParam(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%OutParam.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DO i1 = LBOUND(SrcParamData%OutParam,1), UBOUND(SrcParamData%OutParam,1) + CALL NWTC_Library_Copyoutparmtype( SrcParamData%OutParam(i1), DstParamData%OutParam(i1), CtrlCode, ErrStat2, ErrMsg2 ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg,RoutineName) + IF (ErrStat>=AbortErrLev) RETURN + ENDDO +ENDIF +IF (ALLOCATED(SrcParamData%OutParamLinIndx)) THEN + i1_l = LBOUND(SrcParamData%OutParamLinIndx,1) + i1_u = UBOUND(SrcParamData%OutParamLinIndx,1) + i2_l = LBOUND(SrcParamData%OutParamLinIndx,2) + i2_u = UBOUND(SrcParamData%OutParamLinIndx,2) + IF (.NOT. ALLOCATED(DstParamData%OutParamLinIndx)) THEN + ALLOCATE(DstParamData%OutParamLinIndx(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating DstParamData%OutParamLinIndx.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + END IF + DstParamData%OutParamLinIndx = SrcParamData%OutParamLinIndx ENDIF END SUBROUTINE LD_CopyParam @@ -1826,6 +2081,19 @@ SUBROUTINE LD_DestroyParam( ParamData, ErrStat, ErrMsg, DEALLOCATEpointers ) ENDIF IF (ALLOCATED(ParamData%AA)) THEN DEALLOCATE(ParamData%AA) +ENDIF +IF (ALLOCATED(ParamData%BB)) THEN + DEALLOCATE(ParamData%BB) +ENDIF +IF (ALLOCATED(ParamData%OutParam)) THEN +DO i1 = LBOUND(ParamData%OutParam,1), UBOUND(ParamData%OutParam,1) + CALL NWTC_Library_Destroyoutparmtype( ParamData%OutParam(i1), ErrStat2, ErrMsg2, DEALLOCATEpointers_local ) + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) +ENDDO + DEALLOCATE(ParamData%OutParam) +ENDIF +IF (ALLOCATED(ParamData%OutParamLinIndx)) THEN + DEALLOCATE(ParamData%OutParamLinIndx) ENDIF END SUBROUTINE LD_DestroyParam @@ -1866,6 +2134,8 @@ SUBROUTINE LD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_BufSz = 0 Db_BufSz = Db_BufSz + 1 ! dt Int_BufSz = Int_BufSz + 1 ! IntMethod + Int_BufSz = Int_BufSz + 1 ! nx + Int_BufSz = Int_BufSz + 1 ! nq Int_BufSz = Int_BufSz + 1 ! MM allocated yes/no IF ( ALLOCATED(InData%MM) ) THEN Int_BufSz = Int_BufSz + 2*2 ! MM upper/lower bounds for each dimension @@ -1894,7 +2164,42 @@ SUBROUTINE LD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_BufSz = Int_BufSz + 1 ! AA allocated yes/no IF ( ALLOCATED(InData%AA) ) THEN Int_BufSz = Int_BufSz + 2*2 ! AA upper/lower bounds for each dimension - Int_BufSz = Int_BufSz + SIZE(InData%AA) ! AA + Re_BufSz = Re_BufSz + SIZE(InData%AA) ! AA + END IF + Int_BufSz = Int_BufSz + 1 ! BB allocated yes/no + IF ( ALLOCATED(InData%BB) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! BB upper/lower bounds for each dimension + Re_BufSz = Re_BufSz + SIZE(InData%BB) ! BB + END IF + Int_BufSz = Int_BufSz + 1 ! NumOuts + Int_BufSz = Int_BufSz + 1 ! OutParam allocated yes/no + IF ( ALLOCATED(InData%OutParam) ) THEN + Int_BufSz = Int_BufSz + 2*1 ! OutParam upper/lower bounds for each dimension + ! Allocate buffers for subtypes, if any (we'll get sizes from these) + DO i1 = LBOUND(InData%OutParam,1), UBOUND(InData%OutParam,1) + Int_BufSz = Int_BufSz + 3 ! OutParam: size of buffers for each call to pack subtype + CALL NWTC_Library_Packoutparmtype( Re_Buf, Db_Buf, Int_Buf, InData%OutParam(i1), ErrStat2, ErrMsg2, .TRUE. ) ! OutParam + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN ! OutParam + Re_BufSz = Re_BufSz + SIZE( Re_Buf ) + DEALLOCATE(Re_Buf) + END IF + IF(ALLOCATED(Db_Buf)) THEN ! OutParam + Db_BufSz = Db_BufSz + SIZE( Db_Buf ) + DEALLOCATE(Db_Buf) + END IF + IF(ALLOCATED(Int_Buf)) THEN ! OutParam + Int_BufSz = Int_BufSz + SIZE( Int_Buf ) + DEALLOCATE(Int_Buf) + END IF + END DO + END IF + Int_BufSz = Int_BufSz + 1 ! OutParamLinIndx allocated yes/no + IF ( ALLOCATED(InData%OutParamLinIndx) ) THEN + Int_BufSz = Int_BufSz + 2*2 ! OutParamLinIndx upper/lower bounds for each dimension + Int_BufSz = Int_BufSz + SIZE(InData%OutParamLinIndx) ! OutParamLinIndx END IF IF ( Re_BufSz .GT. 0 ) THEN ALLOCATE( ReKiBuf( Re_BufSz ), STAT=ErrStat2 ) @@ -1927,6 +2232,10 @@ SUBROUTINE LD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Db_Xferred = Db_Xferred + 1 IntKiBuf(Int_Xferred) = InData%IntMethod Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nx + Int_Xferred = Int_Xferred + 1 + IntKiBuf(Int_Xferred) = InData%nq + Int_Xferred = Int_Xferred + 1 IF ( .NOT. ALLOCATED(InData%MM) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -2018,7 +2327,7 @@ SUBROUTINE LD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si Int_Xferred = Int_Xferred + 2 DO i1 = LBOUND(InData%activeDOFs,1), UBOUND(InData%activeDOFs,1) - IntKiBuf(Int_Xferred) = InData%activeDOFs(i1) + IntKiBuf(Int_Xferred) = TRANSFER(InData%activeDOFs(i1), IntKiBuf(1)) Int_Xferred = Int_Xferred + 1 END DO END IF @@ -2037,7 +2346,90 @@ SUBROUTINE LD_PackParam( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, Si DO i2 = LBOUND(InData%AA,2), UBOUND(InData%AA,2) DO i1 = LBOUND(InData%AA,1), UBOUND(InData%AA,1) - IntKiBuf(Int_Xferred) = InData%AA(i1,i2) + ReKiBuf(Re_Xferred) = InData%AA(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( .NOT. ALLOCATED(InData%BB) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BB,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BB,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%BB,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%BB,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%BB,2), UBOUND(InData%BB,2) + DO i1 = LBOUND(InData%BB,1), UBOUND(InData%BB,1) + ReKiBuf(Re_Xferred) = InData%BB(i1,i2) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IntKiBuf(Int_Xferred) = InData%NumOuts + Int_Xferred = Int_Xferred + 1 + IF ( .NOT. ALLOCATED(InData%OutParam) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%OutParam,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%OutParam,1) + Int_Xferred = Int_Xferred + 2 + + DO i1 = LBOUND(InData%OutParam,1), UBOUND(InData%OutParam,1) + CALL NWTC_Library_Packoutparmtype( Re_Buf, Db_Buf, Int_Buf, InData%OutParam(i1), ErrStat2, ErrMsg2, OnlySize ) ! OutParam + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Re_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Re_Buf) > 0) ReKiBuf( Re_Xferred:Re_Xferred+SIZE(Re_Buf)-1 ) = Re_Buf + Re_Xferred = Re_Xferred + SIZE(Re_Buf) + DEALLOCATE(Re_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Db_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Db_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Db_Buf) > 0) DbKiBuf( Db_Xferred:Db_Xferred+SIZE(Db_Buf)-1 ) = Db_Buf + Db_Xferred = Db_Xferred + SIZE(Db_Buf) + DEALLOCATE(Db_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + IF(ALLOCATED(Int_Buf)) THEN + IntKiBuf( Int_Xferred ) = SIZE(Int_Buf); Int_Xferred = Int_Xferred + 1 + IF (SIZE(Int_Buf) > 0) IntKiBuf( Int_Xferred:Int_Xferred+SIZE(Int_Buf)-1 ) = Int_Buf + Int_Xferred = Int_Xferred + SIZE(Int_Buf) + DEALLOCATE(Int_Buf) + ELSE + IntKiBuf( Int_Xferred ) = 0; Int_Xferred = Int_Xferred + 1 + ENDIF + END DO + END IF + IF ( .NOT. ALLOCATED(InData%OutParamLinIndx) ) THEN + IntKiBuf( Int_Xferred ) = 0 + Int_Xferred = Int_Xferred + 1 + ELSE + IntKiBuf( Int_Xferred ) = 1 + Int_Xferred = Int_Xferred + 1 + IntKiBuf( Int_Xferred ) = LBOUND(InData%OutParamLinIndx,1) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%OutParamLinIndx,1) + Int_Xferred = Int_Xferred + 2 + IntKiBuf( Int_Xferred ) = LBOUND(InData%OutParamLinIndx,2) + IntKiBuf( Int_Xferred + 1) = UBOUND(InData%OutParamLinIndx,2) + Int_Xferred = Int_Xferred + 2 + + DO i2 = LBOUND(InData%OutParamLinIndx,2), UBOUND(InData%OutParamLinIndx,2) + DO i1 = LBOUND(InData%OutParamLinIndx,1), UBOUND(InData%OutParamLinIndx,1) + IntKiBuf(Int_Xferred) = InData%OutParamLinIndx(i1,i2) Int_Xferred = Int_Xferred + 1 END DO END DO @@ -2076,6 +2468,10 @@ SUBROUTINE LD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Db_Xferred = Db_Xferred + 1 OutData%IntMethod = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 + OutData%nx = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + OutData%nq = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! MM not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -2182,7 +2578,7 @@ SUBROUTINE LD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg RETURN END IF DO i1 = LBOUND(OutData%activeDOFs,1), UBOUND(OutData%activeDOFs,1) - OutData%activeDOFs(i1) = IntKiBuf(Int_Xferred) + OutData%activeDOFs(i1) = TRANSFER(IntKiBuf(Int_Xferred), OutData%activeDOFs(i1)) Int_Xferred = Int_Xferred + 1 END DO END IF @@ -2204,7 +2600,111 @@ SUBROUTINE LD_UnPackParam( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg END IF DO i2 = LBOUND(OutData%AA,2), UBOUND(OutData%AA,2) DO i1 = LBOUND(OutData%AA,1), UBOUND(OutData%AA,1) - OutData%AA(i1,i2) = IntKiBuf(Int_Xferred) + OutData%AA(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! BB not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%BB)) DEALLOCATE(OutData%BB) + ALLOCATE(OutData%BB(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%BB.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%BB,2), UBOUND(OutData%BB,2) + DO i1 = LBOUND(OutData%BB,1), UBOUND(OutData%BB,1) + OutData%BB(i1,i2) = ReKiBuf(Re_Xferred) + Re_Xferred = Re_Xferred + 1 + END DO + END DO + END IF + OutData%NumOuts = IntKiBuf(Int_Xferred) + Int_Xferred = Int_Xferred + 1 + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! OutParam not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%OutParam)) DEALLOCATE(OutData%OutParam) + ALLOCATE(OutData%OutParam(i1_l:i1_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%OutParam.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i1 = LBOUND(OutData%OutParam,1), UBOUND(OutData%OutParam,1) + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Re_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Re_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Re_Buf = ReKiBuf( Re_Xferred:Re_Xferred+Buf_size-1 ) + Re_Xferred = Re_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Db_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Db_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Db_Buf = DbKiBuf( Db_Xferred:Db_Xferred+Buf_size-1 ) + Db_Xferred = Db_Xferred + Buf_size + END IF + Buf_size=IntKiBuf( Int_Xferred ) + Int_Xferred = Int_Xferred + 1 + IF(Buf_size > 0) THEN + ALLOCATE(Int_Buf(Buf_size),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating Int_Buf.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + Int_Buf = IntKiBuf( Int_Xferred:Int_Xferred+Buf_size-1 ) + Int_Xferred = Int_Xferred + Buf_size + END IF + CALL NWTC_Library_Unpackoutparmtype( Re_Buf, Db_Buf, Int_Buf, OutData%OutParam(i1), ErrStat2, ErrMsg2 ) ! OutParam + CALL SetErrStat(ErrStat2, ErrMsg2, ErrStat, ErrMsg, RoutineName) + IF (ErrStat >= AbortErrLev) RETURN + + IF(ALLOCATED(Re_Buf )) DEALLOCATE(Re_Buf ) + IF(ALLOCATED(Db_Buf )) DEALLOCATE(Db_Buf ) + IF(ALLOCATED(Int_Buf)) DEALLOCATE(Int_Buf) + END DO + END IF + IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! OutParamLinIndx not allocated + Int_Xferred = Int_Xferred + 1 + ELSE + Int_Xferred = Int_Xferred + 1 + i1_l = IntKiBuf( Int_Xferred ) + i1_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + i2_l = IntKiBuf( Int_Xferred ) + i2_u = IntKiBuf( Int_Xferred + 1) + Int_Xferred = Int_Xferred + 2 + IF (ALLOCATED(OutData%OutParamLinIndx)) DEALLOCATE(OutData%OutParamLinIndx) + ALLOCATE(OutData%OutParamLinIndx(i1_l:i1_u,i2_l:i2_u),STAT=ErrStat2) + IF (ErrStat2 /= 0) THEN + CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%OutParamLinIndx.', ErrStat, ErrMsg,RoutineName) + RETURN + END IF + DO i2 = LBOUND(OutData%OutParamLinIndx,2), UBOUND(OutData%OutParamLinIndx,2) + DO i1 = LBOUND(OutData%OutParamLinIndx,1), UBOUND(OutData%OutParamLinIndx,1) + OutData%OutParamLinIndx(i1,i2) = IntKiBuf(Int_Xferred) Int_Xferred = Int_Xferred + 1 END DO END DO @@ -2424,18 +2924,6 @@ SUBROUTINE LD_CopyOutput( SrcOutputData, DstOutputData, CtrlCode, ErrStat, ErrMs END IF DstOutputData%qd = SrcOutputData%qd ENDIF -IF (ALLOCATED(SrcOutputData%qdd)) THEN - i1_l = LBOUND(SrcOutputData%qdd,1) - i1_u = UBOUND(SrcOutputData%qdd,1) - IF (.NOT. ALLOCATED(DstOutputData%qdd)) THEN - ALLOCATE(DstOutputData%qdd(i1_l:i1_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating DstOutputData%qdd.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - END IF - DstOutputData%qdd = SrcOutputData%qdd -ENDIF IF (ALLOCATED(SrcOutputData%WriteOutput)) THEN i1_l = LBOUND(SrcOutputData%WriteOutput,1) i1_u = UBOUND(SrcOutputData%WriteOutput,1) @@ -2474,9 +2962,6 @@ SUBROUTINE LD_DestroyOutput( OutputData, ErrStat, ErrMsg, DEALLOCATEpointers ) IF (ALLOCATED(OutputData%qd)) THEN DEALLOCATE(OutputData%qd) ENDIF -IF (ALLOCATED(OutputData%qdd)) THEN - DEALLOCATE(OutputData%qdd) -ENDIF IF (ALLOCATED(OutputData%WriteOutput)) THEN DEALLOCATE(OutputData%WriteOutput) ENDIF @@ -2522,11 +3007,6 @@ SUBROUTINE LD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S Int_BufSz = Int_BufSz + 2*1 ! qd upper/lower bounds for each dimension Re_BufSz = Re_BufSz + SIZE(InData%qd) ! qd END IF - Int_BufSz = Int_BufSz + 1 ! qdd allocated yes/no - IF ( ALLOCATED(InData%qdd) ) THEN - Int_BufSz = Int_BufSz + 2*1 ! qdd upper/lower bounds for each dimension - Re_BufSz = Re_BufSz + SIZE(InData%qdd) ! qdd - END IF Int_BufSz = Int_BufSz + 1 ! WriteOutput allocated yes/no IF ( ALLOCATED(InData%WriteOutput) ) THEN Int_BufSz = Int_BufSz + 2*1 ! WriteOutput upper/lower bounds for each dimension @@ -2574,21 +3054,6 @@ SUBROUTINE LD_PackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Indata, ErrStat, ErrMsg, S Re_Xferred = Re_Xferred + 1 END DO END IF - IF ( .NOT. ALLOCATED(InData%qdd) ) THEN - IntKiBuf( Int_Xferred ) = 0 - Int_Xferred = Int_Xferred + 1 - ELSE - IntKiBuf( Int_Xferred ) = 1 - Int_Xferred = Int_Xferred + 1 - IntKiBuf( Int_Xferred ) = LBOUND(InData%qdd,1) - IntKiBuf( Int_Xferred + 1) = UBOUND(InData%qdd,1) - Int_Xferred = Int_Xferred + 2 - - DO i1 = LBOUND(InData%qdd,1), UBOUND(InData%qdd,1) - ReKiBuf(Re_Xferred) = InData%qdd(i1) - Re_Xferred = Re_Xferred + 1 - END DO - END IF IF ( .NOT. ALLOCATED(InData%WriteOutput) ) THEN IntKiBuf( Int_Xferred ) = 0 Int_Xferred = Int_Xferred + 1 @@ -2651,24 +3116,6 @@ SUBROUTINE LD_UnPackOutput( ReKiBuf, DbKiBuf, IntKiBuf, Outdata, ErrStat, ErrMsg Re_Xferred = Re_Xferred + 1 END DO END IF - IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! qdd not allocated - Int_Xferred = Int_Xferred + 1 - ELSE - Int_Xferred = Int_Xferred + 1 - i1_l = IntKiBuf( Int_Xferred ) - i1_u = IntKiBuf( Int_Xferred + 1) - Int_Xferred = Int_Xferred + 2 - IF (ALLOCATED(OutData%qdd)) DEALLOCATE(OutData%qdd) - ALLOCATE(OutData%qdd(i1_l:i1_u),STAT=ErrStat2) - IF (ErrStat2 /= 0) THEN - CALL SetErrStat(ErrID_Fatal, 'Error allocating OutData%qdd.', ErrStat, ErrMsg,RoutineName) - RETURN - END IF - DO i1 = LBOUND(OutData%qdd,1), UBOUND(OutData%qdd,1) - OutData%qdd(i1) = ReKiBuf(Re_Xferred) - Re_Xferred = Re_Xferred + 1 - END DO - END IF IF ( IntKiBuf( Int_Xferred ) == 0 ) THEN ! WriteOutput not allocated Int_Xferred = Int_Xferred + 1 ELSE @@ -2957,12 +3404,6 @@ SUBROUTINE LD_Output_ExtrapInterp1(y1, y2, tin, y_out, tin_out, ErrStat, ErrMsg y_out%qd(i1) = y1%qd(i1) + b * ScaleFactor END DO END IF ! check if allocated -IF (ALLOCATED(y_out%qdd) .AND. ALLOCATED(y1%qdd)) THEN - DO i1 = LBOUND(y_out%qdd,1),UBOUND(y_out%qdd,1) - b = -(y1%qdd(i1) - y2%qdd(i1)) - y_out%qdd(i1) = y1%qdd(i1) + b * ScaleFactor - END DO -END IF ! check if allocated IF (ALLOCATED(y_out%WriteOutput) .AND. ALLOCATED(y1%WriteOutput)) THEN DO i1 = LBOUND(y_out%WriteOutput,1),UBOUND(y_out%WriteOutput,1) b = -(y1%WriteOutput(i1) - y2%WriteOutput(i1)) @@ -3033,13 +3474,6 @@ SUBROUTINE LD_Output_ExtrapInterp2(y1, y2, y3, tin, y_out, tin_out, ErrStat, Err y_out%qd(i1) = y1%qd(i1) + b + c * t_out END DO END IF ! check if allocated -IF (ALLOCATED(y_out%qdd) .AND. ALLOCATED(y1%qdd)) THEN - DO i1 = LBOUND(y_out%qdd,1),UBOUND(y_out%qdd,1) - b = (t(3)**2*(y1%qdd(i1) - y2%qdd(i1)) + t(2)**2*(-y1%qdd(i1) + y3%qdd(i1)))* scaleFactor - c = ( (t(2)-t(3))*y1%qdd(i1) + t(3)*y2%qdd(i1) - t(2)*y3%qdd(i1) ) * scaleFactor - y_out%qdd(i1) = y1%qdd(i1) + b + c * t_out - END DO -END IF ! check if allocated IF (ALLOCATED(y_out%WriteOutput) .AND. ALLOCATED(y1%WriteOutput)) THEN DO i1 = LBOUND(y_out%WriteOutput,1),UBOUND(y_out%WriteOutput,1) b = (t(3)**2*(y1%WriteOutput(i1) - y2%WriteOutput(i1)) + t(2)**2*(-y1%WriteOutput(i1) + y3%WriteOutput(i1)))* scaleFactor