main.s

ATmega128和ads7864实现ATP闭环控制

	lsr R17
	ror R16
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
 ; stack offset 4
	ldd R16,y+28
	ldd R17,y+29
	andi R16,1
	andi R17,0
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall add32f
	movw R30,R28
	std z+12,R16
	std z+13,R17
	std z+14,R18
	std z+15,R19
	.dbline 314
	ldi R16,<L18
	ldi R17,>L18
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
 ; stack offset 4
	ldd R16,y+30
	ldd R17,y+31
	lsr R17
	ror R16
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
 ; stack offset 4
	ldd R16,y+30
	ldd R17,y+31
	andi R16,1
	andi R17,0
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall add32f
	movw R30,R28
	std z+16,R16
	std z+17,R17
	std z+18,R18
	std z+19,R19
	.dbline 315
	movw R30,R28
	ldd R2,z+4
	ldd R3,z+5
	ldd R4,z+6
	ldd R5,z+7
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	movw R30,R28
 ; stack offset 4
	ldd R2,z+12
	ldd R3,z+13
	ldd R4,z+14
	ldd R5,z+15
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall add32fs
	movw R30,R28
 ; stack offset 4
	ldd R2,z+16
	ldd R3,z+17
	ldd R4,z+18
	ldd R5,z+19
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall add32fs
	movw R30,R28
 ; stack offset 4
	ldd R2,z+20
	ldd R3,z+21
	ldd R4,z+22
	ldd R5,z+23
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall add32f
	movw R30,R28
	std z+32,R16
	std z+33,R17
	std z+34,R18
	std z+35,R19
	.dbline 319
	movw R30,R28
	ldd R2,z+4
	ldd R3,z+5
	ldd R4,z+6
	ldd R5,z+7
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	movw R30,R28
 ; stack offset 4
	ldd R2,z+12
	ldd R3,z+13
	ldd R4,z+14
	ldd R5,z+15
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall add32fs
	movw R30,R28
 ; stack offset 4
	ldd R2,z+16
	ldd R3,z+17
	ldd R4,z+18
	ldd R5,z+19
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall sub32fs
	movw R30,R28
 ; stack offset 4
	ldd R2,z+20
	ldd R3,z+21
	ldd R4,z+22
	ldd R5,z+23
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall sub32fs
	movw R30,R28
 ; stack offset 4
	ldd R2,z+36
	ldd R3,z+37
	ldd R4,z+38
	ldd R5,z+39
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall div32f
	movw R30,R28
	std z+36,R16
	std z+37,R17
	std z+38,R18
	std z+39,R19
	.dbline 320
	movw R30,R28
	subi R30,192  ; addi 64
	sbci R31,255
	ldd R2,z+0
	ldd R3,z+1
	ldd R4,z+2
	ldd R5,z+3
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	movw R30,R28
 ; stack offset 4
	ldd R2,z+40
	ldd R3,z+41
	ldd R4,z+42
	ldd R5,z+43
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall add32f
	movw R30,R28
	subi R30,192  ; addi 64
	sbci R31,255
	std z+0,R16
	std z+1,R17
	std z+2,R18
	std z+3,R19
	.dbline 322
	ldi R20,60
	ldi R21,240
	ldi R22,0
	ldi R23,0
	movw R30,R28
	std z+28,R20
	std z+29,R21
	std z+30,R22
	std z+31,R23
	.dbline 323
	movw R30,R28
	ldd R2,z+52
	ldd R3,z+53
	ldd R4,z+54
	ldd R5,z+55
	std y+0,R2
	std y+1,R3
	std y+2,R4
	std y+3,R5
	movw R30,R28
	ldd R16,z+28
	ldd R17,z+29
	ldd R18,z+30
	ldd R19,z+31
	xcall _OutputFrequency
	.dbline 324
	ldi R16,10
	xcall _delayms
	.dbline 325
	movw R16,R28
	subi R16,236  ; offset = 20
	sbci R17,255
	xcall _usAD_GetData
	.dbline 326
	ldi R16,<L18
	ldi R17,>L18
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
 ; stack offset 4
	ldd R16,y+24
	ldd R17,y+25
	lsr R17
	ror R16
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
 ; stack offset 4
	ldd R16,y+24
	ldd R17,y+25
	andi R16,1
	andi R17,0
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall add32f
	movw R30,R28
	std z+4,R16
	std z+5,R17
	std z+6,R18
	std z+7,R19
	.dbline 327
	ldi R16,<L18
	ldi R17,>L18
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
 ; stack offset 4
	ldd R16,y+26
	ldd R17,y+27
	lsr R17
	ror R16
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
 ; stack offset 4
	ldd R16,y+26
	ldd R17,y+27
	andi R16,1
	andi R17,0
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall add32f
	movw R30,R28
	std z+8,R16
	std z+9,R17
	std z+10,R18
	std z+11,R19
	.dbline 328
	ldi R16,<L18
	ldi R17,>L18
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
 ; stack offset 4
	ldd R16,y+28
	ldd R17,y+29
	lsr R17
	ror R16
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
 ; stack offset 4
	ldd R16,y+28
	ldd R17,y+29
	andi R16,1
	andi R17,0
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall add32f
	movw R30,R28
	std z+12,R16
	std z+13,R17
	std z+14,R18
	std z+15,R19
	.dbline 329
	ldi R16,<L18
	ldi R17,>L18
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
 ; stack offset 4
	ldd R16,y+30
	ldd R17,y+31
	lsr R17
	ror R16
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
 ; stack offset 4
	ldd R16,y+30
	ldd R17,y+31
	andi R16,1
	andi R17,0
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall add32f
	movw R30,R28
	std z+16,R16
	std z+17,R17
	std z+18,R18
	std z+19,R19
	.dbline 330
	movw R30,R28
	ldd R2,z+4
	ldd R3,z+5
	ldd R4,z+6
	ldd R5,z+7
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	movw R30,R28
 ; stack offset 4
	ldd R2,z+12
	ldd R3,z+13
	ldd R4,z+14
	ldd R5,z+15
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall add32fs
	movw R30,R28
 ; stack offset 4
	ldd R2,z+16
	ldd R3,z+17
	ldd R4,z+18
	ldd R5,z+19
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall add32fs
	movw R30,R28
 ; stack offset 4
	ldd R2,z+20
	ldd R3,z+21
	ldd R4,z+22
	ldd R5,z+23
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall add32f
	movw R30,R28
	std z+32,R16
	std z+33,R17
	std z+34,R18
	std z+35,R19
	.dbline 334
	movw R30,R28
	ldd R2,z+4
	ldd R3,z+5
	ldd R4,z+6
	ldd R5,z+7
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	movw R30,R28
 ; stack offset 4
	ldd R2,z+12
	ldd R3,z+13
	ldd R4,z+14
	ldd R5,z+15
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall add32fs
	movw R30,R28
 ; stack offset 4
	ldd R2,z+16
	ldd R3,z+17
	ldd R4,z+18
	ldd R5,z+19
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall sub32fs
	movw R30,R28
 ; stack offset 4
	ldd R2,z+20
	ldd R3,z+21
	ldd R4,z+22
	ldd R5,z+23
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall sub32fs
	movw R30,R28
 ; stack offset 4
	ldd R2,z+36
	ldd R3,z+37
	ldd R4,z+38
	ldd R5,z+39
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall div32f
	movw R30,R28
	std z+36,R16
	std z+37,R17
	std z+38,R18
	std z+39,R19
	.dbline 335
	movw R30,R28
	subi R30,188  ; addi 68
	sbci R31,255
	ldd R2,z+0
	ldd R3,z+1
	ldd R4,z+2
	ldd R5,z+3
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	movw R30,R28
 ; stack offset 4
	ldd R2,z+40
	ldd R3,z+41
	ldd R4,z+42
	ldd R5,z+43
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall add32f
	movw R30,R28
	subi R30,188  ; addi 68
	sbci R31,255
	std z+0,R16
	std z+1,R17
	std z+2,R18
	std z+3,R19
	.dbline 336
L62:
	.dbline 305
	ldi R20,1
	ldi R21,0
	ldi R22,0
	ldi R23,0
	movw R30,R28
	ldd R2,z+40
	ldd R3,z+41
	ldd R4,z+42
	ldd R5,z+43
	add R2,R20
	adc R3,R21
	adc R4,R22
	adc R5,R23
	movw R30,R28
	std z+40,R2
	std z+41,R3
	std z+42,R4
	std z+43,R5
L64:
	.dbline 305
	ldi R20,232
	ldi R21,3
	ldi R22,0
	ldi R23,0
	movw R30,R28
	ldd R2,z+40
	ldd R3,z+41
	ldd R4,z+42
	ldd R5,z+43
	cp R2,R20
	cpc R3,R21
	cpc R4,R22
	cpc R5,R23
	brsh X5
	xjmp L61
X5:
	.dbline 338
; 	{
; 		ulFY=60500;
; 		OutputFrequency(ulFY,ulFX);
; 		delayms(10);
; 		usAD_GetData(usADDATA);
; 		dADDATA[0]=(double)usADDATA[0];
; 		dADDATA[1]=(double)usADDATA[1];
; 		dADDATA[2]=(double)usADDATA[2];
; 		dADDATA[3]=(double)usADDATA[3];
; 		dTotalData=dADDATA[QDA]+dADDATA[QDB]+dADDATA[QDC]+dADDATA[QDD];
; 		
; 
; 		//dEx=(dADDATA[QDA]+dADDATA[QDD]-dADDATA[QDB]-dADDATA[QDC])/dTotalData;
; 		dEy=(dADDATA[QDA]+dADDATA[QDB]-dADDATA[QDC]-dADDATA[QDD])/dTotalData;
; 		dEy1T=dEy1T+dEy;
; 
; 		ulFY=61500;
; 		OutputFrequency(ulFY,ulFX);
; 		delayms(10);
; 		usAD_GetData(usADDATA);
; 		dADDATA[0]=(double)usADDATA[0];
; 		dADDATA[1]=(double)usADDATA[1];
; 		dADDATA[2]=(double)usADDATA[2];
; 		dADDATA[3]=(double)usADDATA[3];
; 		dTotalData=dADDATA[QDA]+dADDATA[QDB]+dADDATA[QDC]+dADDATA[QDD];
; 		
; 
; 		//dEx=(dADDATA[QDA]+dADDATA[QDD]-dADDATA[QDB]-dADDATA[QDC])/dTotalData;
; 		dEy=(dADDATA[QDA]+dADDATA[QDB]-dADDATA[QDC]-dADDATA[QDD])/dTotalData;
; 		dEy2T=dEy2T+dEy;
; 	}
; 
; 	NOP();
	nop
	.dbline 339
; 	dEy1T=dEy1T/1000;
	movw R30,R28
	subi R30,192  ; addi 64
	sbci R31,255
	ldd R2,z+0
	ldd R3,z+1
	ldd R4,z+2
	ldd R5,z+3
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	ldi R16,<L89
	ldi R17,>L89
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall div32f
	movw R30,R28
	subi R30,192  ; addi 64
	sbci R31,255
	std z+0,R16
	std z+1,R17
	std z+2,R18
	std z+3,R19
	.dbline 340
; 	dEy2T=dEy2T/1000;
	movw R30,R28
	subi R30,188  ; addi 68
	sbci R31,255
	ldd R2,z+0
	ldd R3,z+1
	ldd R4,z+2
	ldd R5,z+3
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	ldi R16,<L89
	ldi R17,>L89
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall div32f
	movw R30,R28
	subi R30,188  ; addi 68
	sbci R31,255
	std z+0,R16
	std z+1,R17
	std z+2,R18
	std z+3,R19
	.dbline 341
; 	NOP();
	nop
	.dbline 342
; 	NOP();
	nop
	.dbline 343
; 	dEx=(dEy1T+dEy2T);
	movw R30,R28
	subi R30,192  ; addi 64
	sbci R31,255
	ldd R2,z+0
	ldd R3,z+1
	ldd R4,z+2
	ldd R5,z+3
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	movw R30,R28
 ; stack offset 4
	subi R30,184  ; addi 72
	sbci R31,255
	ldd R2,z+0
	ldd R3,z+1
	ldd R4,z+2
	ldd R5,z+3
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall add32f
	movw R30,R28
	subi R30,184  ; addi 72
	sbci R31,255
	std z+0,R16
	std z+1,R17
	std z+2,R18
	std z+3,R19
	.dbline 344
; 	dEyTotal=(dEy1T-dEy2T);
	movw R30,R28
	subi R30,192  ; addi 64
	sbci R31,255
	ldd R2,z+0
	ldd R3,z+1
	ldd R4,z+2
	ldd R5,z+3
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	movw R30,R28
 ; stack offset 4
	subi R30,184  ; addi 72
	sbci R31,255
	ldd R2,z+0
	ldd R3,z+1
	ldd R4,z+2
	ldd R5,z+3
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall sub32f
	movw R30,R28
	std z+56,R16
	std z+57,R17
	std z+58,R18
	std z+59,R19
	.dbline 345
; 	NOP();
	nop
	.dbline 346
; 	NOP();
	nop
	.dbline 349
; 
; 
; 	ulFY=Fy0;
	ldi R20,58
	ldi R21,239
	ldi R22,0
	ldi R23,0
	movw R30,R28
	std z+28,R20
	std z+29,R21
	std z+30,R22
	std z+31,R23
	.dbline 350
; 	OutputFrequency(ulFY,ulFX);
	movw R30,R28
	ldd R2,z+52
	ldd R3,z+53
	ldd R4,z+54
	ldd R5,z+55
	std y+0,R2
	std y+1,R3
	std y+2,R4
	std y+3,R5
	movw R30,R28
	ldd R16,z+28
	ldd R17,z+29
	ldd R18,z+30
	ldd R19,z+31
	xcall _OutputFrequency
	xjmp L91
L90:
	.dbline 352
; 	while(1)
; 	for(i=0;i<1000;i++)
	ldi R20,0
	ldi R21,0
	ldi R22,0
	ldi R23,0
	movw R30,R28
	std z+40,R20
	std z+41,R21
	std z+42,R22
	std z+43,R23
	xjmp L96
L93:
	.dbline 353
	.dbline 354
	ldi R16,10
	xcall _delayms
	.dbline 355
	movw R16,R28
	subi R16,236  ; offset = 20
	sbci R17,255
	xcall _usAD_GetData
	.dbline 356
	ldi R16,<L18
	ldi R17,>L18
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
 ; stack offset 4
	ldd R16,y+24
	ldd R17,y+25
	lsr R17
	ror R16
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
 ; stack offset 4
	ldd R16,y+24
	ldd R17,y+25
	andi R16,1
	andi R17,0
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall add32f
	movw R30,R28
	std z+4,R16
	std z+5,R17
	std z+6,R18
	std z+7,R19
	.dbline 357
	ldi R16,<L18
	ldi R17,>L18
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
 ; stack offset 4
	ldd R16,y+26
	ldd R17,y+27
	lsr R17
	ror R16
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
 ; stack offset 4
	ldd R16,y+26
	ldd R17,y+27
	andi R16,1
	andi R17,0
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall add32f
	movw R30,R28
	std z+8,R16
	std z+9,R17
	std z+10,R18
	std z+11,R19
	.dbline 358
	ldi R16,<L18
	ldi R17,>L18
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
 ; stack offset 4
	ldd R16,y+28
	ldd R17,y+29
	lsr R17
	ror R16
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
 ; stack offset 4