;************************************************
;Name	:InterruptSub
;Function:
;Input  :
;Output	:
;Register Used:
;*************************************************
InterruptSub:
	XCH	A,AccBuf
	XCH	A,PFLAG
	B0MOV	PflagBuf,A
	
	b0bts0	FTC0IRQ		;//Analyse the Interrupt resourc
	jmp	TC0INTR
	
	b0bts0	ft0irq
	jmp	t0intr

	b0bts0	FP00IRQ
	jmp	P00INTR

Interrupt90:
	XCH	A,PflagBuf
	B0MOV	PFLAG,A
	XCH	A,AccBuf	;//For release the Acc And Pflag
	RETI
;--------------------------------------------------------
;--------------------------------------------------------
TC0INTR:
	incms	timer5ms
	nop
	mov	a,timer5ms
	sub	a,#100
	bts1	fc
	jmp	tc0intr00
	clr	timer5ms
	bset	t5msflag
tc0intr00:
;----------------------
;左右翼控制
	b0bts0	wingstatflag
	jmp	TC0INTR10
	b0bset	ptwing	
	decms	wingbuf
	jmp	TC0INTR20
	b0bclr	ptwing	
	b0bset	wingstatflag
	mov_	winglowbuf,lowcontant		;set low level 10ms
	mov_	wingbuf,wingloadbuf	;set high level 
;-----
TC0INTR10:
	decms	winglowbuf
	jmp	TC0INTR20
	b0bclr	wingstatflag	
;-----------------------
TC0INTR20:
;尾翼控制
	b0bts0	empennagestatflag
	jmp	TC0INTR30
	b0bset	ptempennage	
	decms	empennagebuf
	jmp	TC0INTR40
	b0bclr	ptempennage	
	b0bset	empennagestatflag
	mov_	empennagelowbuf,lowcontant		;set low level 10ms
	mov_	empennagebuf,empennageloadbuf	;set high level 
;-----
TC0INTR30:
	decms	empennagelowbuf
	jmp	TC0INTR40
	b0bclr	empennagestatflag	
;-----------------------
TC0INTR40:
;螺旋槳控制
	b0bts0	screwstatflag
	jmp	TC0INTR50
	b0bset	ptscrew	
	decms	screwbuf
	jmp	TC0INTR60
	b0bclr	ptscrew	
	b0bset	screwstatflag
	mov_	screwlowbuf,lowcontant	;set low level 10ms
	mov_	screwbuf,screwloadbuf	;set high level 
;-----
TC0INTR50:
	decms	screwlowbuf
	jmp	TC0INTR60
	b0bclr	screwstatflag	
;-----------------------
TC0INTR60:
;擺尾控制
	b0bts0	swingstatflag
	jmp	TC0INTR70
	b0bset	ptswing	
	decms	swingbuf
	jmp	TC0INTR80
	b0bclr	ptswing	
	b0bset	swingstatflag
	mov_	swinglowbuf,lowcontant	;set low level 10ms
	mov_	swingbuf,swingloadbuf	;set high level 
;-----
TC0INTR70:
	decms	swinglowbuf
	jmp	TC0INTR80
	b0bclr	swingstatflag
;-----
TC0INTR80:	
TC0INTR90:
	b0bclr	FTC0IRQ
	jmp	Interrupt90
;--------------------------------------------------------
;--------------------------------------------------------
t0intr:
	mov	a,#6
	sub	a,levecount
	bts0	fc
	jmp	t0intr10
	bset	errorflag
	jmp	t0intr90
t0intr10:
	incms	levecount
	nop
t0intr90:
	bclr	ft0irq
	jmp	interrupt90
;--------------------------------------------------------
;--------------------------------------------------------
P00INTR:		
	bclr	fp00irq
	bclr	ft0enb
	mov	a,t0c
	bts1	singleflag	
	jmp	disasm100				
	mov	level,a		;上升沿進入
	jmp	disasm200		
disasm100:
	mov	leveh,a		;下降沿進入
	jmp	disasm200
disasm200:
	clr	t0c		
	bset	ft0enb	
	bts0	singleflag
	jmp	disasm220
	jmp 	disasm210
disasm210:
	@p00_edge	1		;下降沿進入，設為上升沿中斷
	bts0	firstflag
	jmp	disasm221
	mov	a,levecount
	bts0	fz
	jmp	disasm80
	bset	firstflag
	clr	levecount
	clr	inputcount
	jmp	disasm90	
disasm220:
	@p00_edge	2		;上升沿進入，設為下降沿中斷	
	mov	a,#35;60;56			;448us=8x56
	sub	a,level
	bts0	fc
	jmp	disasm80		;小于448us，出錯
	mov	a,#95;70;75			;600us=8x75
	sub	a,level
	bts0	fc
	jmp	disasm90		;小于600us，返回
	jmp	disasm80		;大于600us，出錯
disasm221:
	incms	inputcount
	nop
	mov	a,inputcount
	sub	a,#5
	bts1	fc
	jmp	disasm70;chksignaltable
	jmp	disasm80	
disasm80:
	@p00_edge	2		;出錯，設為下降沿中斷
	clr	inputcount
	clr	levecount
	bclr	firstflag
	jmp	disasm90
disasm70:
	bset	chksigntableflag	
disasm90:
P00INTR90:
	jmp	Interrupt90
;***********************************************
mnint:
	bts1	chksigntableflag
	jmp	mnint20
	bclr	chksigntableflag
	clr	tablecount
	bclr	fp00ien
chksignaltable:				;查詢控制信號的檔位
	b0mov	y,#signaltable$m
	b0mov	z,#signaltable$l
	mov	a,tablecount
	add	z,a
	mov	a,#0
	adc	y,a
	movc
	sub	a,leveh
	bts0	fc
	jmp	chksignaltableend	
	jmp	signaltableadd
signaltableadd:
	incms	tablecount
	nop
	jmp	chksignaltable
chksignaltableend:
	mov	a,tablecount	
	bts0	fz
	jmp	mnint008		;error
	sub	a,#22
	bts0	fc
	jmp	mnint008		;error
	mov	a,tablecount
	add	a,zerocontant
	mov	gearbuf,a
	mov	a,inputcount
	@jmp_a	5
	jmp	mnint00
	jmp	gearwing
	jmp	gearempennage
	jmp	gearscrew
	jmp	gearswing
;	jmp	mnint00	
gearwing:
	mov	a,gearbuf
	cmprs	a,wingbuf1
	jmp	gearwing10
	incms	samebuf1
	nop
	mov	a,samebuf1
	sub	a,samecontant
	bts1	fc
	jmp	mnint00	
	clr	samebuf1
	call	completey

gearwing50:
	bts1	csyenbflag
	jmp	gearwing20
gearwing60:
	bts1	csyenbflag
	jmp	mnint00
	bts1	wingaddflag
	jmp	gearwing30
	bclr	wingaddflag
	mov	a,equalcontant
	add	a,csycalibratebuf
	add	a,csycalibratebuf
	mov	wingbuf1,a
	jmp	gearwing20

gearwing30:
	bts1	wingsubflag
	jmp	mnint00
	bclr	wingsubflag
	mov	a,equalcontant
	sub	a,csycalibratebuf
	sub	a,csycalibratebuf
	mov	wingbuf1,a
	jmp	gearwing20
gearwing20:
	bts1	add1flag
	jmp	gearwing001
	mov	a,wingbuf1
	sub	a,wingloadbuf
	bts1	fc
	jmp	gearwing002
	sub	a,#2
	bts1	fc
	jmp	mnint00
	bclr	add1flag
	jmp	gearwing002
gearwing001:
	mov	a,wingloadbuf
	sub	a,wingbuf1
	bts1	fc
	jmp	gearwing002
	sub	a,#2
	bts1	fc
	jmp	mnint00
	bset	add1flag
	jmp	gearwing002	
gearwing002:	
	mov	a,wingbuf1
	mov	wingloadbuf,a
	mov	wingbuf,a
	clr	samebuf1
	bclr	wingstatflag
	jmp	mnint00
gearwing10:
	mov	a,gearbuf
	mov	wingbuf1,a	

	jmp	mnint00
gearempennage:
	mov	a,gearbuf
	cmprs	a,empennagebuf1
	jmp	gearempennage10
	incms	samebuf2
	nop
	mov	a,samebuf2
	sub	a,samecontant
	bts1	fc
	jmp	mnint00
	clr	samebuf2
	call	completex
gearempennage50:
	bts1	csxenbflag
	jmp	gearempennage20
gearempennage60:
	bts1	csxenbflag
	jmp	mnint00
	bts1	empenaddflag
	jmp	gearempennage30
	bclr	empenaddflag
	mov	a,equalcontant
	add	a,csxcalibratebuf
	add	a,csxcalibratebuf
	mov	empennagebuf1,a
	jmp	gearempennage20

gearempennage30:
	bts1	empensubflag
	jmp	mnint00
	bclr	empensubflag
	mov	a,equalcontant
	sub	a,csxcalibratebuf
	sub	a,csxcalibratebuf
	mov	empennagebuf1,a
	jmp	gearempennage20
gearempennage20:
	bts1	add2flag
	jmp	gearempennage001
	mov	a,empennagebuf1
	sub	a,empennageloadbuf
	bts1	fc
	jmp	gearempennage002
	sub	a,#2
	bts1	fc
	jmp	mnint00
	bclr	add2flag
	jmp	gearempennage002
gearempennage001:
	mov	a,empennageloadbuf
	sub	a,empennagebuf1
	bts1	fc
	jmp	gearempennage002
	sub	a,#2
	bts1	fc
	jmp	mnint00
	bset	add2flag
	jmp	gearempennage002	
gearempennage002:
	mov	a,empennagebuf1
	mov	empennageloadbuf,a
	mov	empennagebuf,a
	bclr	empennagestatflag
	jmp	mnint00
gearempennage10:
	mov	empennagebuf1,a	
	clr	samebuf2
	jmp	mnint00
gearscrew:
	mov	a,gearbuf
	cmprs	a,screwbuf1
	jmp	gearscrew10
	incms	samebuf3
	nop
	mov	a,samebuf3
	sub	a,samecontant
	bts1	fc
	jmp	mnint00
	bts1	add3flag
	jmp	gearscrew001
	mov	a,screwbuf1
	sub	a,screwloadbuf
	bts1	fc
	jmp	gearscrew002
	sub	a,#2
	bts1	fc
	jmp	mnint00
	bclr	add3flag
	jmp	gearscrew002
gearscrew001:
	mov	a,screwloadbuf
	sub	a,screwbuf1
	bts1	fc
	jmp	gearscrew002
	sub	a,#2
	bts1	fc
	jmp	mnint00
	bset	add3flag
	jmp	gearscrew002	
gearscrew002:
	mov	a,screwbuf1
	mov	screwloadbuf,a
	mov	screwbuf,a
	clr	samebuf3
	bclr	screwstatflag
	jmp	mnint00
gearscrew10:
	mov	a,gearbuf
	mov	screwbuf1,a	
	clr	samebuf3
	jmp	mnint00
gearswing:
	mov	a,gearbuf
	cmprs	a,swingbuf1
	jmp	gearswing10
	incms	samebuf4
	nop
	mov	a,samebuf4
	sub	a,samecontant
	bts1	fc
	jmp	mnint00
	bts1	add4flag
	jmp	gearswing001
	mov	a,swingbuf1
	sub	a,swingloadbuf
	bts1	fc
	jmp	gearswing002
	sub	a,#2
	bts1	fc
	jmp	mnint00
	bclr	add4flag
	jmp	gearswing002
gearswing001:
	mov	a,swingloadbuf
	sub	a,swingbuf1
	bts1	fc
	jmp	gearswing002
	sub	a,#2
	bts1	fc
	jmp	mnint00
	bset	add4flag
	jmp	gearswing002	
gearswing002:	
	mov	a,swingbuf1
	mov	swingloadbuf,a
	mov	swingbuf,a
	clr	samebuf4
	bclr	swingstatflag
	jmp	mnint00
gearswing10:
	mov	a,gearbuf
	mov	swingbuf1,a	
	clr	samebuf4
	jmp	mnint00
mnint008:
	@p00_edge	2		;出錯，設為下降沿中斷
	clr	inputcount
	clr	levecount
	bclr	firstflag
mnint00:
	bset	fp00ien
mnint01:
	bts1	t5msflag
	jmp	mnint90
	bclr	t5msflag
	call	dkeychat	
mnint10:
	incms	timer500ms
	nop
	mov 	a,timer500ms
	sub	a,#100
	bts1	fc
	jmp	mnint90
	clr	timer500ms
	bset	t500msflag
	jmp	mnint90
mnint20:
	bts1	changeflag
	jmp	mnint30
	bclr	changeflag
	jmp	gearwing60
mnint30:
	bset	changeflag
	jmp	gearempennage60
mnint90:
	ret
;***********************************************	
	
signaltable:
	dw	034h	;0
	dw	03ah	;0	;1
	dw	041h	;1
	dw	048h	;2
	dw	04fh	;3
	dw	056h	;4
	dw	05dh	;5
	dw	064h	;6
	dw	06bh	;7	
	dw	072h	;8
	dw	079h	;9
	dw	080h	;10
	dw	087h	;11
	dw	08eh	;12
	dw	095h	;13
	dw	09ch	;14
	dw	0a3h	;15	
	dw	0aah	;16	
	dw	0b1h	;17
	dw	0b8h	;18
	dw	0bfh	;19
	dw	0c6h	;20	;20	
	dw	0ffh	;21

/*
	dw	09dh	;21
	dw	0a2h	;22
	dw	0a7h	;23
	dw	0ach	;24	
	dw	0b3h	;25
	dw	0b8h	;26
	dw	0bdh	;27
	dw	0c2h	;28	
	dw	0ffh;0c7h	;29	;

	dw	0cdh	;30
	dw	0d2h	;31
	dw	0d7h	;32	
	dw	0ddh	;33
	dw	0e1h	;34
	dw	0e7h	;35
	dw	0ech	;36	
	dw	0f2h	;37
	dw	0f7h	;38
	dw	0fch	;39
	dw	0ffh	;40
*/