;=============================================================================
;
;   File i2c.asm
;
;   I2C Interface
;   
    ; Compass0
;   HMC5883L's read address (8-Bit):    0x3D
;   HMC5883L's write address (8-Bit):   0x3C
;   MMA8452Q's read address (8-Bit):    0x39
;   MMA8452Q's write address (8-Bit):   0x38
;
    ; Compass1
;   LSM303D's read address (8-Bit):    0x3D
;   LSM303D's write address (8-Bit):   0x3C
;
    ; Compass2
;   LSM303AGR's Compass read address (8-Bit):    0x3D
;   LSM303AGR's Compass write address (8-Bit):   0x3C
;   LSM303AGR's Acceleration read address (8-Bit):    0x33
;   LSM303AGR's Acceleration write address (8-Bit):   0x32
;    
    ; Compass3
;   LSM303C's Compass read address (8-Bit):    0x3D
;   LSM303C's Compass write address (8-Bit):   0x3C
;   LSM303C's Acceleration read address (8-Bit):    0x3B
;   LSM303C's Acceleration write address (8-Bit):   0x3A
;
    ; RX Circuity
;   RX Circuity read address (8-Bit):    0x51
;   RX Circuity write address (8-Bit):   0x50
;
;
;   Copyright (c) 2012, JD Gascuel, HeinrichsWeikamp, all right reserved.
;=============================================================================
; HISTORY
;  2012-08-22 : [mH] Creation


#include "hwos.inc"                ; Mandatory header
#include "wait.inc"
#include "math.inc"
#include "external_flash.inc"

i2c    CODE

I2C_TX:
	movwf		SSP1BUF
	rcall		WaitMSSP
	bra 		I2C_WaitforACK      ; Returns...

I2C_TwoBytesRX_div16:       ; Get two bytes and devide lo:hi/16 (signed)
    rcall       I2C_OneByteRX       ; Get one byte
	movff		SSP1BUF,hi          ; Data Byte
    rcall       I2C_OneByteRX       ; Get one byte
	movff		SSP1BUF,lo          ; Data Byte
I2C_TwoBytesRX_div16_2:     ; devide lo:hi/16 (signed) only
    bcf			STATUS,C
    btfsc       hi,7        ; Copy sign bit to carry
    bsf         STATUS,C
    rrcf		hi          ; /2
    rrcf		lo
I2C_TwoBytesRX_div8_2:     ; devide lo:hi/8 (signed) only
    bcf			STATUS,C
    btfsc       hi,7        ; Copy sign bit to carry
    bsf         STATUS,C
    rrcf		hi          ; /4
    rrcf		lo
    bcf			STATUS,C
    btfsc       hi,7        ; Copy sign bit to carry
    bsf         STATUS,C
    rrcf		hi          ; /8
	rrcf		lo
    bcf			STATUS,C
    btfsc       hi,7        ; Copy sign bit to carry
    bsf         STATUS,C
    rrcf		hi          ; /16
	rrcf		lo
    return

    global  I2C_RX_accelerometer
I2C_RX_accelerometer:
    btfsc   compass_type3	    ; compass3
    bra	    I2C_RX_accelerometer_compass3    ; yes
    btfsc   compass_type2	    ; compass2
    bra	    I2C_RX_accelerometer_compass2    ; yes
    btfsc   compass_type	    ; compass1?
    bra	    I2C_RX_accelerometer_compass1    ; yes
I2C_RX_accelerometer_compass0:
    bsf			SSP1CON2,SEN		; Start condition
    rcall		WaitMSSP
    movlw		0x38                ; address
    rcall       I2C_TX
    movlw		0x00
    rcall       I2C_TX
    bsf			SSP1CON2,RSEN		; Repeated start condition (!)
    rcall		WaitMSSP
    movlw		0x39                ; address
    rcall       I2C_TX

    rcall       I2C_OneByteRX       ; Get Status Byte
    movf        SSP1BUF,W

    ; Non-flipped screen:
    ; Chip orientation on the PCB requires
    ; Original = Corrected
    ; x = -x
    ; y = -y
    ; z = -z

    ; Flipped screen:
    ; Chip orientation on the PCB requires
    ; Original = Corrected
    ; x = x
    ; y = y
    ; z = -z

    rcall       I2C_TwoBytesRX_div16 ; Get two bytes and devide /16 (signed)
    btfsc       flip_screen             ; 180° rotation ?
    bra         I2C_RX_accelerometer2   ; Yes
    comf        hi                    ; 16bit sign change.
    negf        lo
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        hi,F                ; YES: do it.
I2C_RX_accelerometer2:
    movff       lo,accel_DX+0
    movff       hi,accel_DX+1       ; Copy result

    rcall       I2C_TwoBytesRX_div16 ; Get two bytes and devide /16 (signed)
    btfsc       flip_screen             ; 180° rotation ?
    bra         I2C_RX_accelerometer3   ; Yes
    comf        hi                    ; 16bit sign change.
    negf        lo
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        hi,F                ; YES: do it.
I2C_RX_accelerometer3:
    movff       lo,accel_DY+0
    movff       hi,accel_DY+1       ; Copy result

    rcall       I2C_OneByteRX       ; Get one byte
	movff		SSP1BUF,hi          ; Data Byte
	bsf			SSP1CON2, RCEN      ; Enable recieve mode
    rcall		WaitMSSP
; According to datasheet there should be no Master Acknowlegde for the last Byte (accel_DZ+0)...
	movff		SSP1BUF,lo          ; Data Byte

    rcall       I2C_TwoBytesRX_div16_2; devide lo:hi/16 (signed) only
    comf        hi                    ; 16bit sign change.
    negf        lo
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        hi,F                ; YES: do it.
    movff       lo,accel_DZ+0
    movff       hi,accel_DZ+1       ; Copy result

	bsf			SSP1CON2,PEN		; Stop condition
	bra		WaitMSSP	; (And return)

I2C_RX_accelerometer_compass1:
    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x3C                ; address
    rcall       I2C_TX
    movlw	b'10101000'	    ; 0x28 with auto-increment (MSB=1)
    rcall       I2C_TX
    bsf		SSP1CON2,RSEN	    ; Repeated start condition (!)
    rcall	WaitMSSP
    movlw	0x3D                ; address
I2C_RX_accelerometer_compass1_xx:   ; compass2 and 3 continue here... 
    rcall       I2C_TX
    ; Non-flipped screen:
    ; Chip orientation on the PCB requires
    ; Original = Corrected
    ; x = -x (Compass 1)
    ; x = x (Compass 2)
    ; y = -y
    ; z = -z

    ; Flipped screen:
    ; Chip orientation on the PCB requires
    ; Original = Corrected
    ; x = x (Compass 1)
    ; x = -x (Compass 2)
    ; y = y
    ; z = -z
    
    ; Dump the accelerator data
    rcall       I2C_OneByteRX       
    movff	SSP1BUF,lo  ;accel_DX+0
    rcall       I2C_OneByteRX       
    movff	SSP1BUF,hi  ;accel_DX+1
    rcall       I2C_TwoBytesRX_div16_2; devide lo:hi/16 (signed) only
    btfss	compass_type2	    ; compass 2?
    bra		I2C_RX_accelerometer1_c1    ; No, compass 1
    ; compass 2
    btfss       flip_screen             ; 180° rotation ?
    bra		I2C_RX_accelerometer2_c1    ; No, continue with normal compass1 routines for Y and Z
    ; flipped compass 2, negate x
    comf        hi                    ; 16bit sign change.
    negf        lo
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        hi,F                ; YES: do it.
    bra		I2C_RX_accelerometer2_c1    ; continue with normal compass1 routines for Y and Z
    
I2C_RX_accelerometer1_c1:    
    btfsc       flip_screen             ; 180° rotation ?
    bra         I2C_RX_accelerometer2_c1   ; Yes
    ; non-flipped compass 1, negate x
    comf        hi                    ; 16bit sign change.
    negf        lo
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        hi,F                ; YES: do it.
I2C_RX_accelerometer2_c1:
    ; flipped compass 1, non-flipped compass 2
    movff       lo,accel_DX+0
    movff       hi,accel_DX+1       ; Copy result
    rcall       I2C_OneByteRX       
    movff	SSP1BUF,lo  ;accel_DY+0
    rcall       I2C_OneByteRX       
    movff	SSP1BUF,hi  ;accel_DY+1
    
    rcall       I2C_TwoBytesRX_div16_2; devide lo:hi/16 (signed) only
    btfsc       flip_screen             ; 180° rotation ?
    bra         I2C_RX_accelerometer3_c1   ; Yes
    comf        hi                    ; 16bit sign change.
    negf        lo
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        hi,F                ; YES: do it.
I2C_RX_accelerometer3_c1:
    movff       lo,accel_DY+0
    movff       hi,accel_DY+1       ; Copy result

    rcall       I2C_OneByteRX     
    movff	SSP1BUF,lo  ;accel_DZ+0
    bsf		SSP1CON2, RCEN      ; Enable recieve mode
    rcall	WaitMSSP
; According to datasheet there should be no Master Acknowlegde for the last Byte (accel_DZ+1)...
    movff	SSP1BUF,hi  ;accel_DZ+1
    bsf		SSP1CON2,PEN		; Stop condition
    rcall	WaitMSSP
    rcall       I2C_TwoBytesRX_div16_2; devide lo:hi/16 (signed) only
    comf        hi                    ; 16bit sign change for Z
    negf        lo
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        hi,F                ; YES: do it.
    movff       lo,accel_DZ+0
    movff       hi,accel_DZ+1       ; Copy result
    return

I2C_RX_accelerometer_compass2:
    bsf		SSP1CON2,SEN	    ; Start condition
    rcall	WaitMSSP
    movlw	0x32                ; address
    rcall       I2C_TX
    movlw	b'10101000'	    ; 0x28 with auto-increment (MSB=1)
    rcall       I2C_TX
    bsf		SSP1CON2,RSEN	    ; Repeated start condition (!)
    rcall	WaitMSSP
    movlw	0x33                ; address
    bra		I2C_RX_accelerometer_compass1_xx
    
I2C_RX_accelerometer_compass3:
    bsf		SSP1CON2,SEN	    ; Start condition
    rcall	WaitMSSP
    movlw	0x3A                ; address
    rcall       I2C_TX
    movlw	0x28		    ; 0x28 (OUT_X_L_A)
    rcall       I2C_TX
    bsf		SSP1CON2,RSEN	    ; Repeated start condition (!)
    rcall	WaitMSSP
    movlw	0x3B                ; address
    bra		I2C_RX_accelerometer_compass1_xx
    
I2C_OneByteRX:
    bsf		SSP1CON2, RCEN      ; Enable recieve mode
    rcall	WaitMSSP
    bsf		SSP1CON2,ACKEN	    ; Master acknowlegde
    bra		WaitMSSP	    ; And return!

    global  I2C_RX_compass
I2C_RX_compass:
    btfsc   compass_type3	    ; compass3
    bra	    I2C_RX_compass3    ; yes
    btfsc   compass_type2	    ; compass2
    bra	    I2C_RX_compass2    ; yes
    btfsc   compass_type	    ; compass1?
    bra	    I2C_RX_compass1    ; yes
I2C_RX_compass0:
    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x3C                ; address
    rcall       I2C_TX
    movlw	0x03
    rcall       I2C_TX
    bsf		SSP1CON2,PEN		; Stop condition
    rcall	WaitMSSP

    bcf		PIR1,SSP1IF
    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x3D                ; address
    rcall       I2C_TX

    ; Compass IC sends data in following order:
    ; x MSB
    ; x LSB
    ; z MSB
    ; z LSB
    ; y MSB
    ; y LSB

    ; Non-flipped screen
    ; Chip orientation on the PCB requires
    ; Original = Corrected
    ; x = -y
    ; z = z
    ; y = x

    ; Flipped screen
    ; Chip orientation on the PCB requires
    ; Original = Corrected
    ; x = y
    ; z = z
    ; y = -x

    rcall       I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,compass_DY+1; Data Byte
    rcall       I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,compass_DY+0; Data Byte
    btfsc       flip_screen         ; 180° rotation ?
    bra         I2C_RX_compass0_2     ; Yes
    banksel compass_DY
    comf        compass_DY+1        ; 16bit sign change.
    negf        compass_DY+0
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        compass_DY+1,F      ; YES: do it.
I2C_RX_compass0_2:
    banksel common
    rcall       I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,compass_DZ+1; Data Byte
    rcall       I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,compass_DZ+0; Data Byte
    rcall       I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,compass_DX+1; Data Byte
    bsf		SSP1CON2, RCEN      ; Enable recieve mode
    rcall	WaitMSSP
    movff	SSP1BUF,compass_DX+0; Data Byte
    bsf		SSP1CON2,PEN	    ; Stop condition
    rcall	WaitMSSP
    btfss       flip_screen         ; 180° rotation ?
    return                          ; No, done.
    ; Yes, flip X
    banksel compass_DX
    comf        compass_DX+1        ; 16bit sign change.
    negf        compass_DX+0
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        compass_DX+1,F      ; YES: do it.
    banksel common
    return
    
I2C_RX_compass1: ; New compass
    bsf		SSP1CON2,SEN	    ; Start condition
    rcall	WaitMSSP
    movlw	0x3C                ; address
    rcall       I2C_TX
    movlw	b'10001000'	    ; 0x08 with auto-increment (MSB=1)
    rcall       I2C_TX
    bsf		SSP1CON2,RSEN	    ; Repeated start condition (!)
    rcall	WaitMSSP
    movlw	0x3D                ; address
    rcall       I2C_TX
;rcall	WaitMSSP	    ; Needed? (mH)
    rcall	I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,lo	    ; Data Byte
    rcall	I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,hi	    ; Data Byte
    rcall	I2C_TwoBytesRX_div8_2
    movff	lo,compass_DX+0
    movff	hi,compass_DX+1
    btfss       flip_screen         ; 180° rotation ?
    bra		I2C_RX_compass1_1              ; Yes
    ; Yes, flip X
    banksel compass_DX
    comf        compass_DX+1        ; 16bit sign change.
    negf        compass_DX+0
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        compass_DX+1,F      ; YES: do it.
    banksel common
I2C_RX_compass1_1:
    rcall	I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,lo	    ; Data Byte
    rcall	I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,hi	    ; Data Byte
    rcall	I2C_TwoBytesRX_div8_2
    movff	lo,compass_DY+0
    movff	hi,compass_DY+1
    btfss       flip_screen         ; 180° rotation ?
    bra         I2C_RX_compass1_2   ; Yes
    ; Yes, flip Y
    banksel compass_DY
    comf        compass_DY+1        ; 16bit sign change.
    negf        compass_DY+0
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        compass_DY+1,F      ; YES: do it.
I2C_RX_compass1_2:
    banksel common
    rcall	I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,lo	    ; Data Byte
    bsf		SSP1CON2, RCEN      ; Enable recieve mode
    rcall	WaitMSSP
    movff	SSP1BUF,hi	    ; Data Byte
    rcall	I2C_TwoBytesRX_div8_2
    movff	lo,compass_DZ+0
    movff	hi,compass_DZ+1
    bsf		SSP1CON2,PEN	    ; Stop condition
    bra		WaitMSSP ;(And return)

I2C_RX_compass2:    ; newest compass
    bsf		SSP1CON2,SEN	    ; Start condition
    rcall	WaitMSSP
    movlw	0x3C                ; address
    rcall       I2C_TX
    movlw	0xE8		    ; 0x68 with auto-increment (MSB=1)
    rcall       I2C_TX
    bsf		SSP1CON2,RSEN	    ; Repeated start condition (!)
    rcall	WaitMSSP
    movlw	0x3D                ; address
    rcall       I2C_TX
I2C_RX_compass2_xx:    ; compass 3 enters here
;    rcall	WaitMSSP
    rcall	I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,lo	    ; Data Byte
    rcall	I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,hi	    ; Data Byte
;    rcall	I2C_TwoBytesRX_div8_2
    btfsc       flip_screen         ; 180° rotation ?
    bra		I2C_RX_compass2_1   ; Yes, do nothing with X
    ; No, flip X
    comf        hi                  ; 16bit sign change.
    negf        lo
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        hi,F                ; YES: do it.
I2C_RX_compass2_1:   
    movff	lo,compass_DX+0
    movff	hi,compass_DX+1
    rcall	I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,lo	    ; Data Byte
    rcall	I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,hi	    ; Data Byte
;    rcall	I2C_TwoBytesRX_div8_2
    btfss       flip_screen         ; 180° rotation ?
    bra		I2C_RX_compass2_2   ; No, do nothing with Y
    ; Yes, flip Y
    comf        hi                  ; 16bit sign change.
    negf        lo
    btfsc       STATUS,C            ; Carry to propagate ?
    incf        hi,F                ; YES: do it.
I2C_RX_compass2_2:   
    movff	lo,compass_DY+0
    movff	hi,compass_DY+1
    rcall	I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,lo	    ; Data Byte
    rcall	I2C_OneByteRX       ; Get one byte
    movff	SSP1BUF,hi	    ; Data Byte
    ;rcall	I2C_TwoBytesRX_div8_2
    movff	lo,compass_DZ+0
    movff	hi,compass_DZ+1
    bsf		SSP1CON2,PEN	    ; Stop condition
    bra		WaitMSSP ;(And return)

I2C_RX_compass3:
    bsf		SSP1CON2,SEN	    ; Start condition
    rcall	WaitMSSP
    movlw	0x3C                ; address
    rcall       I2C_TX
    movlw	0xA8		    ; 0x28 with auto-increment (MSB=1)
    rcall       I2C_TX
    bsf		SSP1CON2,RSEN	    ; Repeated start condition (!)
    rcall	WaitMSSP
    movlw	0x3D                ; address
    rcall       I2C_TX
    bra		I2C_RX_compass2_xx
    
    global  I2C_init_compass
I2C_init_compass:
    bsf     compass_enabled
    bcf	    compass_type2
    bcf	    compass_type3
    
    ; probe compass 3
    bsf	SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x3A			; Address byte + Write bit
    movwf	SSP1BUF			; control byte
    rcall	WaitMSSP
    btfss	SSP1CON2,ACKSTAT	; ACK?
    bsf	    compass_type3		; ACK send. compass2 present
    bsf	SSP1CON2,PEN		; Stop condition
    rcall	WaitMSSP
    
    btfsc   compass_type3
    bra	    I2C_init_compass3		; Compass3
    
    ; probe compass 2
    bsf	SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x32			; Address byte + Write bit
    movwf	SSP1BUF			; control byte
    rcall	WaitMSSP
    btfss	SSP1CON2,ACKSTAT	; ACK?
    bsf	    compass_type2		; ACK send. compass2 present
    bsf	SSP1CON2,PEN		; Stop condition
    rcall	WaitMSSP
    
    btfsc   compass_type2
    bra	    I2C_init_compass2		; Compass2
    ; Check for compass0 or compass1...
    bsf	    compass_type	    ; set flag
    bsf	    SSP1CON2,SEN		; Start condition
    rcall   WaitMSSP
    movlw   0x3C                ; address
    rcall   I2C_TX
    movlw   0x0F
    rcall   I2C_TX
    bsf	    SSP1CON2,PEN		; Stop condition
    rcall   WaitMSSP
    bcf	    PIR1,SSP1IF
    bsf	    SSP1CON2,SEN		; Start condition
    rcall   WaitMSSP
    movlw   0x3D                ; address
    rcall   I2C_TX
    rcall   I2C_OneByteRX       ; Get one byte
    movlw   0x49		; 0x49 = Compass1
    cpfseq  SSP1BUF
    bcf	    compass_type	    ; clear flag
    bsf	    SSP1CON2,PEN		; Stop condition
    rcall   WaitMSSP

    btfsc   compass_type	    ; compass1?
    bra	    I2C_init_compass1	    ; yes
; init compass0
    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x3C                ; address
    rcall       I2C_TX
    movlw	0x00
    rcall       I2C_TX
;	movlw	b'01101001'        ; ConfigA:  3Hz, 8 Samples averaged, Test Mode (Positive Bias)
    movlw	b'01101000'        ; ConfigA:  3Hz, 8 Samples averaged
    rcall       I2C_TX
I2C_init_compass_common:
    movff       opt_compass_gain,i2c_temp1    ; 0-7 (230LSB/Gauss to 1370LSB/Gauss)
    swapf       i2c_temp1,F
    comf        i2c_temp1,F
    bcf         STATUS,C
    rlcf        i2c_temp1
    movf        i2c_temp1,W
    clrf        i2c_temp1
    rcall       I2C_TX
    movlw	b'00000000'        ; Continous Mode
    rcall       I2C_TX
    bsf	SSP1CON2,PEN		; Stop condition
    bra		WaitMSSP	; (And return)

I2C_init_compass1:
    bsf		SSP1CON2,SEN	; Start condition
    rcall	WaitMSSP
    movlw	0x3C            ; address
    rcall       I2C_TX
    movlw	0x9F		; 1F with auto-increment (MSB=1)
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL0
    rcall       I2C_TX
    movlw	b'00101111'	; CTRL1 (6,25Hz, BDU=0, x,y,z = ON)
    rcall       I2C_TX
    movlw	b'11000000'	; CTRL2 (50Hz, +/-2g, 
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL3
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL4
    rcall       I2C_TX
    movlw	b'01100100'	; CTRL5 HIGH res, 6,25Hz
    rcall       I2C_TX
init_compass1_common:
    movff       opt_compass_gain,i2c_temp1    ; 0-7 (230LSB/Gauss to 1370LSB/Gauss)
    movlw	b'01100000'	; CTRL6 Full scale (+/-12 Gauss -> 2730LSB/Gauss)
    dcfsnz	i2c_temp1,F	; = 1?
    movlw	b'01100000'	; Yes, CTRL6 Full scale (+/-12 Gauss -> 2730LSB/Gauss)
    dcfsnz	i2c_temp1,F	; = 2?
    movlw	b'01000000'	; Yes, CTRL6 (+/-8 Gauss)
    dcfsnz	i2c_temp1,F	; = 3?
    movlw	b'01000000'	; Yes, CTRL6 (+/-8 Gauss)
    dcfsnz	i2c_temp1,F	; = 4?
    movlw	b'00100000'	; Yes, CTRL6 (+/-4 Gauss)
    dcfsnz	i2c_temp1,F	; = 5?
    movlw	b'00100000'	; Yes, CTRL6 (+/-4 Gauss)
    dcfsnz	i2c_temp1,F	; = 6?
    movlw	b'00000000'	; Yes, CTRL6 (+/-2 Gauss)
    dcfsnz	i2c_temp1,F	; = 7?
    movlw	b'00000000'	; Yes, CTRL6 (+/-2 Gauss)
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL7 Continuous Mode
    rcall       I2C_TX
    bsf		SSP1CON2,PEN		; Stop condition
    bra		WaitMSSP	; (And return)
    
I2C_init_compass2:
    bsf		SSP1CON2,SEN	; Start condition
    rcall	WaitMSSP
    movlw	0x3C            ; address
    rcall       I2C_TX
    movlw	0xE0		; 0x60 with auto-increment (MSB=1)
    rcall       I2C_TX
    movlw	b'10000000'	; CFG_REG_A_M (10Hz, Continuous) 0x60
    rcall       I2C_TX
    movlw	b'00000001'	; CFG_REG_B_M (Low-Pass Filter enabled) 0x61 (set pulse is released every 63 ODR)
    rcall       I2C_TX
    movlw	b'00010000'	; CFG_REG_C_M BDU=1 0x62
    rcall       I2C_TX
    bsf		SSP1CON2,PEN	; Stop condition
    bra		WaitMSSP	;(And return)

I2C_init_compass3:
    bsf		SSP1CON2,SEN	; Start condition
    rcall	WaitMSSP
    movlw	0x3C            ; address
    rcall       I2C_TX
    movlw	0xA0		; 0x20 with auto-increment (MSB=1)
    rcall       I2C_TX
    movlw	b'01110000'	; CTRL_REG1_M (10Hz) 0x20
    rcall       I2C_TX
    movlw	b'01100000'	; CTRL_REG2_M (Full-scale: +/- 16gauss) 0x21
    rcall       I2C_TX
    movlw	b'01000000'	; CTRL_REG3_M (Continuous) 0x22
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL_REG4_M (Z in Low-power mode) 0x23
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL_REG5_M 0x24
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL_REG5_M 0x24
    rcall       I2C_TX
    bsf		SSP1CON2,PEN	; Stop condition
    bra		WaitMSSP	;(And return)
    
    
WaitMSSP:
	decfsz		i2c_temp1,F          ; check for timeout during I2C action
	bra		WaitMSSP2
	bra		I2CFail             ; timeout occured
WaitMSSP2:
	btfss		PIR1,SSP1IF
	bra		WaitMSSP
	clrf		i2c_temp1
	bcf		PIR1,SSP1IF
	return

I2C_WaitforACK:
	btfss		SSP1CON2,ACKSTAT     ; checks for ACK bit from slave
	return
I2CFail:
	rcall		I2CReset            ; I2C Reset
	bcf		PIR1,SSP1IF
	clrf		i2c_temp1
	bsf		i2c_error_flag		; set error flag
	return

I2CReset:                           ; Something went wrong (Slave holds SDA low?)
	clrf		SSP1CON1            ; wake-up slave and reset entire module
	clrf		SSP1CON2
	clrf		SSP1STAT
	bcf			TRISC,3             ; SCL OUTPUT
	bsf			TRISC,4             ; SDA Input
	bcf			PORTC,3
	movlw		d'9'
	movwf		i2c_temp1            ; clock-out 9 clock cycles manually
I2CReset_1:
	bsf			PORTC,3             ; SCL=1
	nop
	nop
	nop
	nop
	btfsc		PORTC,4             ; SDA=1?
	bra			I2CReset_2          ; =1, SDA has been released from slave
	bcf			PORTC,3             ; SCL=0
	nop
	nop
	bcf			PORTC,3
	nop
	nop
	decfsz		i2c_temp1,F
	bra         I2CReset_1          ; check for nine clock cycles
I2CReset_2:
	bsf			TRISC,3             ; SCL Input
	clrf		SSP1CON1            ; setup I²C Mode
	WAITMS		d'10'               ; Reset-Timeout for I2C devices
	movlw		b'00000000'         ; with slew rate control
	movwf		SSP1STAT
	movlw		b'00101000'
	movwf		SSP1CON1
	movlw		b'00000000'
	movwf		SSP1CON2
    movlw       0x27
	movwf		SSP1ADD
	return
    
    
    global  I2C_sleep_compass
I2C_sleep_compass:
    bcf         compass_enabled
    btfsc	compass_type3	    ; compass3?
    bra		I2C_sleep_compass3  ; yes
    btfsc	compass_type2	    ; compass2?
    bra		I2C_sleep_compass2  ; yes
    btfsc	compass_type	    ; compass1?
    bra		I2C_sleep_compass1  ; yes
I2C_sleep_compass0:
    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x3C                ; address
    rcall       I2C_TX
    movlw	0x00
    rcall       I2C_TX
    movlw	b'01101000'        ; ConfigA
    rcall       I2C_TX
    movlw	b'00100000'        ; ConfigB
    rcall       I2C_TX
    movlw	b'00000010'        ; Idle Mode
    rcall       I2C_TX
    bsf		SSP1CON2,PEN		; Stop condition
    bra		WaitMSSP	; (And return)

I2C_sleep_compass1:
    bsf		SSP1CON2,SEN	    ; Start condition
    rcall	WaitMSSP
    movlw	0x3C                ; address
    rcall       I2C_TX
    movlw	0x20		    ; CTRL_REG1
    rcall       I2C_TX
    movlw	b'00000000'         ; data for CTRL_REG1: acceleration sensor Power-down mode
    rcall       I2C_TX
    bsf		SSP1CON2,PEN	    ; Stop condition
    rcall	WaitMSSP
    bsf		SSP1CON2,SEN	    ; Start condition
    rcall	WaitMSSP
    movlw	0x3C                ; address
    rcall       I2C_TX
    movlw	0x26		    ; CTRL_REG7
    rcall       I2C_TX
    movlw	b'00000010'         ; data for CTRL_REG7: magnetic sensor Power-down mode
    rcall       I2C_TX
    bsf		SSP1CON2,PEN	    ; Stop condition
    bra		WaitMSSP	;(And return)

    
I2C_sleep_compass2:
    ; magnetic
    bsf		SSP1CON2,SEN	; Start condition
    rcall	WaitMSSP
    movlw	0x3C            ; address
    rcall       I2C_TX
    movlw	0xE0		; 0x60 with auto-increment (MSB=1)
    rcall       I2C_TX
    movlw	b'00000011'	; CFG_REG_A_M (Idle mode) 0x60
    rcall       I2C_TX
    movlw	b'00000100'	; CFG_REG_B_M 0x61  (set pulse is released only at power-on after PD condition)
    rcall       I2C_TX
    movlw	b'01010001'	; CFG_REG_C_M 0x62
    rcall       I2C_TX
    movlw	b'00000000'	; INT_CTRL_REG_M 0x63
    rcall       I2C_TX

    bsf		SSP1CON2,PEN	; Stop condition
    bra		WaitMSSP	; (And return)

I2C_sleep_compass3:
    ; magnetic
    bsf		SSP1CON2,SEN	; Start condition
    rcall	WaitMSSP
    movlw	0x3C            ; address
    rcall       I2C_TX
    movlw	0xA2		; 0x22 with auto-increment (MSB=1)
    rcall       I2C_TX
    movlw	b'01000010'	; CTRL_REG3_M (Power-down) 0x22
    rcall       I2C_TX
    bsf		SSP1CON2,PEN	; Stop condition
    bra		WaitMSSP	;(And return)
    
I2C_sleep_accelerometer2:
    ; accelerometer
    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x32            ; address
    rcall       I2C_TX
    movlw	0x9F		; 1F with auto-increment (MSB=1)
    rcall       I2C_TX
    movlw	b'00000000'	; TEMP_CFG_REG_A (Temp sensor off) 0x1F
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL_REG1_A (All off) 0x20
    rcall       I2C_TX
    bsf		SSP1CON2,PEN	; Stop condition
    bra		WaitMSSP	; (And return)

I2C_sleep_accelerometer3:
    ; accelerometer
    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x3A            ; address
    rcall       I2C_TX
    movlw	0x20
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL_REG1_A (100Hz, x,y,z = OFF) 0x20
    rcall       I2C_TX
    bsf		SSP1CON2,PEN		; Stop condition
    bra		WaitMSSP	; (And return)
    
   global  I2C_init_accelerometer
I2C_init_accelerometer:
    btfsc   compass_type3	    ; compass3?
    bra	    I2C_init_accelerometer3 ; Yes.
    btfsc   compass_type2	    ; compass2?
    bra	    I2C_init_accelerometer2 ; Yes.
    btfsc   compass_type	    ; compass1?
    return			    ; yes, ignore

    rcall       I2C_sleep_accelerometer ; Regs can only be changed in St.By mode

    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x38                ; address
    rcall       I2C_TX
    movlw	0x0E                ; XYZ_DATA_CFG
    rcall       I2C_TX
    movlw	b'00000000'         ; High pass Filter=0 , +/- 2g range
    rcall       I2C_TX
    bsf		SSP1CON2,PEN		; Stop condition
    rcall	WaitMSSP


    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x38                ; address
    rcall       I2C_TX
    movlw	0x2A                ; CTRL_REG1
    rcall       I2C_TX
;	movlw		b'00110000'         ; CTRL_REG1: 160ms data rate, St.By Mode
    movlw	b'00110100'         ; CTRL_REG1: 160ms data rate, St.By Mode, reduced noise mode
    rcall       I2C_TX
    movlw	b'00000010'         ; CTRL_REG2: High Res in Active mode
    rcall       I2C_TX
    bsf		SSP1CON2,PEN		; Stop condition
    rcall	WaitMSSP

    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x38                ; address
    rcall       I2C_TX
    movlw	0x2A                ; CTRL_REG1
    rcall       I2C_TX
;	movlw		b'00110001'         ; CTRL_REG1: 160ms data rate, Active Mode
    movlw	b'00110101'         ; CTRL_REG1: 160ms data rate, St.By Mode, reduced noise mode, Active Mode
    rcall       I2C_TX
    bsf		SSP1CON2,PEN		; Stop condition
    bra		WaitMSSP	; (And return)
    
I2C_init_accelerometer2:
    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x32            ; address
    rcall       I2C_TX
    movlw	0x9F		; 1F with auto-increment (MSB=1)
    rcall       I2C_TX
    movlw	b'00000000'	; TEMP_CFG_REG_A (Temp sensor off) 0x1F
    rcall       I2C_TX
    movlw	b'01010111'	; CTRL_REG1_A (100Hz, x,y,z = ON) 0x20
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL_REG2_A 0x21
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL_REG3_A 0x22
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL_REG4_A (BDU=0, +/-2g) 0x23
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL_REG5_A 0x24
    rcall       I2C_TX
    bsf		SSP1CON2,PEN		; Stop condition
    bra		WaitMSSP	; (And return)

I2C_init_accelerometer3:
    bsf		SSP1CON2,SEN		; Start condition
    rcall	WaitMSSP
    movlw	0x3A            ; address
    rcall       I2C_TX
    movlw	0x20
    rcall       I2C_TX
    movlw	b'10011111'	; CTRL_REG1_A (10Hz, x,y,z = ON) 0x20
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL_REG2_A 0x21
    rcall       I2C_TX
    movlw	b'00000000'	; CTRL_REG3_A 0x22
    rcall       I2C_TX
    movlw	b'11001100'	; CTRL_REG4_A 0x23
    rcall       I2C_TX
    bsf		SSP1CON2,PEN		; Stop condition
    bra		WaitMSSP	; (And return)
    
    
    global  I2C_sleep_accelerometer
I2C_sleep_accelerometer:
    btfsc   compass_type3	    ; Compass3
    bra	    I2C_sleep_accelerometer3	; Yes
    btfsc   compass_type2	    ; Compass2
    bra	    I2C_sleep_accelerometer2	; Yes
    btfsc   compass_type	    ; compass1?
    return			    ; yes, ignore
    ; compass 0
	bsf			SSP1CON2,SEN		; Start condition
	rcall		WaitMSSP
	movlw		0x38                ; address
    rcall       I2C_TX
	movlw		0x2A                ; CTRL_REG1
    rcall       I2C_TX
	movlw		b'00000000'         ; St. By Mode
    rcall       I2C_TX
	bsf			SSP1CON2,PEN		; Stop condition
	bra	WaitMSSP	    ; (And return)
   
lt2942_init_again:
	clrf	i2c_temp1
	movlw	0x02						; Point to accumulated charge registers
	rcall	I2C_TX_GAUGE
	movff	battery_acumulated_charge+1,SSP1BUF	; Data Byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	movff	battery_acumulated_charge+0,SSP1BUF	; Data Byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	bsf		SSP1CON2,PEN				; Stop condition
	rcall	WaitMSSP
	movff	battery_acumulated_charge+1,sub_a+1
	movff	battery_acumulated_charge+0,sub_a+0
	; and init again...

	global	lt2942_init
lt2942_init:							; Setup Control register B
	clrf	i2c_temp1
	movlw	0x01						; Point to control reg B
	rcall	I2C_TX_GAUGE
	movlw	b'11111000'					; Automatic conversion every two seconds
	movff	WREG, SSP1BUF				; Data Byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	bsf		SSP1CON2,PEN				; Stop condition
	bra		WaitMSSP					; (And return)

	global	lt2942_get_status
lt2942_get_status:						; Read status register
	bcf		battery_gauge_available		; Clear flag
	clrf	i2c_temp1
	movlw	0x00						; Point to Status reg
	rcall	I2C_TX_GAUGE
	rcall	I2C_RX_GAUGE
	movff	SSP1BUF,WREG
	btfss	WREG,7						; 2942 found?
	bsf		battery_gauge_available		; Yes, set flag
	bsf		SSP1CON2,PEN				; Stop condition
	bra		WaitMSSP					; (And return)


	global	lt2942_get_voltage
lt2942_get_voltage:						; Read battery voltage registers
	clrf	i2c_temp1
	movlw	0x08						; Point to voltage registers
	rcall	I2C_TX_GAUGE
	rcall	I2C_RX_GAUGE
	bsf		SSP1CON2,ACKEN				; Master acknowledge
	rcall	WaitMSSP
	movff	SSP1BUF,xA+1
	bsf		SSP1CON2, RCEN				; Enable receive mode
	rcall	WaitMSSP
	movff	SSP1BUF,xA+0
	bsf		SSP1CON2,PEN				; Stop condition
	rcall	WaitMSSP

;	banksel	common
	; xA:2 loaded with raw values
	movlw	LOW  .6000
	movwf	xB+0
	movlw	HIGH .6000
	movwf	xB+1
	call	mult16x16					; xA*xB=xC

	; xC+3:xC+2  -> Result in mV

	; Update battery voltage in mV
	movff	xC+3,batt_voltage+1
	movff	xC+2,batt_voltage+0

	tstfsz	batt_voltage+1				; <256mV?
	return								; No, done.
	bra		lt2942_init					;(and return)

	global	lt2942_get_temperature
lt2942_get_temperature:		; Read temperature registers
	clrf	i2c_temp1
	movlw	0x0C            ; Point to temperature registers
	call	I2C_TX_GAUGE
	call	I2C_RX_GAUGE
	bsf		SSP1CON2,ACKEN	; Master acknowlegde
	rcall	WaitMSSP
	movff	SSP1BUF,xA+1
	bsf		SSP1CON2, RCEN	; Enable recieve mode
	rcall	WaitMSSP
	movff	SSP1BUF,xA+0
	bsf		SSP1CON2,PEN	; Stop condition
	rcall	WaitMSSP

;	banksel	common
    ; xA:2 loaded with raw values
    movlw   LOW     .6000
    movwf   xB+0
    movlw   HIGH    .6000
    movwf   xB+1
    call    mult16x16		;xA*xB=xC

    ; xC+3:xC+2  -> Result in 0.1K

    movlw   LOW	    max_allowed_battery_temp	    ; in 0.1K
    movwf   sub_a+0
    movlw   HIGH    max_allowed_battery_temp
    movwf   sub_a+1
    movff   xC+3,sub_b+1
    movff   xC+2,sub_b+0
    call    subU16  ;  sub_c = sub_a - sub_b (with UNSIGNED values)
    btfss   neg_flag
    return	; temp ok, return
    ; too hot, disable charge if currently charging
    btfss   cc_active
    return	; Not charging, return
    ; charging: Disable now
    bsf	    charge_disable
    bcf	    TRISE,2
    bsf	    battery_overtemp    ; =1: The battery was charged and temp was too high (Only cleared on POR)
    return

	global	lt2942_get_accumulated_charge
lt2942_get_accumulated_charge:			; Read accumulated charge and compute percent
	clrf	i2c_temp1
	movlw	0x00						; Point to status register
	rcall	I2C_TX_GAUGE
	rcall	I2C_RX_GAUGE
	bsf		SSP1CON2,ACKEN				; Master acknowledge
	rcall	WaitMSSP
	movff	SSP1BUF,gauge_status_byte

	bsf		SSP1CON2, RCEN				; Enable receive mode
	rcall	WaitMSSP					; Dummy read (Control byte)
	movf	SSP1BUF,W
	bsf		SSP1CON2,ACKEN				; Master acknowledge
	rcall	WaitMSSP

	bsf		SSP1CON2, RCEN				; Enable receive mode
	rcall	WaitMSSP
	movff	SSP1BUF,sub_a+1
	bsf		SSP1CON2,ACKEN				; Master acknowledge
	rcall	WaitMSSP

	bsf		SSP1CON2, RCEN				; Enable receive mode
	rcall	WaitMSSP
	movff	SSP1BUF,sub_a+0
	bsf		SSP1CON2,PEN				; Stop condition
	rcall	WaitMSSP

	movff	gauge_status_byte,sub_b+0	; copy into bank common
	btfsc	sub_b+0,0					; =1: UVLO Event
	rcall	lt2942_init_again

	movff	sub_a+1,battery_acumulated_charge+1	; Save raw value
	movff	sub_a+0,battery_acumulated_charge+0	; Save raw value

	; Compute batt_percent
	; (charge-battery_offset)/365
	movff	battery_offset+0,sub_b+0
	movff	battery_offset+1,sub_b+1
	call	subU16						; sub_c = sub_a - sub_b (with signed values)

	clrf	batt_percent				; Set to zero
	btfsc	neg_flag					; result negative?
	bra		lt2942_set_to_zero_percent	; Yes, keep LT2942 at zero percent and return

	; > Zero, set batt_percent properly
	movff	sub_c+0,xA+0
	movff	sub_c+1,xA+1
	movff	battery_capacity+0,xB+0
	movff	battery_capacity+1,xB+1
	call	div16x16					; xC = xA / xB with xA as remainder
	movff	xC+0,batt_percent
	return

lt2942_set_to_zero_percent:
	clrf	i2c_temp1
	movlw	0x02						; Point to accumulated charge registers
	rcall	I2C_TX_GAUGE
	movff	battery_offset+1,SSP1BUF
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	movff	battery_offset+0,SSP1BUF
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	bsf		SSP1CON2,PEN				; Stop condition
	bra		WaitMSSP					; (and return)

	global	lt2942_charge_done
lt2942_charge_done:						; Reset accumulating registers to 0xFFFF
	clrf	i2c_temp1
	movlw	0x02						; Point to accumulated charge registers
	rcall	I2C_TX_GAUGE
	setf	SSP1BUF						; Data Byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	setf	SSP1BUF						; Data Byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	bsf		SSP1CON2,PEN				; Stop condition
	bra		WaitMSSP					; (and return)

I2C_TX_GAUGE:							; Sends a byte to the LT2942 Gauge IC
	movwf	i2c_temp2					; Data byte
	bsf		SSP1CON2,SEN				; Start condition
	rcall	WaitMSSP
	movlw	b'11001000'					; Address byte + Write bit
	movwf	SSP1BUF						; control byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	movf	i2c_temp2,W
	bra		I2C_TX						; (and return)

I2C_RX_GAUGE:
	bsf		SSP1CON2,SEN				; Start condition
	rcall	WaitMSSP
	movlw	b'11001001'					; Address byte + Read bit
	movwf	SSP1BUF						; control byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	bsf		SSP1CON2, RCEN				; Enable receive mode
	bra		WaitMSSP					; (and return)


;-----------------------------------------------------------------------------
; Transmitter Functions

 IFDEF _rx_functions

	global	I2C_probe_OSTC_rx
I2C_probe_OSTC_rx:
	movlw	.5
	movwf	lo_temp
I2C_probe_OSTC_rx_1:
	bsf		SSP1CON2,SEN				; Start condition
	rcall	WaitMSSP
	movlw	0x50						; Address byte + Write bit
	movwf	SSP1BUF						; control byte
	rcall	WaitMSSP
	btfss	SSP1CON2,ACKSTAT			; ACK?
	bsf		ostc_rx_present				; ACK sent - OSTC_RX present!
	bsf		SSP1CON2,PEN				; Stop condition
	rcall	WaitMSSP
	btfss	ostc_rx_present				; Do we have the RX?
	return								; No, Done.
	WAITMS	.1
	bsf		SSP1CON2,SEN				; Start condition
	rcall	WaitMSSP
	movlw	0x50						; Address byte + Write bit
	movwf	SSP1BUF						; control byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	movlw	0x1B
	movwf	SSP1BUF						; Data Byte (Get firmware)
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	bsf		SSP1CON2,PEN				; Stop condition
	rcall	WaitMSSP
	WAITMS	.1
	bsf		SSP1CON2,SEN				; Start condition
	rcall	WaitMSSP
	movlw	0x51						; Address byte + Read bit
	movwf	SSP1BUF						; control byte
	rcall	WaitMSSP
	bsf		SSP1CON2, RCEN				; Enable receive mode
	rcall	WaitMSSP
	movff	SSP1BUF,rx_firmware+0
	bsf		SSP1CON2,ACKEN				; Master acknowledge
	rcall	WaitMSSP

	; last byte in read from RX circuity always with a NACK!
	bsf		SSP1CON2, RCEN				; Enable receive mode
	rcall	WaitMSSP
	movff	SSP1BUF,rx_firmware+1
	bsf		SSP1CON2,ACKDT
	bsf		SSP1CON2,ACKEN				; Master NOT acknowledge
	rcall	WaitMSSP
	bcf		SSP1CON2,ACKDT				; Reset ACKDT flag
	bsf		SSP1CON2,PEN				; Stop condition
	rcall	WaitMSSP

	; test for RX part not being ready during this read
	movff	rx_firmware+1,i2c_temp1
	movlw	.147
	cpfseq	i2c_temp1
	bra		I2C_probe_OSTC_rx_2			; not equal
	movff	rx_firmware+0,i2c_temp1
	movlw	.27
	cpfseq	i2c_temp1
	bra		I2C_probe_OSTC_rx_2			; not equal
	bsf		active_reset_ostc_rx
	WAITMS	.5
	bcf		active_reset_ostc_rx
	WAITMS	.250
	WAITMS	.250
	clrf	i2c_temp1
	decfsz	lo_temp,F					; try max. 5 times
	bra		I2C_probe_OSTC_rx_1
	bcf		ostc_rx_present				; Clear flag. Something is wrong
I2C_probe_OSTC_rx_2:
	clrf	i2c_temp1
	return


	global	I2C_get_tankdata
I2C_get_tankdata:
	bsf	SSP1CON2,SEN					; Start condition
	rcall	WaitMSSP
	movlw	0x50						; Address byte + Write bit
	movwf	SSP1BUF						; control byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	movlw	0x1E						; Read buffer2 (48 Bytes)
	movwf	SSP1BUF	; Data Byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	bsf		SSP1CON2,PEN				; Stop condition
	rcall	WaitMSSP
	WAITMS	.1

	; read 48 bytes
	bsf	SSP1CON2,SEN					; Start condition
	rcall	WaitMSSP
	movlw	0x51						; Address byte + read bit
	movwf	SSP1BUF						; control byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	movlw	.47							; 47 with ACK + 1 w/o ACK
	movwf	i2c_temp2
	lfsr	FSR2,rx_buffer+0
I2C_get_tankdata_loop_read:
	bsf	SSP1CON2, RCEN					; Enable receive mode
	rcall	WaitMSSP
	movff	SSP1BUF,POSTINC2
	bcf		SSP1CON2,ACKDT
	bsf		SSP1CON2,ACKEN				; Master acknowledge
	rcall	WaitMSSP
	decfsz	i2c_temp2,F
	bra		I2C_get_tankdata_loop_read

	; 1 w/o ACK
	bsf		SSP1CON2, RCEN				; Enable receive mode
	rcall	WaitMSSP
	movff	SSP1BUF,POSTINC2
	bsf		SSP1CON2,ACKDT
	bsf		SSP1CON2,ACKEN				; Master NOT acknowledge
	rcall	WaitMSSP
	bcf		SSP1CON2,ACKDT				; Reset ACKDT flag

	bsf		SSP1CON2,PEN				; Stop condition
	bra		WaitMSSP					;(and return)


	global	I2C_update_OSTC_rx
I2C_update_OSTC_rx:						; 992*64byte master loop
	bcf		i2c_error_flag				; clear error flag
	; write 64 bytes
	bsf		SSP1CON2,SEN				; Start condition
	rcall	WaitMSSP
	movlw	0x50						; Address byte + Write bit
	movwf	SSP1BUF						; control byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	lfsr	FSR2,buffer					; send buffer for verify
	movlw	.64
	movwf	i2c_temp2
I2C_update_OSTC_loop:					; 64byte flash page loop
	movff	up,POSTINC2					; store for verify
	movff	up,SSP1BUF
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	call	ext_flash_read_block		; Read one byte
	movwf	up							; prepare for transmit
	decfsz	i2c_temp2,F
	bra		I2C_update_OSTC_loop
	bsf		SSP1CON2,PEN				; Stop condition
	rcall	WaitMSSP
	WAITMS	.1

	; read 64 bytes
	bsf		SSP1CON2,SEN				; Start condition
	rcall	WaitMSSP
	movlw	0x51						; Address byte + read bit
	movwf	SSP1BUF						; control byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	lfsr	FSR2,buffer					; send buffer for verify
	movlw	.63							; 63 with ACK + 1 w/o ACK
	movwf	i2c_temp2
I2C_update_OSTC_loop_read:
	bsf		SSP1CON2, RCEN				; Enable receive mode
	rcall	WaitMSSP
	movf	SSP1BUF,W
	cpfseq	POSTINC2					; compare read-back with original
	bsf		i2c_error_flag				; Not equal, set flag
	bcf		SSP1CON2,ACKDT
	bsf		SSP1CON2,ACKEN				; Master acknowledge
	rcall	WaitMSSP
	decfsz	i2c_temp2,F
	bra		I2C_update_OSTC_loop_read

	; 1 w/o ACK
	bsf	SSP1CON2, RCEN					; Enable receive mode
	rcall	WaitMSSP
	movf	SSP1BUF,W
	cpfseq	POSTINC2					; compare read-back with original
	bsf		i2c_error_flag				; Not equal, set flag
	bsf		SSP1CON2,ACKDT
	bsf		SSP1CON2,ACKEN				; Master NOT acknowledge
	rcall	WaitMSSP
	bcf		SSP1CON2,ACKDT				; Reset ACKDT flag

	bsf		SSP1CON2,PEN				; Stop condition
	rcall	WaitMSSP
	WAITMS	.1

	bsf		SSP1CON2,SEN				; Start condition
	rcall	WaitMSSP
	movlw	0x50						; Address byte + Write bit
	movwf	SSP1BUF						; control byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	movlw	0x1F						; Write command!
	movwf	SSP1BUF						; Data Byte
	rcall	WaitMSSP
	rcall	I2C_WaitforACK
	bsf		SSP1CON2,PEN				; Stop condition
	rcall	WaitMSSP
	WAITMS	.5							; Required waiting time

	btfss	i2c_error_flag
	retlw	.0							; All ok
	retlw	.255						; an error occurred

 ENDIF

	END