www.pudn.com > rtu.rar > FREQ32.A96
; File name =FREQ32.A96
; compile with model(nt-ef) control
; #pragma model(nt-ef)
; mk196
; 设计 何风涛
RSEG
R0 EQU 00000H;;;;;;;FAST;
WATCHDOG EQU 0000AH;;;;;;;FAST;
WSR EQU 00014H;;;;;;;FAST;
REAL00 EQU 00080H;;;;;;;FAST;
IMAG00 EQU 00082H;;;;;;;FAST;
REAL01 EQU 00084H;;;;;;;FAST;
IMAG01 EQU 00086H;;;;;;;FAST;
REAL02 EQU 00088H;;;;;;;FAST;
IMAG02 EQU 0008AH;;;;;;;FAST;
REAL03 EQU 0008CH;;;;;;;FAST;
IMAG03 EQU 0008EH;;;;;;;FAST;
REAL04 EQU 00090H;;;;;;;FAST;
IMAG04 EQU 00092H;;;;;;;FAST;
REAL05 EQU 00094H;;;;;;;FAST;
IMAG05 EQU 00096H;;;;;;;FAST;
REAL06 EQU 00098H;;;;;;;FAST;
IMAG06 EQU 0009AH;;;;;;;FAST;
REAL07 EQU 0009CH;;;;;;;FAST;
IMAG07 EQU 0009EH;;;;;;;FAST;
REAL08 EQU 000A0H;;;;;;;FAST;
IMAG08 EQU 000A2H;;;;;;;FAST;
REAL09 EQU 000A4H;;;;;;;FAST;
IMAG09 EQU 000A6H;;;;;;;FAST;
REAL10 EQU 000A8H;;;;;;;FAST;
IMAG10 EQU 000AAH;;;;;;;FAST;
REAL11 EQU 000ACH;;;;;;;FAST;
IMAG11 EQU 000AEH;;;;;;;FAST;
REAL12 EQU 000B0H;;;;;;;FAST;
IMAG12 EQU 000B2H;;;;;;;FAST;
REAL13 EQU 000B4H;;;;;;;FAST;
IMAG13 EQU 000B6H;;;;;;;FAST;
REAL14 EQU 000B8H;;;;;;;FAST;
IMAG14 EQU 000BAH;;;;;;;FAST;
REAL15 EQU 000BCH;;;;;;;FAST;
IMAG15 EQU 000BEH;;;;;;;FAST;
REAL16 EQU 000C0H;;;;;;;FAST;
IMAG16 EQU 000C2H;;;;;;;FAST;
REAL17 EQU 000C4H;;;;;;;FAST;
IMAG17 EQU 000C6H;;;;;;;FAST;
REAL18 EQU 000C8H;;;;;;;FAST;
IMAG18 EQU 000CAH;;;;;;;FAST;
REAL19 EQU 000CCH;;;;;;;FAST;
IMAG19 EQU 000CEH;;;;;;;FAST;
REAL20 EQU 000D0H;;;;;;;FAST;
IMAG20 EQU 000D2H;;;;;;;FAST;
REAL21 EQU 000D4H;;;;;;;FAST;
IMAG21 EQU 000D6H;;;;;;;FAST;
REAL22 EQU 000D8H;;;;;;;FAST;
IMAG22 EQU 000DAH;;;;;;;FAST;
REAL23 EQU 000DCH;;;;;;;FAST;
IMAG23 EQU 000DEH;;;;;;;FAST;
REAL24 EQU 000E0H;;;;;;;FAST;
IMAG24 EQU 000E2H;;;;;;;FAST;
REAL25 EQU 000E4H;;;;;;;FAST;
IMAG25 EQU 000E6H;;;;;;;FAST;
REAL26 EQU 000E8H;;;;;;;FAST;
IMAG26 EQU 000EAH;;;;;;;FAST;
REAL27 EQU 000ECH;;;;;;;FAST;
IMAG27 EQU 000EEH;;;;;;;FAST;
REAL28 EQU 000F0H;;;;;;;FAST;
IMAG28 EQU 000F2H;;;;;;;FAST;
REAL29 EQU 000F4H;;;;;;;FAST;
IMAG29 EQU 000F6H;;;;;;;FAST;
REAL30 EQU 000F8H;;;;;;;FAST;
IMAG30 EQU 000FAH;;;;;;;FAST;
REAL31 EQU 000FCH;;;;;;;FAST;
IMAG31 EQU 000FEH;;;;;;;FAST;
DCZO EQU 00050H;;;;;;;FAST;
DCSK EQU 00052H;;;;;;;FAST;
COSVL EQU 00054H;;;;;;;FAST;
SINVL EQU 00056H;;;;;;;FAST;
TANVL EQU 00058H;;;;;;;FAST;
ARCVL EQU 0005AH;;;;;;;FAST;
YT000 EQU 0005CH;;;;;;;FAST;
WORK EQU 0005EH;;;;;;;FAST;
WORK0 EQU 0005EH;;;;;;;FAST;
WORK1 EQU 0005FH;;;;;;;FAST;
ZREAL EQU 00060H;;;;;;;FAST;
ZREALL EQU 00060H;;;;;;;FAST;
ZREALH EQU 00061H;;;;;;;FAST;
ZIMAG EQU 00062H;;;;;;;FAST;
ZIMAGL EQU 00062H;;;;;;;FAST;
ZIMAGH EQU 00063H;;;;;;;FAST;
XADDR EQU 00064H;;;;;;;FAST;
YADDR EQU 00066H;;;;;;;FAST;
XBSUM EQU 00068H;;;;;;;FAST;
XBSUM01 EQU 00068H;;;;;;;FAST;
XBSUM23 EQU 0006AH;;;;;;;FAST;
SUN EQU 0006CH;;;;;;;FAST;
SUN01 EQU 0006CH;;;;;;;FAST;
SUN23 EQU 0006EH;;;;;;;FAST;
SUX EQU 00070H;;;;;;;FAST;
SUX01 EQU 00070H;;;;;;;FAST;
SUX23 EQU 00072H;;;;;;;FAST;
SUX0 EQU 00070H;;;;;;;FAST;
SUX1 EQU 00071H;;;;;;;FAST;
SUX2 EQU 00072H;;;;;;;FAST;
SUX3 EQU 00073H;;;;;;;FAST;
SUY EQU 00074H;;;;;;;FAST;
SUY01 EQU 00074H;;;;;;;FAST;
SUY23 EQU 00076H;;;;;;;FAST;
SUY0 EQU 00074H;;;;;;;FAST;
SUY1 EQU 00075H;;;;;;;FAST;
SUY2 EQU 00076H;;;;;;;FAST;
SUY3 EQU 00077H;;;;;;;FAST;
SUP EQU 00078H;;;;;;;FAST;
SUP01 EQU 00078H;;;;;;;FAST;
SUP23 EQU 0007AH;;;;;;;FAST;
SUQ EQU 0007CH;;;;;;;FAST;
SUQ01 EQU 0007CH;;;;;;;FAST;
SUQ23 EQU 0007EH;;;;;;;FAST;
CSEG
EXTRN SIN:NULL
EXTRN TAN:NULL
PUBLIC FREQSUM; YT000--连续三周波首地址 XT000--DFT运算内存区
CSEG
FREQSUM:LDB WATCHDOG, #01EH;
LDB WATCHDOG, #0E1H;
CLR WORK;
CLR SUN01;
CLR SUN23;
CLR SUX01;
ADD XADDR, YT000, #02H;
ADD YADDR, YT000, #06H;
FQCHK: LD ZREAL, [XADDR];
ADD XADDR, #0004H;
LD ZIMAG, [YADDR];
ADD YADDR, #0004H;
ADD SUX01, #04H;
JBS ZREALH, 7H, FQP0N; IF ZREAL<0 THEN GOTO FQP0N;
FQP0P: JBS ZIMAGH, 7H, FQXXX; IF ZIMAG<0 THEN GOTO FQXXX;
CMP SUX01, #192;
JGE FQZZZ;
SJMP FQCHK;
FQP0N: JBC ZIMAGH, 7H, FQYYY; IF ZIMAG>=0 THEN GOTO FQYYY;
CMP SUX01, #192;
JGE FQZZZ;
SJMP FQCHK;
FQXXX: INC SUN01; ZREAL>=0 ZIMAG<0;
CMP SUN01, #02H;
JGE FQZZZ;
CMP SUN23, #02H;
JGE FQZZZ;
LD DCZO, SUX01;
CLR SUX01;
CMP SUN23, R0;
JNE FQXXP;
ADD DCZO, #04H;
ADD YT000, DCZO; 第一正过零点
SJMP FQCHK;
FQXXP: SHR DCZO, #02H; 第二正过零点
JBC DCZO, 0H, FQXXN;
INC DCZO;
FQXXN: SHL DCZO, #01H; (第二正过零点-第一正过零点)/2
CLR WORK;
CLR XADDR;
SJMP FQLOOP;
FQYYY: INC SUN23; ZREAL<0 ZIMAG>=0;
CMP SUN01, #02H;
JGE FQZZZ;
CMP SUN23, #02H;
JGE FQZZZ;
LD DCZO, SUX01;
CLR SUX01;
CMP SUN01, R0;
JNE FQYYP;
ADD DCZO, #04H; 第一负过零点
ADD YT000, DCZO;
SJMP FQCHK;
FQYYP: SHR DCZO, #02H; 第二负过零点
JBC DCZO, 0H, FQYYN;
INC DCZO;
FQYYN: SHL DCZO, #01H; (第二负过零点-第一负过零点)/2
LD WORK, #0FF00H;
CLR XADDR;
SJMP FQLOOP;
FQZZZ: LD SUP01, #0FFFFH;
RET;
FQLOOP: ADD XADDR, YT000;
LD REAL00, [XADDR]+; 00
LD IMAG00, [XADDR]+; 01
LD REAL01, [XADDR]+; 02
LD IMAG01, [XADDR]+; 03
LD REAL02, [XADDR]+; 04
LD IMAG02, [XADDR]+; 05
LD REAL03, [XADDR]+; 06
LD IMAG03, [XADDR]+; 07
LD REAL04, [XADDR]+; 08
LD IMAG04, [XADDR]+; 09
LD REAL05, [XADDR]+; 10
LD IMAG05, [XADDR]+; 11
LD REAL06, [XADDR]+; 12
LD IMAG06, [XADDR]+; 13
LD REAL07, [XADDR]+; 14
LD IMAG07, [XADDR]+; 15
LD REAL08, [XADDR]+; 16
LD IMAG08, [XADDR]+; 17
LD REAL09, [XADDR]+; 18
LD IMAG09, [XADDR]+; 19
LD REAL10, [XADDR]+; 20
LD IMAG10, [XADDR]+; 21
LD REAL11, [XADDR]+; 22
LD IMAG11, [XADDR]+; 23
LD REAL12, [XADDR]+; 24
LD IMAG12, [XADDR]+; 25
LD REAL13, [XADDR]+; 26
LD IMAG13, [XADDR]+; 27
LD REAL14, [XADDR]+; 28
LD IMAG14, [XADDR]+; 29
LD REAL15, [XADDR]+; 30
LD IMAG15, [XADDR]+; 31
LD REAL16, [XADDR]+; 32
LD IMAG16, [XADDR]+; 33
LD REAL17, [XADDR]+; 34
LD IMAG17, [XADDR]+; 35
LD REAL18, [XADDR]+; 36
LD IMAG18, [XADDR]+; 37
LD REAL19, [XADDR]+; 38
LD IMAG19, [XADDR]+; 39
LD REAL20, [XADDR]+; 40
LD IMAG20, [XADDR]+; 41
LD REAL21, [XADDR]+; 42
LD IMAG21, [XADDR]+; 43
LD REAL22, [XADDR]+; 44
LD IMAG22, [XADDR]+; 45
LD REAL23, [XADDR]+; 46
LD IMAG23, [XADDR]+; 47
LD REAL24, [XADDR]+; 48
LD IMAG24, [XADDR]+; 49
LD REAL25, [XADDR]+; 50
LD IMAG25, [XADDR]+; 51
LD REAL26, [XADDR]+; 52
LD IMAG26, [XADDR]+; 53
LD REAL27, [XADDR]+; 54
LD IMAG27, [XADDR]+; 55
LD REAL28, [XADDR]+; 56
LD IMAG28, [XADDR]+; 57
LD REAL29, [XADDR]+; 58
LD IMAG29, [XADDR]+; 59
LD REAL30, [XADDR]+; 60
LD IMAG30, [XADDR]+; 61
LD REAL31, [XADDR]+; 62
LD IMAG31, [XADDR]+; 63
F00X00: XCH REAL01, REAL16;
XCH IMAG01, IMAG16;
F00X01: XCH REAL02, REAL08;
XCH IMAG02, IMAG08;
F00X02: XCH REAL03, REAL24;
XCH IMAG03, IMAG24;
F00X03: XCH REAL05, REAL20;
XCH IMAG05, IMAG20;
F00X04: XCH REAL06, REAL12;
XCH IMAG06, IMAG12;
F00X05: XCH REAL07, REAL28;
XCH IMAG07, IMAG28;
F00X06: XCH REAL09, REAL18;
XCH IMAG09, IMAG18;
F00X07: XCH REAL11, REAL26;
XCH IMAG11, IMAG26;
F00X08: XCH REAL13, REAL22;
XCH IMAG13, IMAG22;
F00X09: XCH REAL15, REAL30;
XCH IMAG15, IMAG30;
F00X10: XCH REAL19, REAL25;
XCH IMAG19, IMAG25;
F00X11: XCH REAL23, REAL29;
XCH IMAG23, IMAG29;
;;;;;;;;;;;;;;;;第一次碟形处理;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
F01X00: SUB IMAG01, IMAG00, IMAG01; X01=X00-X01;
F01X01: SUB IMAG03, IMAG02, IMAG03; X03=X02-X03;
F01X02: SUB IMAG05, IMAG04, IMAG05; X05=X04-X05;
F01X03: SUB IMAG07, IMAG06, IMAG07; X07=X06-X07;
F01X04: SUB IMAG09, IMAG08, IMAG09; X09=X08-X09;
F01X05: SUB IMAG11, IMAG10, IMAG11; X11=X10-X11;
F01X06: SUB IMAG13, IMAG12, IMAG13; X13=X12-X13;
F01X07: SUB IMAG15, IMAG14, IMAG15; X15=X14-X15;
F01X08: SUB IMAG17, IMAG16, IMAG17; X17=X16-X17;
F01X09: SUB IMAG19, IMAG18, IMAG19; X19=X18-X19;
F01X10: SUB IMAG21, IMAG20, IMAG21; X21=X20-X21;
F01X11: SUB IMAG23, IMAG22, IMAG23; X23=X22-X23;
F01X12: SUB IMAG25, IMAG24, IMAG25; X25=X24-X25;
F01X13: SUB IMAG27, IMAG26, IMAG27; X27=X26-X27;
F01X14: SUB IMAG29, IMAG28, IMAG29; X29=X28-X29;
F01X15: SUB IMAG31, IMAG30, IMAG31; X31=X30-X31;
;;;;;;;;;;;;;;;;第二次碟形处理;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
F02X00: LD REAL01, IMAG03; X01=X01+X03*(-I);
F02X01: LD REAL05, IMAG07; X05=X05+X07*(-I);
F02X02: LD REAL09, IMAG11; X09=X09+X11*(-I);
F02X03: LD REAL13, IMAG15; X13=X13+X15*(-I);
F02X04: LD REAL17, IMAG19; X17=X17+X19*(-I);
F02X05: LD REAL21, IMAG23; X21=X21+X23*(-I);
F02X06: LD REAL25, IMAG27; X25=X25+X27*(-I);
F02X07: LD REAL29, IMAG31; X29=X29+X31*(-I);
;;;;;;;;;;;;;;;;第三次碟形处理;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
F03X00: LD COSVL, #23170; COSVL=23170/32768=0.7071=SINVL;
;X1=X1+W4*X5; X1.REAL + 0.7071*(X5.REAL+X5.IMAG)+[X1.IMAG + 0.7071*(X5.IMAG-X5.REAL)]*I
;X5=X1-W4*X5; X1.REAL - 0.7071*(X5.REAL+X5.IMAG)+[X1.IMAG - 0.7071*(X5.IMAG-X5.REAL)]*I
;W4=0.7071-0.7071*I;
SUB ZREAL, IMAG05, REAL05; ZREAL=X5.IMAG-X5.REAL
MUL SUQ, ZREAL, COSVL;
SHLL SUQ, #01H; SUQ23=0.7071*(X5.IMAG-X5.REAL)
ADD ZREAL, REAL05, IMAG05; ZREAL=X5.REAL+X5.IMAG
MUL SUP, ZREAL, COSVL;
SHLL SUP, #01H; SUP23=0.7071*(X5.REAL+X5.IMAG)
SUB REAL05, REAL01, SUP23; X1.REAL-SUP23 + (X1.IMAG-SUQ23)*I
SUB IMAG05, IMAG01, SUQ23;
ADD REAL01, SUP23; X1.REAL+SUP23 + (X1.IMAG+SUQ23)*I
ADD IMAG01, SUQ23;
F03X01:;X09=X09+W4*X13; X09.REAL + 0.7071*(X13.REAL+X13.IMAG)+[X09.IMAG + 0.7071*(X5.IMAG-X13.REAL)]*I
;X13=X09-W4*X13; X09.REAL - 0.7071*(X13.REAL+X13.IMAG)+[X09.IMAG - 0.7071*(X5.IMAG-X13.REAL)]*I
;W4=0.7071-0.7071*I;
SUB ZREAL, IMAG13, REAL13; ZREAL=X13.IMAG-X13.REAL
MUL SUQ, ZREAL, COSVL;
SHLL SUQ, #01H; SUQ23=0.7071*(X13.IMAG-X13.REAL)
ADD ZREAL, REAL13, IMAG13; ZREAL=X13.REAL+X13.IMAG
MUL SUP, ZREAL, COSVL;
SHLL SUP, #01H; SUP23=0.7071*(X13.REAL+X13.IMAG)
SUB REAL13, REAL09, SUP23; X09.REAL-SUP23 + (X09.IMAG-SUQ23)*I
SUB IMAG13, IMAG09, SUQ23;
ADD REAL09, SUP23; X09.REAL+SUP23 + (X09.IMAG+SUQ23)*I
ADD IMAG09, SUQ23;
F03X02:;X17=X17+W4*X21; X17.REAL + 0.7071*(X21.REAL+X21.IMAG)+[X17.IMAG + 0.7071*(X21.IMAG-X21.REAL)]*I
;X21=X17-W4*X21; X17.REAL - 0.7071*(X21.REAL+X21.IMAG)+[X17.IMAG - 0.7071*(X21.IMAG-X21.REAL)]*I
;W4=0.7071-0.7071*I;
SUB ZREAL, IMAG21, REAL21; ZREAL=X21.IMAG-X21.REAL
MUL SUQ, ZREAL, COSVL;
SHLL SUQ, #01H; SUQ23=0.7071*(X21.IMAG-X21.REAL)
ADD ZREAL, REAL21, IMAG21; ZREAL=X21.REAL+X21.IMAG
MUL SUP, ZREAL, COSVL;
SHLL SUP, #01H; SUP23=0.7071*(X21.REAL+X21.IMAG)
SUB REAL21, REAL17, SUP23; X17.REAL-SUP23 + (X17.IMAG-SUQ23)*I
SUB IMAG21, IMAG17, SUQ23;
ADD REAL17, SUP23; X17.REAL+SUP23 + (X17.IMAG+SUQ23)*I
ADD IMAG17, SUQ23;
F03X03:;X25=X25+W4*X29; X25.REAL + 0.7071*(X29.REAL+X29.IMAG)+[X25.IMAG + 0.7071*(X29.IMAG-X29.REAL)]*I
;X29=X25-W4*X29; X25.REAL - 0.7071*(X29.REAL+X29.IMAG)+[X25.IMAG - 0.7071*(X29.IMAG-X29.REAL)]*I
;W4=0.7071-0.7071*I;
SUB ZREAL, IMAG29, REAL29; ZREAL=X29.IMAG-X29.REAL
MUL SUQ, ZREAL, COSVL;
SHLL SUQ, #01H; SUQ23=0.7071*(X29.IMAG-X29.REAL)
ADD ZREAL, REAL29, IMAG29; ZREAL=X29.REAL+X29.IMAG
MUL SUP, ZREAL, COSVL;
SHLL SUP, #01H; SUP23=0.7071*(X29.REAL+X29.IMAG)
SUB REAL29, REAL25, SUP23; X25.REAL-SUP23 + (X25.IMAG-SUQ23)*I
SUB IMAG29, IMAG25, SUQ23;
ADD REAL25, SUP23; X25.REAL+SUP23 + (X25.IMAG+SUQ23)*I
ADD IMAG25, SUQ23;
;;;;;;;;;;;;;;;;第四次碟形处理;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
F04X00: LD COSVL, #30274; W2; COSVL= 30274/32768=COS(-11.25*2);
LD SINVL, #52996; W2; SINVL=-12540/32768=SIN(-11.25*2);
;X01=X01+W2*X09; X01.REAL +(COSVL*X09.REAL - SINVL*X09.IMAG)+[X01.IMAG +(COSVL*X09.IMAG + SINVL*X09.REAL)]*I
;X09=X01-W2*X09; X01.REAL -(COSVL*X09.REAL - SINVL*X09.IMAG)+[X01.IMAG -(COSVL*X09.IMAG + SINVL*X09.REAL)]*I
;W02= 30274/32768-12540/32768*I;
MUL SUX, REAL09, COSVL;
SHLL SUX, #01H;
MUL SUY, IMAG09, SINVL;
SHLL SUY, #01H;
SUB SUQ23, SUX23, SUY23; SUQ23=COSVL*X09.REAL - SINVL*X09.IMAG;
MUL SUX, IMAG09, COSVL;
SHLL SUX, #01H;
MUL SUY, REAL09, SINVL;
SHLL SUY, #01H;
ADD SUP23, SUX23, SUY23; SUP23=COSVL*X09.IMAG + SINVL*X09.REAL;
SUB REAL09, REAL01, SUQ23; X01.REAL - SUQ23+
SUB IMAG09, IMAG01, SUP23; (X01.IMAG - SUP23)*I
ADD REAL01, SUQ23; X01.REAL + SUQ23+
ADD IMAG01, SUP23; (X01.IMAG + SUP23)*I
F04X01: LD COSVL, #30274; W2; COSVL= 30274/32768=COS(-11.25*2);
LD SINVL, #52996; W2; SINVL=-12540/32768=SIN(-11.25*2);
;X17=X17+W2*X25; X17.REAL +(COSVL*X25.REAL - SINVL*X25.IMAG)+[X17.IMAG +(COSVL*X25.IMAG + SINVL*X25.REAL)]*I
;X25=X17-W2*X25; X17.REAL -(COSVL*X25.REAL - SINVL*X25.IMAG)+[X17.IMAG -(COSVL*X25.IMAG + SINVL*X25.REAL)]*I
;W02= 30274/32768-12540/32768*I;
MUL SUX, REAL25, COSVL;
SHLL SUX, #01H;
MUL SUY, IMAG25, SINVL;
SHLL SUY, #01H;
SUB SUQ23, SUX23, SUY23; SUQ23=COSVL*X25.REAL - SINVL*X25.IMAG;
MUL SUX, IMAG25, COSVL;
SHLL SUX, #01H;
MUL SUY, REAL25, SINVL;
SHLL SUY, #01H;
ADD SUP23, SUX23, SUY23; SUP23=COSVL*X25.IMAG + SINVL*X25.REAL;
SUB REAL25, REAL17, SUQ23; X17.REAL - SUQ23+
SUB IMAG25, IMAG17, SUP23; (X17.IMAG - SUP23)*I
ADD REAL17, SUQ23; X17.REAL + SUQ23+
ADD IMAG17, SUP23; (X17.IMAG + SUP23)*I
;;;;;;;;;;;;;;;;第五次碟形处理;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
F05X00: LD COSVL, #32138; W01 COSVL= 32138/32768=COS(-11.25*1);
LD SINVL, #59143; W01 SINVL=-06393/32768=SIN(-11.25*1);
;X01=X01+W1*X17; X01.REAL +(COSVL*X17.REAL - SINVL*X17.IMAG)+[X01.IMAG +(COSVL*X17.IMAG + SINVL*X17.REAL)]*I
;X17=X01-W1*X17; X01.REAL -(COSVL*X17.REAL - SINVL*X17.IMAG)+[X01.IMAG -(COSVL*X17.IMAG + SINVL*X17.REAL)]*I
;W01=-32138/32768-06393/32768*I;
MUL SUX, REAL17, COSVL;
SHLL SUX, #01H;
MUL SUY, IMAG17, SINVL;
SHLL SUY, #01H;
SUB SUQ23, SUX23, SUY23; SUQ23=COSVL*X17.REAL - SINVL*X17.IMAG;
MUL SUX, IMAG17, COSVL;
SHLL SUX, #01H;
MUL SUY, REAL17, SINVL;
SHLL SUY, #01H;
ADD SUP23, SUX23, SUY23; SUP23=COSVL*X17.IMAG + SINVL*X17.REAL;
ADD REAL01, SUQ23; X01.REAL + SUQ23+
ADD IMAG01, SUP23; (X01.IMAG + SUP23)*I
CLR SUY01;
CLR SUY23;
ARC00: LD ZREAL, REAL01; 基波分量
LD ZIMAG, IMAG01;
CLR SUP01;
JBC ZREALH, 07H, JN000;
NEG ZREAL;
JN000: JBC ZIMAGH, 07H, JN001;
NEG ZIMAG;
JN001: CMP ZIMAG, ZREAL;
JLE JN002; IF IMAG<=REAL THEN JUMP JN002;
LD SUQ01, ZREAL;
LD SUQ23, ZIMAG;
ST SUQ23, ZREAL;
ST SUQ01, ZIMAG;
LD SUP01, #0FFFFH; 45度标志
JN002: LD SUX23, ZIMAG; IMAG<=REAL;
CLR SUX01;
CMP SUX23, R0;
JE JDIV;
LD SUX01, #0FFFFH;
DEC SUX23;
SHRAL SUX, #01H;
JDIV: CMP R0, ZREAL;
JNE JN003;
LD SUX01, #07FFFH;
SJMP JN004;
JN003: CMP ZREAL, #01H;
JE JN004;
CMP ZREAL, #02H;
JNE JNMMM;
SHRAL SUX, #01H;
SJMP JN004;
JNMMM: DIV SUX, ZREAL;
JN004: LD TANVL, SUX01;
CLR XADDR; FIRST=0;
LD YADDR, #4500; LAST=4500;
JN005: ADD SUQ01, XADDR, YADDR;
SHR SUQ01, #01H; SUQ01=(FIRST+LAST)/2;
LD SUQ23, SUQ01;
SHL SUQ23, #01H;
LD SUX01, TAN[SUQ23]; ;SUX01=TAN(SUQ23);
CMP XADDR, YADDR;
JGE JN007; IF FIRST>=LAST THEN JUMP JN007;
CMP XADDR, SUQ01;
JE JN007;
CMP YADDR, SUQ01;
JE JN007;
SUB SUQ23, YADDR, XADDR;
CMP SUQ23, #01H;
JLE JN007;
CMP TANVL, SUX01;
JE JN007;
JGT JN006;
LD YADDR, SUQ01; ;YADDR=SUQ01=(FIRST+LAST)/2;
SJMP JN005;
JN006: LD XADDR, SUQ01; ;XADDR=SUQ01=(FIRST+LAST)/2;
SJMP JN005;
JN007: LD ARCVL, SUQ01;
SHL ARCVL, #01H;
JBC SUP01, 0H, JN008;
LD SUP01, #18000;
SUB SUP01, ARCVL;
LD ARCVL, SUP01;
JN008: SHR ARCVL, #01H;
CMP REAL01, R0;
JGE J01OR04;
J02OR03:CMP IMAG01, R0;
JGE J0002;
J0003: LD SUX01, #47536; 47536=-18000;
ADD SUX01, ARCVL;
SJMP JQ000;
J0002: LD SUX01, #18000;
SUB SUX01, ARCVL;
SJMP JQ000;
J01OR04:CMP IMAG01, R0;
JGE J0001;
J0004: LD SUX01, ARCVL;
NEG SUX01;
SJMP JQ000;
J0001: LD SUX01, ARCVL;
JQ000: LD IMAG01, SUX01; 谐波角度
INCB WORK0;
CMPB WORK0, #01H;
JE F001;
CMPB WORK0, #02H;
JE F002;
CMPB WORK0, #03H;
JE F003;
F004: LD XBSUM23, ARCVL;
SJMP FQ004;
F001: LD XADDR, #128;
LD SUN01, ARCVL;
LJMP FQLOOP;
F002: LD XADDR, DCZO;
LD SUN23, ARCVL;
LJMP FQLOOP;
F003: LD XADDR, DCZO;
ADD XADDR, #128;
LD XBSUM01, ARCVL;
LJMP FQLOOP;
FQ004: JBC WORK1, 0H, FQ0041;
FQ0040: LD SUQ01, #08CA0H; 360.00 *100
LD SUP01, #08CA0H; 360.00 *100
CLR SUX23;
ADD SUX01, SUN01, XBSUM23;
ADDC SUX23, R0;
NEG XBSUM01;
NEG SUN23;
ADD SUX01, SUN01;
ADDC SUX23, R0;
ADD SUX01, XBSUM23;
ADDC SUX23, R0;
SHRAL SUX, #01H;
ADD SUP01, SUX01;
CLR SUP23;
MULU SUP, #50000;
SJMP FQ0044;
FQ0041: LD SUQ01, #08CA0H; 360.00 *100
LD SUP01, #08CA0H; 360.00 *100
CLR SUX23;
ADD SUX01, SUN23, XBSUM01;
ADDC SUX23, R0;
NEG XBSUM23;
NEG SUN01;
ADD SUX01, SUN01;
ADDC SUX23, R0;
ADD SUX01, XBSUM23;
ADDC SUX23, R0;
SHRAL SUX, #01H;
ADD SUP01, SUX01;
CLR SUP23;
MULU SUP, #50000;
FQ0044: CMP SUQ01, R0;
JE FQNEXT;
DIVU SUP, SUQ01; SUP01=50000 频率系数
RET;
FQNEXT: LD SUP01, #0FFFFH;
RET;
END;