;****************************************************************************************
;Telescope Mount Controller IV
;(c) Martin Cibulski
;
;EEPROM access routines
;
;based on application note AVR100 by Atmel
;***************************************************************************


.MACRO	e_rw_block
	ldi	xh	,high(@0)
	ldi	xl	,low(@0)
	ldi	r16	,@1
	call	eep_rw_block
.ENDMACRO

.MACRO	e_rw_byte
	ldi	xh	,high(@0)
	ldi	xl	,low(@0)
	ldi	r16	,1
	call	eep_rw_block
.ENDMACRO

.MACRO	e_rw_word
	ldi	xh	,high(@0)
	ldi	xl	,low(@0)
	ldi	r16	,2
	call	eep_rw_block
.ENDMACRO

.MACRO	e_rw_long
	ldi	xh	,high(@0)
	ldi	xl	,low(@0)
	ldi	r16	,4
	call	eep_rw_block
.ENDMACRO

.MACRO	e_rw_float
	ldi	xh	,high(@0)
	ldi	xl	,low(@0)
	ldi	r16	,8
	call	eep_rw_block
.ENDMACRO


;****************************************************************************************

.DSEG
eep_cur_profile:	.byte	1			;default profile (0-3)

;****************************************************************************************

.ESEG


.EQU	E_NO_OF_PROFILES	= 4

eep_prof_default:
	.db	1			;0-3 !

eep_profile_0:
	;simulator
	.db	MODE_ALTAZ			;operate in AltAz mode

	.db	0x3F,0x40,0x15,0xBF,0x92,0x17,0x27,0x1A	;alg_az_per_step	4,9087e-4 = 2*PI/(200*64)
	.db	0x3F,0x40,0x15,0xBF,0x92,0x17,0x27,0x1A	;alg_alt_per_step	4,9087e-4 = 2*PI/(200*64)
	.db	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00	;alg_alt_start	0.00
	.db	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00	;alg_z1
	.db	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00	;alg_z2
	.db	1			;mco_z1z2_active
	.db	0			;alg_ana_eep_open
	.db	1			;alg_search_z3_yn
	.db	0			;mot_a_inverted
	.db	0			;mot_b_inverted

	.db	0x00,0x00,0x00,0x80		;speed setpoint 1A
	.db	0x00,0x00,0x00,0x80		;speed setpoint 1B
	.db	0x00,0x00,0x02,0x00		;speed setpoint 2A
	.db	0x00,0x00,0x02,0x00		;speed setpoint 2B
	.db	0x00,0x00,0x06,0x00		;speed setpoint 3A
	.db	0x00,0x00,0x06,0x00		;speed setpoint 3B
	.db	0x00,0x00,0x10,0x00		;speed setpoint 4A
	.db	0x00,0x00,0x10,0x00		;speed setpoint 4B
	.db	0x00,0x00,0x20,0x00		;speed setpoint 5A
	.db	0x00,0x00,0x20,0x00		;speed setpoint 5B
	.db	0x00,0x00,0x00,0x80		;handbox acceleration A
	.db	0x00,0x00,0x00,0x80		;handbox acceleration B
	.db	0x00,0x00,0x01,0x00		;tracking acceleration A
	.db	0x00,0x00,0x01,0x00		;tracking acceleration B

	.db	16			;A3973 fixed off time A  (0-31)
	.db	16			;A3973 fixed off time B  (0-31)
	.db	8			;A3973 fast decay time A (0-15)
	.db	8			;A3973 fast decay time B (0-15)
	.db	A39_BLANK_12			;A3973 blank time A      (_4,_6,_8,_12)
	.db	A39_BLANK_12			;A3973 blank time B      (_4,_6,_8,_12)
	.db	A39_RANGE_LOW			;A3973 range A           (_LOW,_HIGH)
	.db	A39_RANGE_LOW			;A3973 range B           (_LOW,_HIGH)
	.db	A39_RECT_DISABLE			;A3973 sync rectification(active, disabled, passive, low side)
	.db	A39_RECT_DISABLE			;A3973 sync rectification(active, disabled, passive, low side)
	.db	high(-19),low(-19)		;coeff of sin(3x)
	.db	high( -1),low( -1)		;coeff of sin(5x)
	.db	0,0			;coeff of sin(7x)

eep_profile_1:
	;14" scope
	.db	MODE_ALTAZ			;operate in AltAz mode

	.db	0x3E,0x98,0x1D,0xB8,0x94,0x87,0x42,0x61	;alg_az_per_step	3.5935134843e-7	
	.db	0x3E,0x94,0xD2,0x95,0x32,0x2C,0x9B,0x41	;alg_alt_per_step	3.1028075591e-7
	.db	0x3F,0xEC,0x9F,0x97,0x1F,0xF8,0x4F,0xCD	;alg_alt_start	0,8944812416 = 51.25 degrees
	.db	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00	;alg_z1
	.db	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00	;alg_z2
	.db	1			;mco_z1z2_active
	.db	0			;alg_ana_eep_open
#ifdef SIMULATE
	.db	0			;alg_search_z3_yn
#else
	.db	1			;alg_search_z3_yn
#endif
	.db	0			;mot_a_inverted
	.db	0			;mot_b_inverted

	.db	0x00,0x00,0x08,0x00		;speed setpoint 1A
	.db	0x00,0x00,0x08,0x00		;speed setpoint 1B
	.db	0x00,0x00,0x40,0x00		;speed setpoint 2A
	.db	0x00,0x00,0x40,0x00		;speed setpoint 2B
	.db	0x00,0x01,0x00,0x00		;speed setpoint 3A
	.db	0x00,0x01,0x00,0x00		;speed setpoint 3B
	.db	0x00,0x03,0x00,0x00		;speed setpoint 4A
	.db	0x00,0x03,0x00,0x00		;speed setpoint 4B
	.db	0x00,0x0C,0x00,0x00		;speed setpoint 5A
	.db	0x00,0x0C,0x00,0x00		;speed setpoint 5B
	.db	0x00,0x00,0x80,0x00		;handbox acceleration A
	.db	0x00,0x00,0x80,0x00		;handbox acceleration B
	.db	0x00,0x00,0x80,0x00		;tracking acceleration A
	.db	0x00,0x00,0x80,0x00		;tracking acceleration B

	.db	7			;A3973 fixed off time A  (0-31)
	.db	7			;A3973 fixed off time B  (0-31)
	.db	3			;A3973 fast decay time A (0-15)
	.db	3			;A3973 fast decay time B (0-15)
	.db	A39_BLANK_12			;A3973 blank time A      (_4,_6,_8,_12)
	.db	A39_BLANK_12			;A3973 blank time B      (_4,_6,_8,_12)
	.db	A39_RANGE_HIGH			;A3973 range A           (_LOW,_HIGH)
	.db	A39_RANGE_HIGH			;A3973 range B           (_LOW,_HIGH)
	.db	A39_RECT_PASSIVE			;A3973 sync rectification(active, disabled, passive, low side)
	.db	A39_RECT_PASSIVE			;A3973 sync rectification(active, disabled, passive, low side)
	.db	0,17			;coeff of sin(3x)
	.db	0,0			;coeff of sin(5x)
	.db	0,0			;coeff of sin(7x)

eep_profile_2:
	;focault tester, positioner mode
	.db	MODE_POSITIONER			;operate as positioner

	.db	0x3F,0x04,0x7A,0xE1,0x47,0xAE,0x14,0x7B	;mm/microstep 0.0000390625 = 1/(400*64)
	.db	0x3F,0x04,0x7A,0xE1,0x47,0xAE,0x14,0x7B	;mm/microstep 0.0000390625 = 1/(400*64)
	.db	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00	;alg_alt_start = 0.0
	.db	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00	;alg_z1 = 0.0
	.db	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00	;alg_z2 = 0.0
	.db	0			;mco_z1z2_active
	.db	0			;alg_ana_eep_open
	.db	0			;alg_search_z3_yn
	.db	0			;mot_a_inverted
	.db	1			;mot_b_inverted

	.db	0x00,0x00,0x08,0x00		;speed setpoint 1A
	.db	0x00,0x00,0x08,0x00		;speed setpoint 1B
	.db	0x00,0x00,0x40,0x00		;speed setpoint 2A
	.db	0x00,0x00,0x40,0x00		;speed setpoint 2B
	.db	0x00,0x01,0x00,0x00		;speed setpoint 3A
	.db	0x00,0x01,0x00,0x00		;speed setpoint 3B
	.db	0x00,0x03,0x00,0x00		;speed setpoint 4A
	.db	0x00,0x03,0x00,0x00		;speed setpoint 4B
	.db	0x00,0x0C,0x00,0x00		;speed setpoint 5A
	.db	0x00,0x0C,0x00,0x00		;speed setpoint 5B
	.db	0x00,0x00,0x40,0x00		;handbox acceleration A
	.db	0x00,0x00,0x40,0x00		;handbox acceleration B
	.db	0x00,0x00,0x20,0x00		;tracking acceleration A
	.db	0x00,0x00,0x20,0x00		;tracking acceleration B

	.db	16			;A3973 fixed off time A  (0-31)
	.db	16			;A3973 fixed off time B  (0-31)
	.db	8			;A3973 fast decay time A (0-15)
	.db	8			;A3973 fast decay time B (0-15)
	.db	A39_BLANK_12			;A3973 blank time A      (_4,_6,_8,_12)
	.db	A39_BLANK_12			;A3973 blank time B      (_4,_6,_8,_12)
	.db	A39_RANGE_LOW			;A3973 range A           (_LOW,_HIGH)
	.db	A39_RANGE_LOW			;A3973 range B           (_LOW,_HIGH)
	.db	A39_RECT_DISABLE			;A3973 sync rectification(active, disabled, passive, low side)
	.db	A39_RECT_DISABLE			;A3973 sync rectification(active, disabled, passive, low side)
	.db	high( 22),low( 22)		;coeff of sin(3x)
	.db	high( -3),low( -3)		;coeff of sin(5x)
	.db	high(  0),low(  0)		;coeff of sin(7x)

eep_profile_3:
	;focault tester, speed mode
	.db	MODE_SPEED			;operate in speed mode

	.db	0x3F,0x04,0x7A,0xE1,0x47,0xAE,0x14,0x7B	;mm/microstep 0.0000390625 = 1/(400*64)
	.db	0x3F,0x04,0x7A,0xE1,0x47,0xAE,0x14,0x7B	;mm/microstep 0.0000390625 = 1/(400*64)
	.db	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00	;alg_alt_start = 0.0
	.db	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00	;alg_z1 = 0.0
	.db	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00	;alg_z2 = 0.0
	.db	0			;mco_z1z2_active
	.db	0			;alg_ana_eep_open
	.db	0			;alg_search_z3_yn
	.db	1			;mot_a_inverted
	.db	1			;mot_b_inverted

	.db	0x00,0x00,0x08,0x00		;speed setpoint 1A
	.db	0x00,0x00,0x08,0x00		;speed setpoint 1B
	.db	0x00,0x00,0x40,0x00		;speed setpoint 2A
	.db	0x00,0x00,0x40,0x00		;speed setpoint 2B
	.db	0x00,0x01,0x00,0x00		;speed setpoint 3A
	.db	0x00,0x01,0x00,0x00		;speed setpoint 3B
	.db	0x00,0x03,0x00,0x00		;speed setpoint 4A
	.db	0x00,0x03,0x00,0x00		;speed setpoint 4B
	.db	0x00,0x0C,0x00,0x00		;speed setpoint 5A
	.db	0x00,0x0C,0x00,0x00		;speed setpoint 5B
	.db	0x00,0x00,0x80,0x00		;handbox acceleration A
	.db	0x00,0x00,0x80,0x00		;handbox acceleration B
	.db	0x00,0x00,0x80,0x00		;tracking acceleration A
	.db	0x00,0x00,0x80,0x00		;tracking acceleration B

	.db	16			;A3973 fixed off time A  (0-31)
	.db	16			;A3973 fixed off time B  (0-31)
	.db	8			;A3973 fast decay time A (0-15)
	.db	8			;A3973 fast decay time B (0-15)
	.db	A39_BLANK_12			;A3973 blank time A      (_4,_6,_8,_12)
	.db	A39_BLANK_12			;A3973 blank time B      (_4,_6,_8,_12)
	.db	A39_RANGE_LOW			;A3973 range A           (_LOW,_HIGH)
	.db	A39_RANGE_LOW			;A3973 range B           (_LOW,_HIGH)
	.db	A39_RECT_DISABLE			;A3973 sync rectification(active, disabled, passive, low side)
	.db	A39_RECT_DISABLE			;A3973 sync rectification(active, disabled, passive, low side)
	.db	high(-10),low(-10)		;coeff of sin(3x)
	.db	0,0			;coeff of sin(5x)
	.db	0,0			;coeff of sin(7x)

.EQU	E_PROFILE_SIZE	= eep_profile_1 - eep_profile_0


;****************************************************************************************
;Load/Save RAM variables from/to EEPROM profile
;
;Parameters:
;r16	profile number	0 .. E_NO_OF_PROFILES-1
;r17	save=1 load=0
;****************************************************************************************

.CSEG
eep_save_profile:
	ldi	r17	,1
	clt
	cpi	r16	,E_NO_OF_PROFILES
	brcs	eep_spr_inrange
	
	ret			;return false

eep_load_profile:
	clr	r17
	clt
	cpi	r16	,E_NO_OF_PROFILES
	brcs	eep_lpr_inrange
	
	ret			;return false

eep_lpr_inrange:
	sts	eep_cur_profile	,r16
eep_spr_inrange:
	ldi	zh	,high(eep_profile_0)
	ldi	zl	,low(eep_profile_0)
	ldi	r18	,E_PROFILE_SIZE
	mul	r16	,r18
	add	zl	,r0
	adc	zh	,r1

	push	r16

	e_rw_byte	main_op_mode
	e_rw_float	mco_az_per_step
	e_rw_float	mco_alt_per_step
	e_rw_float	mco_alt_start
	e_rw_float	mco_z1
	e_rw_float	mco_z2
	e_rw_byte	mco_z1z2_active
	e_rw_byte	alg_eep_open
	e_rw_byte	alg_search_z3_yn
	e_rw_byte	mot_a_inverted
	e_rw_byte	mot_b_inverted

	e_rw_long	mot_a_ssp1_raw
	e_rw_long	mot_b_ssp1
	e_rw_long	mot_a_ssp2_raw
	e_rw_long	mot_b_ssp2
	e_rw_long	mot_a_ssp3_raw
	e_rw_long	mot_b_ssp3
	e_rw_long	mot_a_ssp4_raw
	e_rw_long	mot_b_ssp4
	e_rw_long	mot_a_ssp5
	e_rw_long	mot_b_ssp5
	e_rw_long	mot_a_hacc
	e_rw_long	mot_b_hacc
	e_rw_long	mot_a_gacc
	e_rw_long	mot_b_gacc

	e_rw_byte	a39_offtimA
	e_rw_byte	a39_offtimB
	e_rw_byte	a39_dectimA
	e_rw_byte	a39_dectimB
	e_rw_byte	a39_blanktimA
	e_rw_byte	a39_blanktimB
	e_rw_byte	a39_rangeA
	e_rw_byte	a39_rangeB
	e_rw_byte	a39_synrecA
	e_rw_byte	a39_synrecB

	e_rw_word	a39_sin_coeff3
	e_rw_word	a39_sin_coeff5
	e_rw_word	a39_sin_coeff7

	tst	r17
	breq	eep_spr_loaded

	PUTS_ROM	msg_profile_saved1
	pop	r16
	subi	r16	,-'1'
	call	putc_func
	PUTS_ROM	msg_profile_saved2

	set				;return true
	ret

eep_spr_loaded:
	clr	r16
	call	mot_calc_az_speeds
	PUTS_ROM	msg_profile_loaded1
	pop	r16
	subi	r16	,-'1'
	call	putc_func
	PUTS_ROM	msg_profile_loaded2

	call	alg_init

	set				;return true
	ret

eep_spr_saved:

;****************************************************************************************
;Write one byte into EEPROM
;
;Parameters:
;r16	byte
;zh:zl	EEPROM adress
;****************************************************************************************

.CSEG

eep_write:
	sbic	EECR	,EEWE		;if EEWE not clear
	rjmp	eep_write			;    wait more

	out 	EEARH	,zh		;output address high for 8515
	out	EEARL	,zl		;output address low for 8515

	out	EEDR	,r16		;output data
	cli
	sbi 	EECR	,EEMWE		;set master write enable, remove if 1200 is used	
	sbi	EECR	,EEWE		;set EEPROM Write strobe
					;This instruction takes 4 clock cycles since
					;it halts the CPU for two clock cycles
	sei
	ret

;****************************************************************************************
;Read one byte from EEPROM
;
;Parameters:
;zh:zl	EEPROM adress
;
;Result:
;r16	byte
;****************************************************************************************

eep_read:
	sbic	EECR	,EEWE		;if EEWE not clear
	rjmp	eep_read			;    wait more

	out 	EEARH	,zh		;output address high
	out	EEARL	,zl		;output address low

	sbi	EECR	,EERE		;set EEPROM Read strobe
					;This instruction takes 4 clock cycles since
					;it halts the CPU for two clock cycles
	in	r16	,EEDR		;get data
	ret

		
;****************************************************************************************
;Read/Write RAM block (1-256 bytes) from/to EEPROM
;
;Parameters:
;r16	block size
;r17	1=write 0=read
;xh:xl	RAM start adress
;zh:zl	EEPROM start adress
;****************************************************************************************

eep_rw_block:
	ld	r0	,X		;RAM content

eep_rwb_wait:
	sbic	EECR	,EEWE		;wait if EEPROM busy
	rjmp	eep_rwb_wait

	out 	EEARH	,zh		;set EEPROM adress
	out	EEARL	,zl
	sbi	EECR	,EERE		;set EEPROM read strobe (4 CPU cyles)
	in	r1	,EEDR		;read EEPROM content

	tst	r17			;read(0) or write(1) ?
	brne	eep_rw_write

	st	X	,r1		;reading from EEPROM, put byte into RAM
	rjmp	eep_rwb_next

eep_rw_write:
	cp	r0	,r1		;writing to EEPROM, check if EEPROM = RAM
	breq	eep_rwb_next			;equal, nothing to do

	out	EEDR	,r0		;write byte into EEPROM
	cli
	sbi 	EECR	,EEMWE		;set master write enable
	sbi	EECR	,EEWE		;set EEPROM Write strobe (4 CPU cycles)
	sei

eep_rwb_next:
	adiw	zh:zl	,1
	adiw	xh:xl	,1
	dec	r16
	brne	eep_rw_block

	ret


;****************************************************************************************
;Read/write RAM block (1-256 bytes) from/to EEPROM
;
;Parameters:
;r16	block size
;zh:zl	EEPROM start adress
;xh:xl	RAM start adress
;****************************************************************************************

eep_read_block:
	sbic	EECR	,EEWE		;if EEWE not clear
	rjmp	eep_read_block			;   wait more

	out 	EEARH	,zh		;output address high
	out	EEARL	,zl		;output address low
	sbi	EECR	,EERE		;set EEPROM Read strobe
					;This instruction takes 4 clock cycles since
					;it halts the CPU for two clock cycles
	in	r0	,EEDR		;get data
	st	X+	,r0
	adiw	zh:zl	,1
	dec	r16
	brne	eep_read_block

	ret



;***********************************************************************
;Read RAM block (1-65535 bytes) from Program memory
;
;Parameters:
;r17:r16	Block size
;zh:zl	Program memory start adress (byte adress !)
;xh:xl	RAM start adress
;***********************************************************************

util_read_flash:
	push	r16
	push	r17

util_rdf_loop:
	lpm	r0	,Z+
	st	X+	,r0
	subi	r16	,1
	sbci	r17	,0
	brne	util_rdf_loop

	pop	r17
	pop	r16
	sub	zl	,r16
	sbc	zh	,r17
	sub	xl	,r16
	sbc	xh	,r17
	ret

;****************************************************************************************