C sawmisc.f cleaned, hopefully will not give bugs on HP or DEC C A. Gole, 11 April 95 C ============================================================= C C SUPPORTING FUNCTION FOR FPINT C C ============================================================= C BY SCOTT WOODFORD C 6 JULY 1994 C REVISED BY SCOTT WOODFORD C 1 SEPTEMBER 1994 C BASED ON CODE BY A.M. GOLE C 27 SEPT 1990 C ============================================================= C REAL FUNCTION SONOX3(INPUT,TLENG,DT,DEBLCK) C C ============================================================= C ARGUMENTS: C C INPUT REAL INPUT C TLENG REAL INPUT C DEBLCK INTEGER INPUT C DT REAL OUTPUT C ============================================================= C INCLUDE 'emt.d' INTEGER ICH,NEXC,DUM REAL TIME,DELT,PRINT,FINTIM,STOR C REAL INPUT,TLENG,DT COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC C DUM = IZCD1(INPUT,DT) IF(DEBLCK.EQ.0) THEN DUM = 0 DT = 0.0 ENDIF IF(DUM.EQ.-1.OR.TIME.LT.DELT) THEN DUM = 0 DT = 0.0 ENDIF C IF(DUM.EQ.1) THEN STOR(NEXC+1) = 1.0 STOR(NEXC+2) = TIME+TLENG+DELT*1.0E-6 ENDIF IF(TIME.GT.STOR(NEXC+2)) THEN STOR(NEXC+1) = 0.0 ENDIF C SONOX3 = STOR(NEXC+1) NEXC = NEXC+2 C RETURN END C ============================================================= C C SUPPORTING FUNCTION FOR FPINT C C ============================================================= C BY SCOTT WOODFORD C 19 JULY 1994 C REVISED BY SCOTT WOODFORD C 1 SEPTEMBER 1994 C ============================================================= C REAL FUNCTION BINOX3(ON,OFF,DT) C C ============================================================= C ARGUMENTS: C C ON REAL INPUT C OFF REAL INPUT C DT REAL OUTPUT C ============================================================= C INCLUDE 'emt.d' INTEGER ICH,NEXC,PD_1,PD_2 REAL TIME,DELT,PRINT,FINTIM,STOR C REAL ON,OFF,DT,TD_1,TD_2 COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC C PD_1 = IZCD1(ON,TD_1) PD_2 = IZCD1(OFF,TD_2) IF(DEBLCK.EQ.0) THEN PD_1 = 0 PD_2 = 0 TD_1 = 0.0 TD_2 = 0.0 ENDIF C IF(PD_2.EQ.1) THEN STOR(NEXC+1) = 0.0 DT = TD_2 ELSEIF(PD_1.EQ.1) THEN STOR(NEXC+1) = 1.0 DT = TD_1 ELSE DT = 0.0 ENDIF C BINOX3 = STOR(NEXC+1) NEXC = NEXC+1 C RETURN END C ============================================================= C C HARMONIC DISTORTION CALCULATION EMTDC BLOCK C C ============================================================= C C BY SCOTT WOODFORD C 2 AUGUST 1994 C C ============================================================= C REAL FUNCTION THD(IN,OUT,N,MODE) C C ============================================================= C ARGUMENTS: C C IN MAGNITUDES OF HARMONIC COMPONENTS OF THE INPUT; C IN(1) IS THE FUNDAMENTAL COMPONENT C OUT DISTORTION OF THE CORRESPONDING INPUT HARMONIC; C OUT(N) IS THE NTH HARMONIC DISTORTION C OUT(2) IS THE 2ND HARMONIC DISTORTION C N NUMBER OF HARMONICS C MODE=0 OUTPUT IN PER UNIT C MODE=1 OUTPUT IN PERCENT C THD TOTAL HARMONIC DISTORTION OUTPUT C ============================================================= C INTEGER N,MODE,I REAL IN(N),OUT(N),INVFND C INVFND=1.0/IN(1) OUT(1) = 1.0 DO 10 I=2,N OUT(I) = IN(I)*INVFND 10 CONTINUE THD = 0.0 DO 20 I=2,N THD = THD+OUT(I)*OUT(I) 20 CONTINUE THD = SQRT(THD) IF(MODE.GE.1) THEN THD = THD*100.0 DO 30 I=1,N OUT(I) = OUT(I)*100.0 30 CONTINUE ENDIF RETURN END C C*********************************************************************** INTEGER FUNCTION IZCD1(X,DT) C+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ C ZERO XING DETECTION WITH INTERPOLATION..THE FUNCTION C TAKES ON A VALUE +1 FOR +VE ZERO XING AND -1 FOR NEG C ZERO XINGS. DT IS THE TIME BEFORE THE PRESENT TIMESTEP C WHEN THE CHANGE OCCURRED. (DT < DELT OF COURSE!) C ---------------------------------------------------------------------- C WRITTEN BY A.M. GOLE 27/9/90 C altered by Scott Woodford 18 August,1994 C+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ C C INCLUDE AND COMMON BLOCK... include 'emt.d' include 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /S4/VAR(ND11),CON(ND12),PGB(ND13) C VARIABLE DECLERATIONS INTEGER NEXC,ICH REAL TIME,DELT,PRINT,FINTIM,STOR,VAR,CON,PGB real DIFF c IZCD1=0 DT=0.0 C TO DETECT +VE CROSSING..... IF(STOR(NEXC+1).LT.0.0.AND.X.GE.0.0)THEN DIFF=X-STOR(NEXC+1) IZCD1=1 DT=X/DIFF*DELT ENDIF C TO DETECT -VE CROSSING..... IF(STOR(NEXC+1).GT.0.0.AND.X.LE.0.0)THEN DIFF=X-STOR(NEXC+1) IZCD1=-1 DT=X/DIFF*DELT ENDIF STOR(NEXC+1)=X IF(TIME.LT.DELT) THEN IZCD1=0 DT=0.0 ENDIF NEXC=NEXC+1 C RETURN END C*********************************************************************** SUBROUTINE LATCH1 (ISBAR,IRBAR,IQ,IQBAR) C__________________________________________________________________ C Models an S/R Flipflop latch with inputs ISBAR and IRBAR. C A low on ISBAR sets the latch, a low on IRBAR Resets it. C The Q and Qbar outputs of the latch are IQ,IQBAR respectively. C All signals are 1 for the logic hi and 0 for the logic lo states. C.................................................................. C C INCLUDE AND COMMON BLOCK... include 'emt.d' include 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /S4/VAR(ND11),CON(ND12),PGB(ND13) C VARIABLE DECLERATIONS INTEGER NEXC,ICH REAL TIME,DELT,PRINT,FINTIM,STOR,VAR,CON,PGB C.................................................................. IQ = NINT ( STOR(NEXC+1)) IQBAR = 1-IQ C IF(ISBAR.GE.1 .AND. IRBAR.LE.0) THEN IQ = 0 IQBAR = 1 ELSE IF(ISBAR.LE.0 .AND. IRBAR.GE.1) THEN IQ = 1 IQBAR = 0 ELSE IF(ISBAR.LE.0 .AND. IRBAR.LE.0) THEN C ...(this is the forbidden state in the normal operation of the ff) IQ = 1 IQBAR =1 ENDIF C STOR (NEXC+1) = IQ NEXC = NEXC+1 C RETURN END C******************************************************************** INTEGER FUNCTION MONO2 (IN , TS) C____________________________________________________________________ C Models a resettable Monostable multivibrator. A positive edge on IN C results in the output going Hi and it remains hi for a period TS C after the trailing edge of IN. If IN shud go hi before the period C TS has elapsed, the Mono is retriggered and stays hi for the period C TS following the trailing edge of the new hi. C C........................................................ C C INCLUDE AND COMMON BLOCK... include 'emt.d' include 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /S4/VAR(ND11),CON(ND12),PGB(ND13) C VARIABLE DECLERATIONS INTEGER NEXC,ICH REAL TIME,DELT,PRINT,FINTIM,STOR,VAR,CON,PGB C C........................................................ ISTATE = NINT (STOR(NEXC+1) ) TSW = STOR(NEXC+2) C IF(IN.GE.1) THEN ISTATE = 1 TSW = TIME +TS ENDIF IF ( TIME . GE . TSW) THEN ISTATE =0 ENDIF STOR (NEXC+1 ) = ISTATE STOR (NEXC+2) = TSW NEXC = NEXC +2 MONO2 = ISTATE RETURN END C********************************************************************** INTEGER FUNCTION XNOR(IX,IY) C_--------------------------------------------------------------------- C C Does the EXCLUSIVE NOR operation on signals IX and IY C which should be logic levels (integers) 0 or 1. C..................................................................... C INTEGER XNOR IF((IX.EQ.0.AND.IY.EQ.0).OR.(IX.GE.1.AND.IY.GE.1)) THEN XNOR = 1 ELSE XNOR = 0 ENDIF RETURN END C____________________________________________________________________ C******************************************************************** INTEGER FUNCTION MONO3 (IN , TS) C____________________________________________________________________ C Models an edge triggered Monostable multivibrator. A positive edge C on IN results in the output going Hi and it remains hi for a period C TS after being turned on. If IN shud go hi again before the period C TS has elapsed, the Mono is retriggered and stays hi for the period C TS following the trailing edge of the new hi. C C........................................................ C C INCLUDE AND COMMON BLOCK... include 'emt.d' include 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /S4/VAR(ND11),CON(ND12),PGB(ND13) C VARIABLE DECLERATIONS INTEGER NEXC,ICH REAL TIME,DELT,PRINT,FINTIM,STOR,VAR,CON,PGB INTEGER TRIG C C........................................................ TRIG = IZCD1(In-0.5,DT) IF(TRIG.LE.0) TRIG = 0 C That was the edge detection... MONO3 = MONO2(TRIG,TS) C RETURN END C*********************************************************************** SUBROUTINE LATCK2 (IS,IR,IPRSET,ICLEAR,ITRIG,IQ,IQBAR) C__________________________________________________________________ C Models an S/R Flipflop latch with inputs IS and IR and trigger C ITRIG. States only change on rising edge of ITRIG. C A Hi on IS sets the latch, a Hi on IR Resets it. C The Q and Qbar outputs of the latch are IQ,IQBAR respectively. C Can be preset (IPRSET=1) or cleared (ICLEAR = 1) C State is unchanged if IPRSEt = ICLEAR = 1 (Forbidden State) C All signals are 1 for the logic hi and 0 for the logic lo states. C.................................................................. C C INCLUDE AND COMMON BLOCK... include 'emt.d' include 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /S4/VAR(ND11),CON(ND12),PGB(ND13) C VARIABLE DECLERATIONS INTEGER NEXC,ICH REAL TIME,DELT,PRINT,FINTIM,STOR,VAR,CON,PGB C.................................................................. C Preset and clear override, if preset = 1 , clear =0 then latch is set C if preset = 0 , clear =1 then latch is re-set. If both are 1, latch C state is frozen EVEN if a trigger exists. C................................................................ C Define stor location for latch: NIQ = NEXC + 1 NEXC = NEXC + 1 C............................. C Default states are the old states of the latch..Also at startup C IQ = 0 as all STOR locations are zero C IQ = NINT ( STOR(NIQ)) IQBAR = 1-IQ C........................................................... C Detecting the rising Edge of ITRIG: C ITRIG1 = IZCD1(ITRIG-0.5,DT) C C............................................................. C Determining state transitions: IF(ITRIG1.GE.1) THEN IF(IS.GE.1.AND.IR.LE.0) THEN IQ = 1 IQBAR = 0 ELSE IF(IS.LE.0.AND.IR.GE.1) THEN IQ = 0 IQBAR = 1 ELSE C .....Forbidden input condition or both inputs zero CONTINUE ENDIF ENDIF C Preset/Clear: IF(IPRSET.GE.1.AND.ICLEAR.LE.0) THEN IQ = 1 IQBAR = 0 ENDIF IF(ICLEAR.GE.1.AND.IPRSET.LE.0) THEN IQ = 0 IQBAR = 1 ENDIF IF(ICLEAR * IPRSET.GE.1) THEN IQ = NINT ( STOR(NIQ)) IQBAR = 1-IQ ENDIF C............................................................ C Storing for use in next timestep: STOR(NIQ) = IQ C RETURN END C************************************************************** INTEGER FUNCTION ITIMR1(ISTART,DURATN) C__________________________________________________________________ C Timers similar to those in interblocking logic (BP1) C If ISTART = 0, timer is reset. If ISTART =1, the timer C times out after a duration DURATN and then sets its output C high (1). C C A.M. Gole, E&C HVDC, 22 May 1992 C___________________________________________________________________ C Input: C ISTART: ISTART = 0 resets timer, Istart = 1 sttarts timer C which outputs a 1 after the duration DURATN C state (ms) . It is assumed that ISQRN = 0 for C DURATN : Duration (s) C................................................................... C C INCLUDE AND COMMON BLOCK... include 'emt.d' include 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /S4/VAR(ND11),CON(ND12),PGB(ND13) C VARIABLE DECLERATIONS INTEGER NEXC,ICH REAL TIME,DELT,PRINT,FINTIM,STOR,VAR,CON,PGB REAL TWOPI,PI2BY3 C____________________________________________________________________ C IF(TIME.LE.DELT) STOR(NEXC+1) = DURATN C JTIMR = 0 C IF(ISTART.LE.0) THEN C ...reset Timer... STOR(NEXC+1) = DURATN ELSE C ..start timer... STOR(NEXC+1) = STOR(NEXC+1) - DELT C ....If timer times out: IF(STOR(NEXC+1).LE.0.0) JTIMR = 1 ENDIF C ITIMR1 = JTIMR C NEXC = NEXC + 1 C RETURN END C*********************************************************************** C*********************************************************************** SUBROUTINE ORAND1(SET,STQUO,Q) C______________________________________________________________________ C Models special AND-OR latch in interblocking controls. C Latch is set for Set = 1 and maintains status quo for C STQUO = 1 and SET = 0. IF STQUO and SET are both 0, the C latch is reset. C........................................................ C DATA TWOPI/6.28319/,PI2BY3/2.094395/,ms/1.0E-3/ C INCLUDE AND COMMON BLOCK... include 'emt.d' include 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /S4/VAR(ND11),CON(ND12),PGB(ND13) C VARIABLE DECLERATIONS C Everything is integer unless otherwise specd) INTEGER NEXC,ICH INTEGER SET,STQUO,Q,QOLD REAL TIME,DELT,PRINT,FINTIM,STOR,VAR,CON,PGB REAL TWOPI,PI2BY3 REAL LIMIT,ms C____________________________________________________________________ C QOLD = NINT(STOR(NEXC+1)) Q = MAX0(SET, STQUO * QOLD) STOR(NEXC +1) = Q C NEXC = NEXC + 1 C............................................................. RETURN END C******************************************************************** INTEGER FUNCTION AND2OR(A,B,C,D) C_____________________________________________________________________ C Evaluates A.B + C.D ..a very common and/or block used in Interblocking. C INTEGER A,B,C,D,AND2OR AND2OR = MAX0(A*B, C*D) RETURN END C*********************************************************************** C*********************************************************************** FUNCTION DELAX(TD,N,X) C TD = DELAY, N = NO OF SAMPLES IN TD, X = INPUT SIGNAL C NOTE: TD CAN BE CHANGED DURING THE SOLUTION. C HOWEVER, N STAYS CONSTANT. C ***************** VERSION 2.0 COMPATIBLE, FEB. 1988 ******************* INCLUDE 'emt.d' C C VARIABLE DECLARATIONS C REAL DELAX REAL DELT REAL FINTIM INTEGER I,ICH INTEGER J INTEGER L INTEGER M INTEGER N,NEXC REAL PRINT REAL STOR REAL TD,TIME REAL X C COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC C INITIALIZATION AT TIME = 0 IF (TIME.LT.DELT) THEN STOR(NEXC+1)=TD/N J=NEXC+2 L=N+J DO 7 I=J,L STOR(I)=X 7 CONTINUE ENDIF C C DYNAMIC SOLUTION J=NEXC+2 L=N+J DELAX=STOR(L) IF (TIME.GE.STOR(NEXC+1)) THEN STOR(NEXC+1)=TIME+TD/N C STOR(J)=X DO 9 I=1,N M=L-I STOR(M+1)=STOR(M) 9 CONTINUE STOR(J)=X ENDIF NEXC=L RETURN END C ***************************************************************** FUNCTION GRP2(U,T,DLIM,ULIM) C ---------------------------------------------------------------------- C Real pole function with non windup limits. C Usage: Y = GRP2(T,DLIM,ULIM,U) means: C Y = U/(1+sT) limited (nonwindup) to [DLIM,ULIM] C C Inputs: C U: Input to real pole C T: time constant C DLIM,ULIM: lower and Upper limits C C................................................................. C WRITTEN BY A.M. GOLE 92/10/06 C................................................................. C C INCLUDE AND COMMON BLOCK... include 'emt.d' include 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /S4/VAR(ND11),CON(ND12),PGB(ND13) C VARIABLE DECLERATIONS INTEGER NEXC,ICH REAL TIME,DELT,PRINT,FINTIM,STOR,VAR,CON,PGB C........................................................... C Pulling out old values of U and state C XOLD = STOR(NEXC+1) UOLD = STOR(NEXC+2) C Trapezoidal Integration: C X1 = ( XOLD * (T-0.5*DELT) + (U + UOLD) * 0.5*DELT ) / C ------------------------------------------------ . (T+0.5*DELT) C X = X1 IF(X.GE.ULIM) X = ULIM IF(X.LE.DLIM) X = DLIM C STOR(NEXC+1) = X STOR(NEXC+2) = U NEXC = NEXC +2 C GRP2 = X C RETURN END C******************************************************************* SUBROUTINE T2M3(ION,IOFF,TON,TOFF,INITST,IOUT) C__________________________________________________________________ C Models a timed transition diagram, ie, at t = 0, the state is C low or hi flagged by initst = 0/1 and thus iout is 0 or 1. C If in the low state (with iout = 0) then a transition from 0 --> 1 C of ION causes IOUT to go to 1 after a time TON has elapsed. C Likewise to go from 1 ---> 0 it takes time TOFF after Ioff makes C a 0 --> 1 transition. C A.M. Gole, E&C HVDC, 10 June 1992 C___________________________________________________________________ C Inputs: C ION: Trigger (activated on 0 --> 1 transition) to go to hi state C IOFF: Trigger (activated on 0 --> 1 transition) to go to lo state C TON : Delay to go from lo to hi state C TOFF: Delay to go from hi to lo state C INITST: Initial state 0 = lo, 1 = hi C Outputs: C IOUT : Final output, 0 = lo/1 = hi state C................................................................... C DATA TWOPI/6.28319/,PI2BY3/2.094395/,ms/1.0E-3/ C INCLUDE AND COMMON BLOCK... include 'emt.d' include 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /S4/VAR(ND11),CON(ND12),PGB(ND13) C VARIABLE DECLERATIONS INTEGER NEXC,ICH REAL TIME,DELT,PRINT,FINTIM,STOR,VAR,CON,PGB REAL TWOPI,PI2BY3 REAL LIMIT,ms C____________________________________________________________________ C C Storage locations: C NSTATE = NEXC + 1 NTIMER = NEXC + 2 NEXC = NEXC + 3 C(We r reserving 1 spare store location..We may need it later) C ISTATE = NINT(STOR(NSTATE)) TIMER = STOR(NTIMER) C IF(TIME.LE.0.5*DELT) THEN C Initialize state: IF(INITST.EQ.0) ISTATE = 0 IF(INITST.EQ.1) ISTATE = 2 ENDIF C C.................................................... IF(ISTATE.EQ.0) THEN C ...lo state IOUT = 0 IF(ION.GE.1) THEN C ... load timer and go to state 1 TIMER = TON ISTATE = 1 ENDIF C.................................................... ELSE IF (ISTATE.EQ.1) THEN C ... Timer intermediate to a 0 --> 1 transition, IOUT = 0 C ...decrement timer TIMER = TIMER - DELT C ...Timer timed out? IF(TIMER.LT.DELT) THEN C ... Go to Hi state ISTATE = 2 IOUT = 1 ENDIF C.................................................... C ELSE IF (ISTATE.EQ.2) THEN C ... Hi state IOUT = 1 IF(IOFF.GE.1) THEN C ...load timer and go to state 3 TIMER = TOFF ISTATE = 3 ENDIF C.................................................... ELSE IF (ISTATE.EQ.3) THEN C ... Timer state between 1 --> 0 transition IOUT = 1 C ...decrement timer TIMER = TIMER - DELT C ...Timer timed out? IF(TIMER.LT.DELT) THEN C ..go to lo state ISTATE = 0 IOUT = 0 ENDIF C.................................................... ENDIF C STOR(NSTATE) = ISTATE STOR(NTIMER) = TIMER C RETURN END C************************************************************** INTEGER FUNCTION ITIMR3(ISTART,IRESET,DURUP,DURDN,INITST) C__________________________________________________________________ C Timer similar to those built with DC191 Cards (BP1) C If ISTART =1, the timer times out after a duration DURUP and C then sets its output high (1). If IRESET = 1, Timer is reset C After duration DURDN * 3/16 C C A.M. Gole, E&C HVDC, 13 Oct 1992 C___________________________________________________________________ C Input: C ISTART: ISTART = Istart = 1 sttarts timer C which outputs a 1 after the duration DURUP C IRESET: =1 for reset, which fully resets the timer after C a duration DURDN (The output actually resets in about C 3/16 of this time. If ISTART appears again within the C period of full reset, the next up duration will be C less than DURUP) C DURUP : Duration (s) after which output goes hi after ISTART C sets. C DURDN : Time to reset the timer C INITST: =0 if timer output =0 at t=0; =1 if output =1 C................................................................... C C INCLUDE AND COMMON BLOCK... include 'emt.d' include 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /S4/VAR(ND11),CON(ND12),PGB(ND13) C VARIABLE DECLERATIONS INTEGER NEXC,ICH REAL TIME,DELT,PRINT,FINTIM,STOR,VAR,CON,PGB REAL TWOPI,PI2BY3 REAL LIMIT C____________________________________________________________________ C Initializion, Calculation of Charging and disharging "Currents" C IF(TIME.LT.DELT) THEN STOR(NEXC+1) = 16.0/DURUP STOR(NEXC+2) = 16.0/DURDN IF(INITST.GE.1) THEN STOR(NEXC+3) = 11 ELSE STOR(NEXC+3) = -8 ENDIF ENDIF C CHGUP = STOR(NEXC+1) CHGDN = STOR(NEXC+2) C ..(Integrator integrates between -8 to +11 V) C CHARGE = CHGUP * ISTART - CHGDN * IRESET C VCAP1 = STOR(NEXC+3) + CHARGE * DELT VCAP = LIMIT(-8.0,11.0,VCAP1) STOR(NEXC+3) = VCAP C C Thresholding: C IF(VCAP.GE.8.0) IVOUT = 1 IF(VCAP.LT.8.0) IVOUT = 0 C ITIMR3 = IVOUT C NEXC = NEXC + 3 PGB(3) = VCAP C RETURN END C********************************************************************* C ====================================================================== SUBROUTINE PHD3(AIN, BIN, PHASE) C ---------------------------------------------------------------------- C ====================================================================== C Include and Common Block Declarations C ---------------------------------------------------------------------- INCLUDE 'emt.d' INCLUDE 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /S4/VAR(ND11),CON(ND12),PGB(ND13) REAL PI_, TWO_PI, PI_BY3, PI2_BY3 INTEGER NN,IEDA,IEDX,AXORB,ISIGN REAL PHASE,AIN,BIN,A,B DATA PI_/3.141592654/, TWO_PI/6.283185307/ DATA PI_BY3/1.047197551/, PI2_BY3/2.094395102/ C C ====================================================================== REAL TIME,DELT,PRINT,FINTIM,STOR,VAR,CON,PGB INTEGER ICH,NEXC C C ===================================== NN = NEXC ISIGN = NINT(STOR(NN+1)) NEXC = NEXC + 1 C================================= IF(AIN.GT.0.0) THEN A=1.0 ELSE A=0.0 ENDIF IF(BIN.GT.0.0) THEN B=1.0 ELSE B=0.0 ENDIF AXORB = (1-A)*B + (1-B)*A IF(AXORB.GT.1) AXORB = 1 IEDA = IZCD1(A-0.5,DT) IEDX = IZCD1(AXORB-0.5,DT) IF(IEDX.GE.1) THEN IF(IABS(IEDA).GE.1.0) THEN ISIGN = 1 ELSE ISIGN = -1 ENDIF ENDIF C PGB(9) = ISIGN PHASE = FLOAT(ISIGN * AXORB) STOR(NN+1) = ISIGN RETURN END C ============================================================= C C ONLINE FAST FOURIER TRANSFORM EMTDC BLOCK C C *********************!!!*WARNING*!!!************************* C C The values STORed by this subroutine are infiltrated by the C FFTSEQ and FTRAC1 subroutines. If any of them are altered C independently or moved relative to each other in DSDYN, C big problems may occur. C C ============================================================= C C BASED ON ORIGINAL FORTRAN CODE BY ROHITHA JAYASINGHE C 23 APRIL 1992 C FINAL VERSION BY SCOTT WOODFORD C 9 JUNE 1994 C ============================================================= C SUBROUTINE FFT180(INPRE,MAG,PHSANG,DC,FREQ,AMODE, + PMODE,HARMS,AAFILT,UPDATE) C C ============================================================= C ARGUMENTS: C C INPRE INPUT SIGNAL C MAG MAGNITUDES OF HARMONIC COMPONENTS OF THE INPUT; C MAG(1) IS THE FUNDAMENTAL COMPONENT C PHSANG PHASE ANGLES OF HARMONIC COMPONENTS OF THE INPUT; C PHSANG(1) IS THE FUNDAMENTAL COMPONENT C DC DC COMPONENT OF INPUT SIGNAL C FREQ INITIAL FUNDAMENTAL FREQUENCY C AMODE=0 MAGNITUDE OUTPUT IN RMS VALUES C AMODE=1 MAGLITUDE OUTPUT IN PEAK VALUES C PMODE=0 PHASE OUTPUT IN RADIANS C PMODE=1 PHASE OUTPUT IN DEGREES C HARMS NUMBER OF HARMONICS C AAFILT=0 UNFILTERED OPERATION C AAFILT=1 ANTI-ALIASING FILTER USED C UPDATE OUTPUT VARIABLE USED BY FFTSEQ SUBROUTINE: C UPDATE=0 A SAMPLE WAS NOT TAKEN IN THE CURRENT TIME STEP; C UPDATE=NSAMP WHEN NON-ZERO C ============================================================= C VARIABLE DEFINITIONS: C C N NUMBER OF SAMPLES PER CYCLE C FTRUE FUNDAMENTAL FREQUENCY C INPUT FILTERED INPUT SIGNAL C INPOLD FILTERED INPUT SIGNAL OF PREVIOUS TIME STEP C TMNSMP TIME OF NEXT SAMPLE C FINTRP INTERPOLATION FACTOR C SMPTIM TIME BETWEEN SAMPLES C NSAMP SAMPLE NUMBER VARIES BETWEEN 1 AND N C X ARRAY OF SAMPLE POINTS C ============================================================= C INCLUDE 'emt.d' C INTEGER ICH,NEXC,HARMS,AAFILT REAL TIME,DELT,PRINT,FINTIM,STOR C INTEGER N,N2,NSAMP,J,AMODE,PMODE,UPDATE REAL INPRE,MAG(HARMS),PHSANG(HARMS),DC,FREQ REAL TMNSMP,FINTRP,INPOLD,X(64),INPUT REAL DEGS,RMSFCT,FTRUE COMMON/S1/TIME,DELT,ICH,PRINT,FINTIM COMMON/S2/STOR(ND10),NEXC COMMON/IO/IIN,IOUT C C INITIALIZING NECESSARY CONSTANTS C N2=HARMS+1 N=N2*2 DEGS=57.295780 RMSFCT=0.70710678 C C INITIALIZING THE FREQUENCY - C STOR(NEXC+180) IS THE STOR LOCATION ALTERED BY C THE FTRAC1 SUBROUTINE. C IF(TIME.LT.DELT) THEN FTRUE=FREQ STOR(NEXC+180)=FTRUE ELSE FTRUE=STOR(NEXC+180) ENDIF C C ANTI-ALIASING FILTER C IF(AAFILT.EQ.1) THEN INPUT=PFFT49(INPRE,0.84375*N2*FTRUE,1.0,3.8637,7.464, + 9.1415,7.464,3.8637,1.0) ELSE INPUT=INPRE NEXC=NEXC+49 ENDIF C C INITIALIZATION OF STOR LOCATIONS C IF (TIME.LT.DELT) THEN STOR(NEXC+1) = 1.0/(FTRUE*N) STOR(NEXC+2) = 1.0 STOR(NEXC+3) = 0.0 DO 20 J=1,N STOR(NEXC+3+J)=0.0 20 CONTINUE ENDIF C C ITERATION LOOP C TMNSMP = STOR(NEXC+1) IF(TIME.GE.TMNSMP) THEN FINTRP = (TMNSMP-TIME+DELT)/DELT INPOLD = STOR(NEXC+3) NSAMP = MOD(NINT(STOR(NEXC+2)),N) + 1 STOR(NEXC+3+NSAMP) =FINTRP*(INPUT-INPOLD) + INPOLD SMPTIM = 1.0/(FTRUE*N) IF(SMPTIM.LT.DELT) THEN WRITE(IOUT,111) WRITE(IOUT,112) CALL OUTMSG(401,0,0,0,0.0,0.0,'FFT ERROR') STOP 111 FORMAT(/,1X,'FFT ERROR:') 112 FORMAT(1X,'SAMPLING RATE IS LESS THAN CALCULATION TIME STEP.') ENDIF DO 10 J=1,N X(J)=STOR(NEXC+3+J) 10 CONTINUE CALL FFTCAL(X,MAG,PHSANG,DC,HARMS) IF(AAFILT.EQ.1) THEN CALL ZFFT(MAG,PHSANG,HARMS) ENDIF DO 70 J=1,HARMS IF(AMODE.EQ.0) THEN MAG(J)=MAG(J)*RMSFCT ENDIF STOR(NEXC+3+N+J) = MAG(J) IF(PMODE.EQ.1) THEN PHSANG(J)=PHSANG(J)*DEGS ENDIF STOR(NEXC+3+N+N2-1+J) = PHSANG(J) 70 CONTINUE STOR(NEXC+3+2*N-1)=DC STOR(NEXC+1) = TMNSMP+SMPTIM STOR(NEXC+2) = FLOAT(NSAMP) UPDATE=NSAMP C ELSE DO 80 J = 1,HARMS MAG(J) = STOR(NEXC+3+N+J) PHSANG(J) = STOR(NEXC+3+N+N2-1+J) 80 CONTINUE DC=STOR(NEXC+3+2*N-1) UPDATE=0 ENDIF C STOR(NEXC+3)=INPUT NEXC = NEXC + 131 C RETURN END C C================================================================= C FAST FOURIER TRANSFORM OF A DISCRETE SIGNAL C ORIGINALLY WRITTEN BY IONI T FERNANDO C 19 APRIL 1992 C MODIFIED FOR EMTDC BY ROHITHA JAYASINGHE C 25 MARCH 1993 C ================================================================ C SUBROUTINE FFTCAL(XREAL,MAG,PHSANG,DC,HARMS) C C================================================================= C MAG(J) - (J)th harmonic magnitude C PHSANG(J) - (J)th harmonic phase angle C reconstructed waveform - C for J=1,HARMS C MAG(J)*COS(J*W*TIME + PHSANG(J)) C +DC C C================================================================= COMPLEX X(64),Y(64) INTEGER N,R,I,N2,HARMS REAL MAG(HARMS),PHSANG(HARMS),XREAL(64),DC,INVN COMPLEX W,POWERW,MULTW INTEGER MAXK,MAXJ,LK INTEGER QUOTNT,REMAIN,JHAT,MAIN,RI,RIR,JJOUT,JJIN INTEGER MAXI C C Variable definitions C -------------------- C N - no. of samples per cycle (64) C R - radix of the FFT (2) C MAXI - LOGR(N) C C Initialisation steps C -------------------- N2=HARMS+1 N=N2*2 INVN=1.0/REAL(N) R = 2 IF(N.EQ.16) THEN MAXI = 4 ELSEIF(N.EQ.32) THEN MAXI = 5 ELSEIF(N.EQ.64) THEN MAXI = 6 ELSE CCC ERROR! STOP ENDIF PI=3.14159265 PI2BYN = 2.0*PI/N W = CMPLX(COS(PI2BYN),-SIN(PI2BYN)) C DO 10 I=1,N X(I)=CMPLX(XREAL(I),0.0) 10 CONTINUE c C Begin computations C CALCULATIONS IN THIS DO LOOP HAVE NOT BEEN ALTERED BY SW. C DO 20 I=MAXI,1,-1 C C transformation using Tr - Operation 1 C Y(N) = ( I(N/R) X T(R) ).X(N) C DO 30 J=1,N2 Y(J) = X(J) + X(J+N2) Y(J+N2) = X(J) - X(J+N2) 30 CONTINUE C C Routine for operation 2 C X(N) = ( I(N/2**I) X DELTA(2**I) ).Y(N) C MAXK=(R**(I-1))-1 MAXJ=(N/(R**I))-1 POWERW=W**(MAXJ+1) DO 40 L=0,1 DO 45 K=0,MAXK LK = L*K MULTW = POWERW**LK DO 47 J=0,MAXJ JJ = ((MAXJ+1)*(((MAXK+1)*L)+K))+J+1 X(JJ) = MULTW*Y(JJ) 47 CONTINUE 45 CONTINUE 40 CONTINUE C C Routine for operation 3 C Y(N) = ( I(N/2**I) X P2(2**I) ).X(N) C RI=R**I RIR=RI/R MAIN=N/RI DO 50 J=0,(RI-1) REMAIN =MOD(J,R) QUOTNT=(J-REMAIN)/R JHAT =(REMAIN*RIR)+QUOTNT DO 55 K=0,(MAIN-1) JJOUT=(J*MAIN)+K JJIN=(JHAT*MAIN)+K Y(JJOUT+1)=X(JJIN+1) 55 CONTINUE 50 CONTINUE C C Transfer data back to register x C X(N) = Y(N) C DO 60 J=1,N X(J)=Y(J) 60 CONTINUE 20 CONTINUE C DO 70 J=1,HARMS MAG(N2-J) = CABS(Y(N2+1+J)+CONJG(Y(N2+1-J)))*INVN IF(CABS(Y(N2+1+J)).GE.1.0E-20) THEN PHSANG(N2-J) = ATAN2(-AIMAG(Y(N2+1+J)),REAL(Y(N2+1+J))) ELSE PHSANG(N2-J)=0.0 ENDIF 70 CONTINUE IF(CABS(Y(1)).GE.1E-20) THEN DC= CABS(Y(1)+CONJG(Y(N2+1)))*INVN*REAL(Y(1))/CABS(Y(1)) ELSE DC = 0.0 ENDIF C RETURN END C C ====================================================================== FUNCTION PFFT49(XIN,FG,A0,A1,A2,A3,A4,A5,A6) C ---------------------------------------------------------------------- C Models Anti-Aliasing Filter at input of Firing Control C State Variable formulation with trapezoidal Integration is used C..................................................................... C Written by A.M. Gole, September 23 1993 C..................................................................... C C Inputs: C XIN : input signal C FG : Scaling frequency, actually s = jw/(2*pi*fg) C A0..A6 : Coeffecients of Denominator C A6 is coeffecient of x^6, etc... C C Output: C PFFT49: (FUNCTION VALUE, USAGE: Y = PFFT49(XIN,FG,A0,....A6) C ==================================================================== C Include and Common Block Declarations C -------------------------------------------------------------------- INCLUDE 'emt.d' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC REAL AMDT(6,6),AMDTI(6,6),APDT(6,6),B(6),X(6) REAL AN(6),BB(6),AAA(6,6),AX(6),BU(6) DOUBLE PRECISION DUMINV(36) INTEGER LLL(6),MMM(6) C C Variable Declarations C -------------------------------------------------------------------- REAL TIME,DELT,PRINT,FINTIM,STOR INTEGER ICH,NEXC C............................................................... C NAAA = NEXC NBB = NAAA+36 NSTAT = NBB + 6 NXOLD = NSTAT+6 C............................................................... C Initial Stuff: C..................................................................... C WG = 6.2831853*FG GAIN = 1/A6 IF(TIME.LT.DELT) THEN C AN(1) = A0*GAIN AN(2) = A1*GAIN AN(3) = A2*GAIN AN(4) = A3*GAIN AN(5) = A4*GAIN AN(6) = A5*GAIN C Forming the Matrices (I-A*DT/2), I+A*DT/2, B C DO 10 I = 1,6 IF(I.NE.6) THEN B(I) = 0.0 ELSE B(I) = 1 ENDIF DO 20 J = 1,6 APDT(I,J) = 0.0 AMDT(I,J) = 0.0 IF(I.EQ.J.AND.I.NE.6) THEN APDT(I,J)= 1.0 AMDT(I,J)= 1.0 ELSE IF(I.EQ.J-1.AND.I.NE.6) THEN APDT(I,J) = DELT*WG*0.5 AMDT(I,J) = -DELT*WG*0.5 ENDIF IF(I.EQ.6) THEN C Last row is special: APDT(I,J) = -AN(J)*0.5*DELT*WG AMDT(I,J) = AN(J)*0.5*DELT*WG IF(J.EQ.6) THEN APDT(I,J) = 1+APDT(I,J) AMDT(I,J) = 1+AMDT(I,J) ENDIF ENDIF 20 CONTINUE 10 CONTINUE C................................................................. C Inverting AMDT: C DUMINV (36), LLL(6), MMM(6) are dummy matrices required for Inversion CALL MATINV(AMDT,AMDTI,6,36,DUMINV,LLL,MMM) C C Form the required Matrix Products (I-A*DT/2)^-1 * (I+A*DT/2) C and (I-A*DT/2)*B C CALL MATMUL(AMDTI,APDT,6,6,6,AAA,1.0) DO 30 I = 1,6 BB(I) = 0.0 DO 40 J = 1,6 BB(I) = BB(I)+AMDTI(I,J)*B(J) 40 CONTINUE 30 CONTINUE C................................................................... C STORING IN STOR LOCATIONS: DO 50 I = 1,6 STOR(NBB+I) = BB(I) DO 60 J = 1,6 STOR(NAAA+6*(I-1)+J) = AAA(I,J) 60 CONTINUE 50 CONTINUE ENDIF C--------------------------------------------------------------------- C Reading the matrices: DO 70 I = 1,6 BB(I)=STOR(NBB+I) X(I) = STOR(NSTAT+I) DO 80 J = 1,6 AAA(I,J)=STOR(NAAA+6*(I-1)+J) 80 CONTINUE 70 CONTINUE OLDXIN = STOR(NXOLD+1) C..................................................................... C Matrix..Vector multiplication to find new statevector: C DO 90 I = 1,6 AX(I) = 0.0 BU(I) = BB(I)*0.5*(XIN+OLDXIN)*DELT*WG*GAIN DO 100 J = 1,6 AX(I) = AX(I)+AAA(I,J)*X(J) 100 CONTINUE 90 CONTINUE C DO 110 J = 1,6 X(J) = AX(J)+BU(J) 110 CONTINUE C..................................................... C Storing the statevector etc. DO 120 J =1,6 STOR(NSTAT+J) = X(J) 120 CONTINUE STOR(NXOLD+1) = XIN PFFT49 = X(1) C NEXC = NXOLD+1 RETURN END C C ========================================================== C C POST-ANTI-ALIASED-FFT PHASE AND MAGNITUDE CORRECTOR C BY SCOTT WOODFORD C 13 JUNE 1994 C C ========================================================== C If the anti-aliasing filter coefficients are changed C this corrector must be changed also. C C 3rd-order Butterworth Filter: C C cutoff frequency: 27 C C pole pairs: C C exp((+/-)13*pi*j/12) C exp((+/-)15*pi*j/12) C exp((+/-)17*pi*j/12) C ========================================================== C SUBROUTINE ZFFT(MAGNIT,PHASE,N) C C ========================================================== C INTEGER N,I REAL MAGNIT(N),PHASE(N) C IF(N.EQ.31) THEN C MAGNIT(17) = MAGNIT(17) * 1.001933 MAGNIT(18) = MAGNIT(18) * 1.003835 MAGNIT(19) = MAGNIT(19) * 1.007329 MAGNIT(20) = MAGNIT(20) * 1.013528 MAGNIT(21) = MAGNIT(21) * 1.024177 MAGNIT(22) = MAGNIT(22) * 1.041898 MAGNIT(23) = MAGNIT(23) * 1.070459 MAGNIT(24) = MAGNIT(24) * 1.114969 MAGNIT(25) = MAGNIT(25) * 1.181910 MAGNIT(26) = MAGNIT(26) * 1.278881 MAGNIT(27) = MAGNIT(27) * 1.414213 MAGNIT(28) = MAGNIT(28) * 1.595853 MAGNIT(29) = MAGNIT(29) * 1.832153 MAGNIT(30) = MAGNIT(30) * 2.130751 MAGNIT(31) = MAGNIT(31) * 2.499407 C PHASE(1) = PHASE(1) + 0.143124 PHASE(2) = PHASE(2) + 0.286392 PHASE(3) = PHASE(3) + 0.429950 PHASE(4) = PHASE(4) + 0.573947 PHASE(5) = PHASE(5) + 0.718538 PHASE(6) = PHASE(6) + 0.863887 PHASE(7) = PHASE(7) + 1.010166 PHASE(8) = PHASE(8) + 1.157562 PHASE(9) = PHASE(9) + 1.306281 PHASE(10) = PHASE(10) + 1.456550 PHASE(11) = PHASE(11) + 1.608628 PHASE(12) = PHASE(12) + 1.762811 PHASE(13) = PHASE(13) + 1.919451 PHASE(14) = PHASE(14) + 2.078964 PHASE(15) = PHASE(15) + 2.241856 PHASE(16) = PHASE(16) + 2.408742 PHASE(17) = PHASE(17) + 2.580375 PHASE(18) = PHASE(18) + 2.757659 PHASE(19) = PHASE(19) + 2.941654 PHASE(20) = PHASE(20) + 3.133535 PHASE(21) = PHASE(21) + 3.334479 PHASE(22) = PHASE(22) + 3.545426 PHASE(23) = PHASE(23) + 3.766677 PHASE(24) = PHASE(24) + 3.997365 PHASE(25) = PHASE(25) + 4.234960 PHASE(26) = PHASE(26) + 4.475149 PHASE(27) = PHASE(27) + 4.712389 PHASE(28) = PHASE(28) + 4.941071 PHASE(29) = PHASE(29) + 5.156726 PHASE(30) = PHASE(30) + 5.356696 PHASE(31) = PHASE(31) + 5.540098 C ELSEIF(N.EQ.15) THEN C MAGNIT(9) = MAGNIT(9) * 1.003835 MAGNIT(10) = MAGNIT(10) * 1.013528 MAGNIT(11) = MAGNIT(11) * 1.041898 MAGNIT(12) = MAGNIT(12) * 1.114969 MAGNIT(13) = MAGNIT(13) * 1.278881 MAGNIT(14) = MAGNIT(14) * 1.595853 MAGNIT(15) = MAGNIT(15) * 2.130751 C PHASE(1) = PHASE(1) + 0.286392 PHASE(2) = PHASE(2) + 0.573947 PHASE(3) = PHASE(3) + 0.863887 PHASE(4) = PHASE(4) + 1.157562 PHASE(5) = PHASE(5) + 1.456550 PHASE(6) = PHASE(6) + 1.762811 PHASE(7) = PHASE(7) + 2.078964 PHASE(8) = PHASE(8) + 2.408742 PHASE(9) = PHASE(9) + 2.757659 PHASE(10) = PHASE(10) + 3.133535 PHASE(11) = PHASE(11) + 3.545426 PHASE(12) = PHASE(12) + 3.997365 PHASE(13) = PHASE(13) + 4.475149 PHASE(14) = PHASE(14) + 4.941071 PHASE(15) = PHASE(15) + 5.356696 C ELSEIF(N.EQ.7) THEN C MAGNIT(5) = MAGNIT(5) * 1.013528 MAGNIT(6) = MAGNIT(6) * 1.114969 MAGNIT(7) = MAGNIT(7) * 1.595853 C PHASE(1) = PHASE(1) + 0.573947 PHASE(2) = PHASE(2) + 1.157562 PHASE(3) = PHASE(3) + 1.762811 PHASE(4) = PHASE(4) + 2.408742 PHASE(5) = PHASE(5) + 3.133535 PHASE(6) = PHASE(6) + 3.997365 PHASE(7) = PHASE(7) + 4.941071 C ENDIF C DO 10 I=1,N IF(PHASE(I).GT.3.14159265) THEN PHASE(I) = PHASE(I) - 6.2831853 ENDIF 10 CONTINUE C RETURN END C C ============================================================ C C TO CONVERT 3-PHASE HARMONIC INPUTS TO SEQUENCED OUTPUTS C C ********************!!!*WARNING*!!!************************* C C This subroutine infiltrates the values STORed by the FFT180 C subroutine. If either of them are altered independently C or moved relative to each other in DSDYN, big problems C may occur. C C BASED ON SUBROUTINE SEQUENCE BY ROHITHA JAYASINGHE C 12 MARCH 1993 C ADAPTED BY SCOTT WOODFORD C 13 JUNE 1994 C ============================================================ C SUBROUTINE FFTSEQ(MA,MB,MC,PA,PB,PC,N,PMODE,UPDATE) C C ============================================================ C INCLUDE 'emt.d' C INTEGER NEXC C INTEGER N,I,PMODE,UPDATE INTEGER NEXMA,NEXMB,NEXMC INTEGER NEXPA,NEXPB,NEXPC REAL MA(N),MB(N),MC(N),PA(N),PB(N),PC(N) COMPLEX ALPHA,ALPHA2 COMPLEX PSRA,PSRB,PSRC COMPLEX PSR1,PSR2,PSR0 C COMMON/S2/STOR(ND10),NEXC C IF(UPDATE.NE.0) THEN NEXMC = NEXC-180+49+3+2*(N+1) NEXMB = NEXMC-180 NEXMA = NEXMB-180 NEXPA = NEXMA+N NEXPB = NEXMB+N NEXPC = NEXMC+N C ALPHA = CMPLX(-0.5,0.8660254) ALPHA2 = CMPLX(-0.5,-0.8660254) C DO 10 I=1,N PSRA = CMPLX(MA(I)*COS(PA(I)),MA(I)*SIN(PA(I))) PSRB = CMPLX(MB(I)*COS(PB(I)),MB(I)*SIN(PB(I))) PSRC = CMPLX(MC(I)*COS(PC(I)),MC(I)*SIN(PC(I))) C PSR1 = PSRA + PSRB*ALPHA + PSRC*ALPHA2 PSR2 = PSRA + PSRB*ALPHA2 + PSRC*ALPHA PSR0 = PSRA + PSRB + PSRC C MA(I) = CABS(PSR1)*0.3333333 MB(I) = CABS(PSR2)*0.3333333 MC(I) = CABS(PSR0)*0.3333333 C IF(CABS(PSR1).GE.1E-20) THEN PA(I) = ATAN2(AIMAG(PSR1),REAL(PSR1)) ELSE PA(I) = 0.0 ENDIF IF(CABS(PSR2).GE.1E-20) THEN PB(I) = ATAN2(AIMAG(PSR2),REAL(PSR2)) ELSE PB(I) = 0.0 ENDIF IF(CABS(PSR0).GE.1E-20) THEN PC(I) = ATAN2(AIMAG(PSR0),REAL(PSR0)) ELSE PC(I) = 0.0 ENDIF C IF(PMODE.EQ.1) THEN PA(I) = PA(I)*57.2957795 PB(I) = PB(I)*57.2957795 PC(I) = PC(I)*57.2957795 ENDIF C STOR(NEXMA+I)=MA(I) STOR(NEXMB+I)=MB(I) STOR(NEXMC+I)=MC(I) STOR(NEXPA+I)=PA(I) STOR(NEXPB+I)=PB(I) STOR(NEXPC+I)=PC(I) 10 CONTINUE C ENDIF C RETURN END C ============================================================ C C FREQUENCY TRACKING SUBROUTINE FOR FFT180. C C ********************!!!*WARNING*!!!************************* C C This subroutine infiltrates the values STORed by the FFT180 C subroutine. If either of them are altered independently C or moved relative to each other in DSDYN, big problems C may occur. C C BASED ON CODE BY ROHITHA JAYASINGHE C BY SCOTT WOODFORD C 13 JUNE 1994 C ============================================================ C SUBROUTINE FTRAC1(PHSNEW,MODE,PMODE,UPDATE) C C ============================================================ C INCLUDE 'emt.d' C INTEGER ICH,NEXC REAL TIME,DELT,PRINT,FINTIM,STOR C INTEGER MODE,PMODE,UPDATE REAL PHSNEW,PHSDIF,PHSOLD,FTRUE COMMON/S1/TIME,DELT,ICH,PRINT,FINTIM COMMON/S2/STOR(ND10),NEXC C IF(UPDATE.EQ.1) THEN FTRUE = STOR(NEXC) IF(TIME.GE.(2.0/FTRUE)) THEN PHSOLD = STOR(NEXC+1) PHSDIF = PHSNEW - PHSOLD IF(PMODE.EQ.0) THEN CONST = 0.15915494 ELSE CONST = 0.002777778 ENDIF FTRUE = FTRUE*(1.0+CONST*PHSDIF) STOR(NEXC) = FTRUE IF(MODE.EQ.2) THEN STOR(NEXC-180) = FTRUE ELSEIF(MODE.GT.2) THEN STOR(NEXC-360) = FTRUE ENDIF ENDIF STOR(NEXC+1) = PHSNEW ENDIF C NEXC = NEXC +1 C RETURN END C ============================================================= SUBROUTINE FILT15(ND,FB,Q,RIP,TYPE,BAND,IN,OUT,MODE) C C TO CONTROL TFDP10 IN DOUBLE PRECISION AS A FILTER BLOCK. C BY SCOTT WOODFORD C 9 AUGUST 1994 C ============================================================= INCLUDE 'emt.d' INTEGER ICH,NEXC REAL TIME,DELT,PRINT,FINTIM,STOR COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /IO/IIN,IOUT C INTEGER NMAX,NMAX1,NMAX2 PARAMETER(NMAX=10,NMAX1=11,NMAX2=100) C INTEGER NSAW PARAMETER(NSAW=10) REAL TSAW DOUBLE PRECISION GSTOR,HSTOR DOUBLE PRECISION XSTOR,BSTOR,ISTOR INTEGER RECALT,ISAW COMMON /SAW1/ + GSTOR(NMAX2,NSAW),HSTOR(NMAX,NSAW), + XSTOR(NMAX,NSAW),BSTOR(NSAW),ISTOR(NSAW), + RECALT(NSAW),ISAW,TSAW C INTEGER ND,TYPE,BAND,I,TRIG REAL FB,Q,RIP,IN,OUT DOUBLE PRECISION D(NMAX1),N(NMAX1),XINIT(NMAX) DOUBLE PRECISION DQ,WB,DRIP C C DOUBLE PRECISION STOR MANAGER: C IF(TIME.LT.DELT) THEN IF(ISAW.LT.1.OR.ISAW.GT.NSAW) THEN TSAW=TIME ISAW=1 ELSE ISAW=ISAW+1 ENDIF ELSEIF(TSAW.LT.TIME) THEN TSAW=TIME ISAW=1 ELSE ISAW=ISAW+1 ENDIF IF(ISAW.GT.NSAW) THEN WRITE(IOUT,117) NSAW WRITE(IOUT,118) CALL OUTMSG(401,0,0,0,0.0,0.0,'TFDP10 ERROR') STOP 117 FORMAT(1X,'A MAXIMUM NUMBER OF ',I3) 118 FORMAT(1X,'TRANSFER FUNCTION BLOCKS MAY BE USED.') ENDIF C C TEST TO DETERMINE WHETHER OR NOT TO C REEVALUATE MATRICES C TRIG=0 IF(TIME.LT.DELT) THEN STOR(NEXC+1) = FB STOR(NEXC+2) = RIP STOR(NEXC+3) = Q TRIG=1 ELSE IF(ABS(FB-STOR(NEXC+1)).GT.1.0E-6) THEN TRIG=1 STOR(NEXC+1) = FB ENDIF IF(ABS(RIP-STOR(NEXC+2)).GT.1.0E-6) THEN TRIG=1 STOR(NEXC+2) = RIP ENDIF IF(ABS(Q-STOR(NEXC+3)).GT.1.0E-6) THEN TRIG=1 STOR(NEXC+3) = Q ENDIF ENDIF NEXC = NEXC+5 C C REEVALUATE MATRICES FOLLOWING SNAPSHOT AND AT STARTUP C IF(RECALT(ISAW).NE.23456789) THEN TRIG=1 RECALT(ISAW)=23456789 ENDIF C C DETERMINE THE FILTER COEFFICIENTS C IF(TRIG.EQ.1) THEN DQ = DBLE(Q) DRIP = DBLE(RIP) WB = FB*6.283185308 CALL CHLPFC(ND,DRIP,D,N,WB,DQ,TYPE,BAND) DO 10 I=1,NMAX XINIT(I) = 0.0 10 CONTINUE ENDIF C C PERFORM TRANSFER FUNCTION CALCULATIONS C CALL TFDP10(ND,N,D,IN,OUT,XINIT,MODE,TRIG) C RETURN END C C ============================================================= SUBROUTINE TFUN35(ND,N,D,IN,OUT,XINIT,MODE,WB) C C TO CONTROL TFDP10 IN DOUBLE PRECISION AS A TRANSFER FUNCTION. C BY SCOTT WOODFORD C 9 AUGUST 1994 C ============================================================= C INCLUDE 'emt.d' INTEGER ICH,NEXC REAL TIME,DELT,PRINT,FINTIM,STOR COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /IO/IIN,IOUT C INTEGER NMAX,NMAX1,NMAX2 PARAMETER(NMAX=10,NMAX1=11,NMAX2=100) C INTEGER NSAW PARAMETER(NSAW=10) REAL TSAW DOUBLE PRECISION GSTOR,HSTOR DOUBLE PRECISION XSTOR,BSTOR,ISTOR INTEGER RECALT,ISAW COMMON /SAW1/ + GSTOR(NMAX2,NSAW),HSTOR(NMAX,NSAW), + XSTOR(NMAX,NSAW),BSTOR(NSAW),ISTOR(NSAW), + RECALT(NSAW),ISAW,TSAW C INTEGER ND,MODE,I,TRIG REAL N(NMAX1),D(NMAX1),IN,OUT,XINIT(NMAX),WB DOUBLE PRECISION DDP(NMAX1),NDP(NMAX1),XINITD(NMAX),WBDP C C DOUBLE PRECISION STOR MANAGER: C IF(TIME.LT.DELT) THEN IF(ISAW.LT.1.OR.ISAW.GT.NSAW) THEN TSAW=TIME ISAW=1 ELSE ISAW=ISAW+1 ENDIF ELSEIF(TSAW.LT.TIME) THEN TSAW=TIME ISAW=1 ELSE ISAW=ISAW+1 ENDIF IF(ISAW.GT.NSAW) THEN WRITE(IOUT,117) NSAW WRITE(IOUT,118) CALL OUTMSG(401,0,0,0,0.0,0.0,'TFDP10 ERROR') STOP 117 FORMAT(1X,'A MAXIMUM NUMBER OF ',I3) 118 FORMAT(1X,'TRANSFER FUNCTION BLOCKS MAY BE USED.') ENDIF C C TEST TO DETERMINE WHETHER OR NOT TO C REEVALUATE MATRICES C TRIG=0 IF(TIME.LT.DELT) THEN DO 15 I=1,ND STOR(NEXC+I)=N(I) STOR(NEXC+NMAX1+I)=D(I) 15 CONTINUE STOR(NEXC+NMAX1*2+1)=WB TRIG=1 ELSE DO 45 I=1,ND IF(ABS(N(I)-STOR(NEXC+I)).GT.1.0E-6) THEN TRIG=1 STOR(NEXC+I)=N(I) ENDIF IF(ABS(D(I)-STOR(NEXC+NMAX1+I)).GT.1.0E-6) THEN TRIG=1 STOR(NEXC+NMAX1+I)=D(I) ENDIF 45 CONTINUE IF(ABS(WB-STOR(NEXC+NMAX1*2+1)).GT.1.0E-6) THEN TRIG=1 STOR(NEXC+NMAX1*2+1)=WB ENDIF ENDIF NEXC = NEXC+25 C C REEVALUATE MATRICES FOLLOWING SNAPSHOT AND AT STARTUP C IF(RECALT(ISAW).NE.23456789) THEN TRIG=1 RECALT(ISAW)=23456789 ENDIF C IF(TRIG.EQ.1) THEN DO 10 I=1,NMAX XINITD(I) = XINIT(I) NDP(I) = N(I) DDP(I) = D(I) 10 CONTINUE NDP(NMAX1) = N(NMAX1) DDP(NMAX1) = D(NMAX1) WBDP = WB CALL CSCALE(NDP,DDP,ND,WBDP,1) ENDIF C C PERFORM TRANSFER FUNCTION CALCULATIONS C CALL TFDP10(ND,NDP,DDP,IN,OUT,XINITD,MODE,TRIG) C RETURN END C ============================================================= SUBROUTINE CHLPFC(M,RIPPLE,DVEC,NVEC,WBASE,Q,FTYPE,BTYPE) C C THIS SUBROUTINE CALCULATES CHEBYSHEV AND BUTTERWORTH C FILTER POLYNOMIAL COEFFICIENTS IN DOUBLE PRECISION C BY SCOTT WOODFORD C 9 AUGUST 1994 C ============================================================= C ARGUMENTS: C C M ORDER OF FILTER C RIPPLE PASSBAND RIPPLE OF CHEBYSHEV FILTER (DB) C NVEC COEFFICIENTS OF THE NUMERATOR POLYNOMIAL: C NVEC(1) IS THE CONSTANT COEFFICIENT C NVEC(M) IS THE HIGHEST ORDER COEFFICIENT (MAY BE ZERO) C DVEC COEFFICIENTS OF THE DENOMINATOR POLYNOMIAL: C DVEC(1) IS THE CONSTANT COEFFICIENT C DVEC(M) IS THE HIGHEST ORDER COEFFICIENT C WBASE CUTOFF OR CENTRE FREQUENCY (RAD/S) C Q Q FACTOR (IGNORED FOR HIGHPASS AND LOWPASS FILTERS) C FTYPE=1 BUTTERWORTH FILTER C FTYPE=2 CHEBYSHEV FILTER C BTYPE=1 LOWPASS FILTER C BTYPE=2 HIGHPASS FILTER C BTYPE=3 BANDPASS FILTER C BTYPE=4 BANDSTOP FILTER C ============================================================= INTEGER NMAX,NMAX1,NMAX2 PARAMETER(NMAX=10,NMAX1=11,NMAX2=100) C INTEGER N,I,FTYPE,BTYPE,M DOUBLE PRECISION DVEC(NMAX1),NVEC(NMAX1),PI DOUBLE PRECISION INVN,K,RIPPLE,WBASE,Q,INVB,BW DOUBLE PRECISION WCOR,INVE,DUM,SINHV,COSHV,THETA DOUBLE PRECISION A(2,NMAX),B(2,NMAX),C(2,NMAX),ROOTS(2,NMAX) DOUBLE PRECISION DUMC(2),DUM2(2) C COMMON /IO/IIN,IOUT C C CALCULATE NORMALIZED LOWPASS POLES: C IF(BTYPE.LE.2) N=M IF(BTYPE.GE.3) N=M/2 PI = 3.141592654 INVN = 1.0/DBLE(FLOAT(N)) K = 1.0 IF(FTYPE.EQ.2) THEN INVE = 1.0/SQRT(10.0**(0.1*RIPPLE)-1) IF(MOD(N,2).EQ.0) K = 10.0**(-0.05*RIPPLE) DUM = (SQRT(1.0+INVE*INVE)+INVE)**INVN SINHV = 0.5*(DUM-1.0/DUM) COSHV = 0.5*(DUM+1.0/DUM) WCOR = COSH(INVN*LOG(INVE+SQRT(INVE*INVE-1.0))) IF(BTYPE.EQ.1) WCOR = 1.0/WCOR ELSEIF(FTYPE.EQ.1) THEN SINHV = 1.0 COSHV = 1.0 WCOR = 1.0 ENDIF DO 10 I=1,N THETA = PI*((DBLE(FLOAT(I))-0.5)*INVN+0.5) ROOTS(1,I) = COS(THETA)*SINHV ROOTS(2,I) = SIN(THETA)*COSHV 10 CONTINUE C DO 15 I=1,NMAX1 DVEC(I) = 0.0 NVEC(I) = 0.0 15 CONTINUE IF(BTYPE.LE.2) THEN CALL POLYCL(ROOTS,DVEC,N) NVEC(1) = DVEC(1)*K CALL CSCALE(NVEC,DVEC,N,WBASE*WCOR,BTYPE) ELSEIF(BTYPE.EQ.3) THEN IF(MOD(M,2).NE.0) THEN WRITE(IOUT,103) WRITE(IOUT,104) M CALL OUTMSG(401,0,0,0,0.0,0.0,'FILT15/CHLPFC WARNING') 103 FORMAT(/,1X,'WARNING: BANDPASS OR BANDSTOP FILTER ORDER') 104 FORMAT(1X,'MUST BE AN EVEN NUMBER; CHANGE ORDER FROM ',I2) STOP ENDIF INVB = Q/WBASE DO 20 I=1,N A(1,I) = WCOR*INVB A(2,I) = 0.0 B(1,I) = -ROOTS(1,I) B(2,I) = -ROOTS(2,I) C(1,I) = WCOR*Q*WBASE C(2,I) = 0.0 20 CONTINUE CALL POLYQD(A,B,C,DVEC,N) DUMC(1) = 1.0 DUMC(2) = 0.0 DO 30 I=1,N CALL MULDPC(DUMC(1),ROOTS(1,I),DUM2(1)) DUMC(1) = DUM2(1) DUMC(2) = DUM2(2) 30 CONTINUE NVEC(N+1) = DUMC(1)*K ELSEIF(BTYPE.EQ.4) THEN IF(MOD(M,2).NE.0) THEN WRITE(IOUT,103) WRITE(IOUT,104) M CALL OUTMSG(401,0,0,0,0.0,0.0,'FILT15/CHLPFC WARNING') STOP ENDIF BW = WBASE/Q DO 40 I=1,N A(1,I) = -ROOTS(1,I) A(2,I) = -ROOTS(2,I) B(1,I) = WCOR*BW B(2,I) = 0.0 C(1,I) = -ROOTS(1,I)*WBASE*WBASE C(2,I) = -ROOTS(2,I)*WBASE*WBASE 40 CONTINUE CALL POLYQD(A,B,C,DVEC,N) DO 50 I=1,N A(1,I) = 1.0 A(2,I) = 0.0 B(1,I) = 0.0 B(2,I) = 0.0 C(1,I) = WBASE*WBASE C(2,I) = 0.0 50 CONTINUE CALL POLYQD(A,B,C,NVEC,N) DUMC(1) = 1.0 DUMC(2) = 0.0 DO 60 I=1,N CALL MULDPC(DUMC(1),ROOTS(1,I),DUM2(1)) DUMC(1) = DUM2(1) DUMC(2) = DUM2(2) 60 CONTINUE DO 70 I=1,M+1 NVEC(I) = NVEC(I)*DUMC(1)*K 70 CONTINUE ENDIF C RETURN END C ============================================================= SUBROUTINE POLYCL(ROOTS,POLY,N) C C THIS SUBROUTINE CALCULATES THE REAL COEFFICIENTS C OF A POLYNOMIAL WITH N DOUBLE PRECISION C COMPLEX ROOTS C C POLY(I,1) IS THE CONSTANT COEFFICIENT C POLY(I,N+1) IS THE HIGHEST ORDER COEFFICIENT C ============================================================= INTEGER NMAX,NMAX1,NMAX2 PARAMETER(NMAX=10,NMAX1=11,NMAX2=100) C INTEGER N,I,J DOUBLE PRECISION POLY(NMAX1) DOUBLE PRECISION POLYC(2,NMAX1),ROOTS(2,NMAX) DOUBLE PRECISION DUMC(2),DUM2(2) C DO 20 I=1,NMAX1 POLYC(1,I) = 0.0 POLYC(2,I) = 0.0 20 CONTINUE POLYC(1,N+1) = 1.0 POLYC(1,N) = -ROOTS(1,1) POLYC(2,N) = -ROOTS(2,1) DO 30 I=2,N DO 40 J=N+1-I,N CALL MULDPC(ROOTS(1,I),POLYC(1,J+1),DUMC(1)) CALL SUBDPC(POLYC(1,J),DUMC(1),DUM2(1)) POLYC(1,J) = DUM2(1) POLYC(2,J) = DUM2(2) 40 CONTINUE 30 CONTINUE DO 50 I=1,NMAX1 POLY(I) = POLYC(1,I) 50 CONTINUE C RETURN END C ============================================================= SUBROUTINE POLYQD(A,B,C,POLY,N) C C THIS SUBROUTINE CALCULATES THE REAL COEFFICIENTS C OF A POLYNOMIAL WITH N DOUBLE PRECISION C COMPLEX QUADRATIC FACTORS C C A*S^2 + B*S + C C C A IS THE ARRAY OF COEFFICIENTS IN S^S C B IS THE ARRAY OF COEFFICIENTS IN S C C IS THE ARRAY OF CONSTANT COEFFICIENTS C POLY(I,1) IS THE CONSTANT COEFFICIENT: C POLY(1,1) IS ITS REAL PART C POLY(2,1) IS ITS IMAGINARY PART C POLY(I,2*N+1) IS THE HIGHEST ORDER COEFFICIENT C ============================================================= INTEGER NMAX,NMAX1,NMAX2 PARAMETER(NMAX=10,NMAX1=11,NMAX2=100) C INTEGER N,I,J,M DOUBLE PRECISION A(2,N),B(2,N),C(2,N),POLYC(2,NMAX1) DOUBLE PRECISION DUM(2,NMAX1),DUM1(2),DUM2(2) DOUBLE PRECISION POLY(NMAX1) C M = N*2+1 DO 10 I=1,NMAX1 POLYC(1,I) = 0.0 POLYC(2,I) = 0.0 10 CONTINUE POLYC(1,1) = C(1,1) POLYC(2,1) = C(2,1) POLYC(1,2) = B(1,1) POLYC(2,2) = B(2,1) POLYC(1,3) = A(1,1) POLYC(2,3) = A(2,1) DO 20 I=2,N DO 40 J=1,M DUM(1,J) = 0.0 DUM(2,J) = 0.0 40 CONTINUE DO 30 J=1,M-2 CALL MULDPC(POLYC(1,J),C(1,I),DUM1(1)) CALL ADDDPC(DUM1(1),DUM(1,J),DUM2(1)) DUM(1,J) = DUM2(1) DUM(2,J) = DUM2(2) CALL MULDPC(POLYC(1,J),B(1,I),DUM1(1)) CALL ADDDPC(DUM1(1),DUM(1,J+1),DUM2(1)) DUM(1,J+1) = DUM2(1) DUM(2,J+1) = DUM2(2) CALL MULDPC(POLYC(1,J),A(1,I),DUM1(1)) CALL ADDDPC(DUM1(1),DUM(1,J+2),DUM2(1)) DUM(1,J+2) = DUM2(1) DUM(2,J+2) = DUM2(2) 30 CONTINUE DO 50 J=1,M POLYC(1,J) = DUM(1,J) POLYC(2,J) = DUM(2,J) 50 CONTINUE 20 CONTINUE DO 60 I=1,NMAX1 POLY(I) = POLYC(1,I) 60 CONTINUE RETURN END C ============================================================= C NTH ORDER TRANSFER FUNCTION EMTDC BLOCK C C ============================================================= C SIMULATION OF A NTH ORDER TRANSFER FUNCTION C BASED ON ORIGINAL FORTRAN CODES BY: C IONI FERNANDO 12 MAY 1992 C ANI GOLE 23 SEPTEMBER 1993 C BY SCOTT WOODFORD C 9 AUGUST 1994 C ============================================================= C SUBROUTINE TFDP10(ND,N,D,IN,OUT,XINIT,MODE,TRIG) C C ============================================================= C ARGUMENTS: C C ND ORDER OF DENOMINATOR POLYNOMIAL C N COEFFICIENTS OF THE NUMERATOR POLYNOMIAL: C N(1) IS THE CONSTANT COEFFICIENT C N(ND) IS THE HIGHEST ORDER COEFFICIENT (MAY BE ZERO) C D COEFFICIENTS OF THE DENOMINATOR POLYNOMIAL: C D(1) IS THE CONSTANT COEFFICIENT C D(ND) IS THE HIGHEST ORDER COEFFICIENT C IN INPUT SIGNAL C OUT OUTPUT SIGNAL C XINIT(ND) INITIAL STATE VARIABLE CONDITIONS C MODE=0 SIMPLIFIED TRAPEZOIDAL INTEGRATION C MODE=1 TRAPEZOIDAL INTEGRATION C TRIG=1 FORCES RECALCULATION OF MATRICES C ============================================================= C INCLUDE 'emt.d' INTEGER ICH,NEXC REAL TIME,DELT,PRINT,FINTIM,STOR COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC COMMON /IO/IIN,IOUT C INTEGER NMAX,NMAX1,NMAX2 PARAMETER(NMAX=10,NMAX1=11,NMAX2=100) C INTEGER NSAW PARAMETER(NSAW=10) REAL TSAW DOUBLE PRECISION GSTOR,HSTOR DOUBLE PRECISION XSTOR,BSTOR,ISTOR INTEGER RECALT,ISAW COMMON /SAW1/ + GSTOR(NMAX2,NSAW),HSTOR(NMAX,NSAW), + XSTOR(NMAX,NSAW),BSTOR(NSAW),ISTOR(NSAW), + RECALT(NSAW),ISAW,TSAW C INTEGER ND,MODE,TRIG,ND1 DOUBLE PRECISION N(NMAX1),D(NMAX1),XINIT(NMAX) REAL IN,OUT C INTEGER I,J DOUBLE PRECISION BETO,DELT2,DUM DOUBLE PRECISION APDT(NMAX,NMAX),AMDT(NMAX,NMAX),B(NMAX) DOUBLE PRECISION X(NMAX),XLEFT(NMAX),XRIGHT(NMAX) DOUBLE PRECISION G(NMAX,NMAX),H(NMAX),U DOUBLE PRECISION BETA(NMAX1),AMDTI(NMAX,NMAX) C C START OF EXECUTABLE CODE C C C INITIALIZATION ROUTINE C IF(TIME.LT.DELT.OR.TRIG.EQ.1) THEN C C ARRAY SIZE ERROR CHECK: C IF(ND.GT.NMAX) THEN WRITE(IOUT,111) WRITE(IOUT,116) WRITE(IOUT,117) NMAX WRITE(IOUT,118) WRITE(IOUT,119) CALL OUTMSG(401,0,0,0,0.0,0.0,'TFDP10 ERROR') STOP 111 FORMAT(/,1X,'TRANSFER FUNCTION:') 116 FORMAT(1X,'ARRAY OVERFLOW: ORDER OF DENOMINATOR') 117 FORMAT(1X,'CANNOT BE GREATER THAN ',I3,'.') 118 FORMAT(1X,'TRY FACTORING THE TRANSFER FUNCTION AND') 119 FORMAT(1X,'USE TWO OR MORE CASCADED BLOCKS.') ENDIF C C INITIALIZING MATRICES C DO 10 I=1,ND H(I) = 0.0 XLEFT(I) = 0.0 XRIGHT(I) = 0.0 DO 20 J=1,ND APDT(I,J)= 0.0 AMDT(I,J)= 0.0 G(I,J) = 0.0 20 CONTINUE 10 CONTINUE ND1=ND+1 DO 25 I=1,ND1 BETA(I) = 0.0 25 CONTINUE C C INITIALIZE X C IF(TIME.LT.DELT) THEN DO 35 I=1,ND STOR(NEXC+I)=XINIT(I) 35 CONTINUE ENDIF C C REVERSING THE COEFFICIENT MATRICES C DO 340 I=1,ND1/2 DUM=N(I) N(I)=N(ND1+1-I) N(ND1+1-I)=DUM DUM=D(I) D(I)=D(ND1+1-I) D(ND1+1-I)=DUM 340 CONTINUE C C SCALING THE COEFFICIENT MATRICES C DO 55 I=1,ND1 N(I)=N(I)/D(1) 55 CONTINUE DO 65 I=2,ND1 D(I)=D(I)/D(1) 65 CONTINUE D(1)=1.0 C C MATRIX FORMATION: C C MATRICES I-A*DT/2 AND I+A*DT/2 C DELT2=DELT*0.5 DO 30 I=1,ND-1 APDT(I,I)=1.0 AMDT(I,I)=1.0 APDT(I,I+1)=DELT2 AMDT(I,I+1)=-DELT2 30 CONTINUE DO 40 I=1,ND APDT(ND,I)=-D(ND+2-I)*DELT2 AMDT(ND,I)=D(ND+2-I)*DELT2 40 CONTINUE APDT(ND,ND)=1.0+APDT(ND,ND) AMDT(ND,ND)=1.0+AMDT(ND,ND) C C INVERTING AMDT: C CALL SWMINV(AMDT,AMDTI,ND,NMAX) C C ARRAY B C DO 310 I=1,ND1 BETA(I)=N(I) DO 320 J=2,I BETA(I)=BETA(I)-(D(J)*BETA(I+1-J)) 320 CONTINUE 310 CONTINUE BETO=BETA(1) DO 130 I=1,ND B(I)=BETA(I+1) 130 CONTINUE C C MATRIX G C DO 440 I=1,ND DO 450 K=1,ND G(I,K) = 0.0 DO 460 J=1,ND G(I,K) = G(I,K)+AMDTI(I,J)*APDT(J,K) 460 CONTINUE 450 CONTINUE 440 CONTINUE C C ARRAY H C DO 420 I=1,ND H(I) = 0.0 DO 430 J=1,ND H(I) = H(I)+AMDTI(I,J)*B(J)*DELT 430 CONTINUE 420 CONTINUE C C INITIAL PUSHES TO STOR AND DOUBLE PRECISION STORS C DO 155 I=1,ND DO 165 J=1,ND GSTOR(J+(I-1)*ND,ISAW)=G(I,J) 165 CONTINUE HSTOR(I,ISAW)=H(I) 155 CONTINUE BSTOR(ISAW)=BETO C ENDIF C C THE ITERATION LOOP C C PULLS FROM DOUBLE PRECISION STORS C DO 175 I=1,ND DO 185 J=1,ND G(I,J)=GSTOR(J+(I-1)*ND,ISAW) 185 CONTINUE H(I)=HSTOR(I,ISAW) X(I)=XSTOR(I,ISAW) 175 CONTINUE BETO=BSTOR(ISAW) C C PULL STATE VECTOR FROM STOR AT MATRIX REEVALUATION C OR AFTER SNAPSHOT C IF(TRIG.EQ.1) THEN DO 405 I=1,ND X(I) = STOR(NEXC+I) 405 CONTINUE ENDIF C C CALCULATION SEGMENT C DO 400 I=1,ND XLEFT(I) = 0.0 DO 410 J=1,ND XLEFT(I) = XLEFT(I)+G(I,J)*X(J) 410 CONTINUE 400 CONTINUE C IF(MODE.EQ.0) THEN U=IN ELSEIF(MODE.EQ.1) THEN U=(IN+ISTOR(ISAW))*0.5 ISTOR(ISAW)=IN ENDIF C DO 220 I=1,ND XRIGHT(I)=H(I)*U 220 CONTINUE DO 230 I=1,ND X(I)=XLEFT(I)+XRIGHT(I) 230 CONTINUE OUT=X(1)+BETO*IN C C PUSHES TO STOR AND DOUBLE PRECISION STOR C DO 225 I=1,ND XSTOR(I,ISAW)=X(I) STOR(NEXC+I)=X(I) 225 CONTINUE C NEXC = NEXC+NMAX C RETURN END C ============================================================= SUBROUTINE SWMINV(A,AINV,N,NSIZE) C C THIS SUBROUTINE INVERTS THE MATRIX A OF SIZE N C STORED IN A MATRIX OF SIZE NSIZE < NMAX. C IT MAKES USE OF MINV IN SVC400.F FOR THE ACTUAL INVERSION. C ============================================================= INTEGER NMAX,NMAX1,NMAX2 PARAMETER(NMAX=10,NMAX1=11,NMAX2=100) C INTEGER N,L(NMAX),M(NMAX),I,J,JJ,NSIZE DOUBLE PRECISION A(NSIZE,NSIZE),AINV(NSIZE,NSIZE) DOUBLE PRECISION AD(NMAX2),DETDUM C JJ=0 DO 10 I=1,N DO 20 J=1,N JJ=JJ+1 AD(JJ)=A(I,J) 20 CONTINUE 10 CONTINUE CALL MINV(AD,N*N,N,DETDUM,L,M) JJ=0 DO 30 I=1,N DO 40 J=1,N JJ=JJ+1 AINV(I,J)=AD(JJ) 40 CONTINUE 30 CONTINUE C RETURN END C ============================================================= SUBROUTINE CSCALE(NVEC,DVEC,N,WBASE,TYPE) C C THIS SUBROUTINE MULTIPLIES EACH COEFFICIENT BY A SCALING C FACTOR, AND CONVERTS LOWPASS FILTERS TO HIGHPASS. C BY SCOTT WOODFORD C 9 AUGUST 1994 C ============================================================= C ARGUMENTS: C C NVEC COEFFICIENTS OF THE NUMERATOR POLYNOMIAL: C NVEC(1) IS THE CONSTANT COEFFICIENT C NVEC(N) IS THE HIGHEST ORDER COEFFICIENT (MAY BE ZERO) C DVEC COEFFICIENTS OF THE DENOMINATOR POLYNOMIAL: C DVEC(1) IS THE CONSTANT COEFFICIENT C DVEC(N) IS THE HIGHEST ORDER COEFFICIENT C N ORDER OF DENOMINATOR POLYNOMIAL C WBASE FREQUENCY SCALED TO 1 RAD/S (IN RAD/S). C TYPE=1 STANDARD (LOWPASS) SCALING C TYPE=2 HIGHPASS TRANSFORMATION AND SCALING C ============================================================= INTEGER N,I,J,TYPE DOUBLE PRECISION NVEC(N+1),DVEC(N+1),WBASE,SFACT,DUM C IF(TYPE.EQ.1) THEN SFACT=1.0/WBASE DO 30 I=2,N+1 DO 20 J=I,N+1 NVEC(J)=NVEC(J)*SFACT DVEC(J)=DVEC(J)*SFACT 20 CONTINUE 30 CONTINUE ELSEIF(TYPE.EQ.2) THEN DO 60 I=2,N+1 DO 70 J=I,N+1 NVEC(J)=NVEC(J)*WBASE DVEC(J)=DVEC(J)*WBASE 70 CONTINUE 60 CONTINUE DO 40 I=1,(N+1)/2 DUM=NVEC(I) NVEC(I)=NVEC(N+2-I) NVEC(N+2-I)=DUM DUM=DVEC(I) DVEC(I)=DVEC(N+2-I) DVEC(N+2-I)=DUM 40 CONTINUE ENDIF C RETURN END C C ============================================================= SUBROUTINE ADDDPC(A,B,C) C C THIS SUBROUTINE ADDS DOUBLE PRECISION COMPLEX ARRAYS C ============================================================= DOUBLE PRECISION A(2),B(2),C(2) C C(1)=A(1)+B(1) C(2)=A(2)+B(2) C RETURN END C ============================================================= SUBROUTINE SUBDPC(A,B,C) C C THIS SUBROUTINE SUBTRACTS DOUBLE PRECISION COMPLEX ARRAYS C ============================================================= DOUBLE PRECISION A(2),B(2),C(2) C C(1)=A(1)-B(1) C(2)=A(2)-B(2) C RETURN END C ============================================================= SUBROUTINE MULDPC(A,B,C) C C THIS SUBROUTINE MULTIPLIES DOUBLE PRECISION COMPLEX ARRAYS C ============================================================= DOUBLE PRECISION A(2),B(2),C(2) C C(1)=A(1)*B(1)-A(2)*B(2) C(2)=A(1)*B(2)+A(2)*B(1) C RETURN END C************************************************************************* C SUBROUTINE FOR SIMULATION OF PHASE LOCKED LOOP. C VECTOR MEASURMENT OF PHASE IS USED. C Dr. A. Gole , UNIVERSITY OF MANITOBA C************************************************************************* C SUBROUTINE TVEKT3(VA,VB,VC,GP,GI,VBAS,FBAS,OMEGA,THETA,ERRDEG) INCLUDE 'emt.d' INCLUDE 'emt.e' COMMON /S1/TIME,DELT,ICH,PRINT,FINTIM COMMON /S2/STOR(ND10),NEXC REAL TIME,DELT,PRINT,FINTIM REAL STOR,GP,GI,VA,VB,VC INTEGER NEXC,ICH REAL THETA,VBAS,FBAS,VBAS1 REAL VSIN,VCOS,VALPHA,VBETA,ERR,OMEG0,TLO,THI,OMEGA REAL LIMIT C C VBAS & FBAS ARE L-L NOMINAL VOLTS AND FREQ. RESPECTIVELY. C VBAS IS NOT CRITICAL, AS LONG AS IT IS REASONABLE. IF(TIME.LT.DELT) STOR(NEXC+3) = 1.0/VBAS VBAS1 = STOR(NEXC+3) THETA = STOR(NEXC+1) VSIN = SIN(THETA) VCOS = COS(THETA) VALPHA = (VA-0.5*(VB+VC)) * 0.66666667 * 1.22474487 * VBAS1 VBETA = (VB-VC) * 0.57735027 * 1.22474487 * VBAS1 ERR = VCOS * VALPHA + VSIN * VBETA IF(ABS(ERR).GE.1E-10) THEN SINERR = ERR/AMAX1((VALPHA*VALPHA+VBETA*VBETA),ABS(ERR)) ELSE SINERR = 0.0 ENDIF ERRDEG = ASIN(SINERR)*57.29578 C C ERRDEG IS THE ERROR IN PHASE BETWEEN THETA AND THE C POSITIVE SEQ. FUNDAMENTAL VOLTAGE. THE ACTUAL ERROR C ERR USED IN THE LOOP CONTROL PROCESS IS PROPORTIONAL TO C THE SINE OF THIS. C STOR(NEXC+2) = STOR(NEXC+2) + ERR * DELT OMEG0 = 6.28318531 * FBAS + GI * STOR(NEXC+2) + GP * ERR TLO = 6.28318531 * FBAS * 0.8 THI = 6.28318531 * FBAS * 1.20 OMEGA = LIMIT(TLO,THI,OMEG0) THETA = THETA + OMEGA * DELT IF ( THETA .GE. 6.28318531 ) THETA = THETA - 6.28318531 STOR(NEXC+1) = THETA C CONVERT TO HERTZ OMEGA = OMEGA * 0.15915494 NEXC = NEXC + 3 RETURN END C C ============================================================= SUBROUTINE TVPULS(THETA,PMODE) C C THIS SUBROUTINE MAKES A 6-PHASE ANGLE VECTOR C ============================================================= C ARGUMENTS: C C THETA(1) INPUT PHASE ANGLE C PMODE=0 OUTPUT IS IN RADIANS C PMODE=1 OUTPUT IS IN DEGREES C C THETA(N) OUTPUT = THETA(1) - N*60 DEGREES C ============================================================= C REAL THETA(6) INTEGER I,PMODE C IF(PMODE.EQ.0) THEN DO 10 I=2,6 THETA(I) = THETA(I-1)-1.04719755 IF(THETA(I).LT.0) THETA(I) = THETA(I)+6.2831853 10 CONTINUE ELSE THETA(1) = THETA(1)*57.29578 DO 20 I=2,6 THETA(I) = THETA(I-1)-60.0 IF(THETA(I).LT.0) THETA(I) = THETA(I)+360.0 20 CONTINUE ENDIF RETURN END C C ============================================================= C REAL FUNCTION APLIN(N,XC,YC,X) C C ============================================================= C LINEARLY INTERPOLATED NON-LINEAR TRANSFER CHARACTERISTIC C C BY A.M.GOLE C 1989 C ============================================================= C ARGUMENTS: C N NUMBER OF DATA POINTS IN TRANSFER CHARACTERISTIC C XC(N) X COORDINATES OF DATA POINTS (MUST BE INCREASING FROM C XC(1) TO XC(N). C YC(N) Y COORDINATES OF DATA POINTS C X INPUT C APLIN LINEARLY INTERPOLATED OUTPUT C ============================================================= C INTEGER I,N REAL X,Y,XC(N),YC(N) C DO 10 I=2,N IF(X.LE.XC(I)) THEN DEN = XC(I)-XC(I-1) C IF(DEN.EQ.0.0) THEN C DO SOMETHING TO STOP C (THIS SITUATION SHOULD HAVE BEEN PREVENTED BY DRAFT) C ENDIF UNM = YC(I)-YC(I-1) Y = YC(I-1)+UNM/DEN*(X-XC(I-1)) GOTO 20 ENDIF 10 CONTINUE C DEN = XC(N)-XC(N-1) C IF(DEN.EQ.0.0) THEN C DO SOMETHING TO STOP C (THIS SITUATION SHOULD HAVE BEEN PREVENTED BY DRAFT) C ENDIF UNM = YC(N)-YC(N-1) Y = YC(N-1)+UNM/DEN*(X-XC(N-1)) 20 APLIN = Y C RETURN END C ============================================================= C C INTERPOLATING SIGNAL SAMPLER EMTDC BLOCK C C ============================================================= C C BASED ON ORIGINAL FORTRAN CODE BY ROHITHA JAYASINGHE C 23 APRIL 1992 C FINAL VERSION BY SCOTT WOODFORD C 1 SEPTEMBER 1994 C ============================================================= C REAL FUNCTION SAMP5(IN,F,TRIG,DT,MODE) C C ============================================================= C ARGUMENTS: C C IN INPUT SIGNAL C F SAMPLING FREQUENCY (MODE=0 ONLY) C TRIG SAMPLE TRIGGER C DT DT FOR INTERPOLATION (MODE=1 ONLY): C DT = TIME - TIME OF PULSE C MODE=0 USE INPUT FREQUENCY C MODE=1 USE INPUT PULSE TRAIN C ============================================================= C INCLUDE 'emt.d' C INTEGER ICH,NEXC,IIN,IOUT REAL TIME,DELT,PRINT,FINTIM,STOR COMMON/S1/TIME,DELT,ICH,PRINT,FINTIM COMMON/S2/STOR(ND10),NEXC COMMON/IO/IIN,IOUT C REAL IN,F,DT,TMNSMP,FINTRP,INPOLD,FCHANGED INTEGER TRIG,MODE,IFLAG C C FREQUENCY-BASED SAMPLING: C FFLAG = 0 C CALCULATE SAMPLING TIME (1/F) ONLY IF SAMPLING FREQUENCY CHANGED IF ( ( STOR(NEXC+5) .NE. F ) .OR. (TIME.LT.DELT) ) THEN C PREVENT DIVISION BY ZERO IF (F .GE. 1.0E-10) THEN STOR(NEXC+4) = 1.0/F C SAMPLIING TIME CANNOT BE LESS THAN SIMULATION TIME STEP IF ( STOR(NEXC+4) .LT. DELT) STOR(NEXC+4) = DELT ELSE STOR(NEXC+4) = 1.0E+10 ENDIF STOR(NEXC+5) = F IFLAG= 1 ENDIF C IF(MODE.EQ.0) THEN IF(TIME.LT.DELT) THEN STOR(NEXC+2) = IN STOR(NEXC+3) = IN ENDIF TMNSMP = STOR(NEXC+1) + STOR(NEXC+4) IF(TIME.GE.TMNSMP) THEN C DO NOT INTERPOLATE IF THE SAMPLING FREQUENCY CHANGED JUST NOW IF( IFLAG .NE. 1) THEN FINTRP = (TIME-TMNSMP)/DELT INPOLD = STOR(NEXC+2) STOR(NEXC+3) = IN+FINTRP*(INPOLD-IN) ELSE STOR(NEXC+3) = IN ENDIF STOR(NEXC+1) = TMNSMP ENDIF ELSE C C PULSE-BASED SAMPLING C IF(TIME.LT.DELT) THEN STOR(NEXC+2) = IN STOR(NEXC+3) = IN ENDIF IF((TRIG.GE.1)) THEN IF((DT.GE.0.0).AND.(DT.LT.DELT)) THEN FINTRP = DT/DELT INPOLD = STOR(NEXC+2) STOR(NEXC+3) = IN+FINTRP*(INPOLD-IN) ENDIF ENDIF ENDIF C STOR(NEXC+2) = IN SAMP5 = STOR(NEXC+3) NEXC = NEXC+5 C RETURN END