; Elevation sensor processing
; by F1OAT (frible@teaser.fr)
; Nov 2006
;
; Designed for
;
; 	MXD2125 accelerometer
; 	PICAXE-08M
; 	ADM483 for P-BUS over RS-485

; PICAXE-08M connection

symbol	pin_x = 2	; pin #5 <= MXD2125 X output
symbol	pin_y = 4	; pin #3 <= MXD2125 Y output
symbol	pin_rts = 0	; pin #7 => ADM483 /RE and DE
symbol	pin_pbus = 1; pin #6 <=> ADM483 bidir RO and DI

; CORDIC section variables definition

symbol	x = w2
symbol	y = w3

symbol	ang = w4
symbol	ang_l = b8
symbol	ang_h	= b9

symbol	dx = w5
symbol	dy = w6

symbol	i = b0
symbol	d = b1
symbol  	neg = b2
symbol	tmp = b3


; PBUS section variables definition

symbol	devid_len = b0
symbol	checksum = b1
symbol	cpt = b2
symbol	cmd = b3
symbol	pbus_data = b4

symbol 	baudrate = N1200	; In fact, it is 2400 at 8 MHz

symbol	CMD_CPING = 0x5F
symbol	CMD_RECHO = 0x6F

symbol	MY_PBUS_DEVID_LEN = 0x10
symbol	N1 = MY_PBUS_DEVID_LEN + CMD_RECHO	; temp variable
symbol	CMD_RECHO_CHECKSUM = 256 - N1

; Main loop
		
	setfreq m8	; Switch to 8 MHz operations

	; Very simplified P-BUS input implementation
	; Can handle only 105F91 CPING frame for dev #1
	
wait_pbus:
	input pin_pbus
	low pin_rts
	serin pin_pbus,baudrate, (0x10, 0x5F, 0x91)

p_cordic:
	; CORDIC section	

	x = 0
	dx = 0
	y = 0
	dy = 0
	
	; Cumulate several pulses measurement to enhance accuracy
	
;	for i=1 to 16
		pulsin pin_x,1,ang	; high level pulse duration
		x = x + ang
		pulsin pin_x,0,ang	; low level pulse duration
		dx = dx + ang
		pulsin pin_y,1,ang	; high level pulse duration
		y = y + ang
		pulsin pin_y,0,ang	; low level pulse duration
		dy = dy + ang
;	next i
	
	neg = 0

	; Put vector in proper 90° quadrant
	
	if x >= dx then qx1
	x = dx - x
	ang = 2048	; 180°
	neg = 1 - neg
	goto qx2
qx1:
	x = x - dx
	ang = 4096	; 360°
qx2:
	if y >= dy then qy1
	y = dy - y
	neg = 1 - neg
	goto qy2
qy1:
	y = y - dy

qy2:
	d = 1


	; CORDIC iterations

	for i=0 to 9
		if y <= 32767 then pos
		y = 65535 - y
		neg = 1-neg
pos:		
		dx = x / d
		dy = y / d

		x = x + dy
		y = y - dx
				
		lookup i, (512,302,159,81,40,20,10,5,2,1), dx ; ATAN table for ang_premult=1024/90

		if neg=1 then sub
		ang = ang + dx
		goto nextstep
sub:
		ang = ang - dx 
nextstep:
		d = d*2
	next i
	
	ang = ang % 4096
		
	; Send P-BUS answer

send_answer:	
	devid_len = MY_PBUS_DEVID_LEN+2
	checksum = devid_len + CMD_RECHO + ang_l + ang_h
	checksum = 256 - checksum
	
	low pin_pbus
	high	pin_rts
	serout pin_pbus,baudrate,(devid_len,CMD_RECHO, ang_h, ang_l, checksum)
	
	goto wait_pbus