--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/ms5541.asm	Wed Apr 24 19:22:45 2013 +0200
@@ -0,0 +1,512 @@
+;=============================================================================
+;
+;   File ms5541.asm
+;
+;   Sensor subroutines
+;
+;   Copyright (c) 2011, JD Gascuel, HeinrichsWeikamp, all right reserved.
+;=============================================================================
+; HISTORY
+;  2011-08-03 : [mH] moving from OSTC code
+
+#include    "ostc3.inc"                  ; Mandatory header
+#include 	"math.inc"					; Math routines
+
+sensors     CODE
+
+;=============================================================================
+; Expose internal variables, to ease debug:
+    global D1, D2
+    global C1, C2, C3, C4, C5, C6
+    global xdT, xdT2, OFF, SENS, amb_pressure_avg, temperature_avg
+
+;=============================================================================
+	global calculate_compensation
+calculate_compensation:
+	; xdT = D2 - C5    (s16 range -11.400 .. +12.350)
+	movf   C5+0,W                       ;  Get Value to be subtracted
+	subwf  D2+0,W             	        ;  Do the Low Byte
+	movwf  xdT+0
+	movf   C5+1,W                       ;  Then the high byte.
+	subwfb D2+1,W
+	movwf  xdT+1
+
+    ; Second order temperature calculation
+    ; xdT/128 is in range -89..+96, hence signed 8bit. dT/128 = (2*dT)/256
+    rlcf    xdT+0,W                     ; put hit bit in carry.
+    rlcf    xdT+1,W                     ; inject in high byte.
+    movwf   isr_xA+0                    ; and put result in low byte.
+    clrf    isr_xA+1
+    btfsc   xdT+1,7                     ; If dT < 0
+    setf    isr_xA+1                    ; then signextend to -1
+	movff	isr_xA+0,isr_xB+0           ; copy A to B
+	movff	isr_xA+1,isr_xB+1
+	call	isr_signed_mult16x16        ; dT*dT --> xC (32 bits)
+
+	; dT >= 0: divide by 8, ie. 3 shifts rights.
+	; dT <  0: divide by 2, ie. 1 shifts rights.
+    movlw   .3
+	btfss	xdT+1,7                     ; Was dT negative ?
+	movlw   .1
+calc_loop_1:
+    bcf     STATUS,C                    ;dT^2 is positive, so injected zeros.
+    rrcf    isr_xC+1,F
+    rrcf    isr_xC+0,F
+    decfsz  WREG
+    bra     calc_loop_1
+
+    movf    isr_xC+0,W                  ; dT2 = dT - (dT/128)*(dT/128)/(2 ...or... 8)
+	subwf   xdT+0,W
+	movwf   xdT2+0
+	movf    isr_xC+1,W
+	subwfb  xdT+1,W
+	movwf   xdT2+1
+
+    ; Calculate OFF = C2 + ((C4-250)*dT2)/2^12 + 10000
+    ; (range +9.246 .. +18.887)
+    movff   C4+0,isr_xA+0
+    movff   C4+1,isr_xA+1
+;	movlw   LOW(-.250)                  ; C4 - 250 --> A
+;	addwf	C4+0,W
+;	movwf   isr_xA+0
+;	movlw   -1                          ; HIGH(- .250) is not understood...
+;	addwfc  C4+1,W
+;	movwf   isr_xA+1
+	
+	movff   xdT2+0,isr_xB+0             ; dT2 --> B
+	movff   xdT2+1,isr_xB+1
+	call    isr_signed_mult16x16
+    movlw   .12-.8                      ; A 12bit shift = 1 byte + 4 bits.
+    call    isr_shift_C31
+
+    movlw   LOW(.10000)                 ; Add 10000
+    addwf   isr_xC+1,F
+    movlw   HIGH(.10000)
+    addwfc  isr_xC+2,F
+    
+    movf    C2+0,W                      ; Add C2, and save into OFF
+	addwf   isr_xC+1,W
+	movwf   OFF+0
+	movf	C2+1,W
+	addwfc  isr_xC+2,W
+	movwf   OFF+1
+
+    ; Calculate SENS = C1/2 + ((C3+200)*dT)/2^13 + 3000
+    movlw   LOW(.200)                   ; C3+200 --> A
+    addwf   C3+0,W
+    movwf   isr_xA+0
+    movlw   HIGH(.200)
+    addwfc  C3+1,W
+    movwf   isr_xA+1
+                                        ; B still contains dT2
+	call    isr_signed_mult16x16        ; A*B --> C
+    movlw   .13-.8                      ; A 13bit shift = 1 byte + 5 bits.
+    call    isr_shift_C31
+    
+    bcf     STATUS,C                    ; SENS = C1 / 2
+    rrcf    C1+1,W
+    movwf   SENS+1
+    rrcf    C1+0,W
+    movwf   SENS+0
+
+    movlw   LOW(.3000)                  ; Add 3000
+    addwf   isr_xC+1,F
+    movlw   HIGH(.3000)
+    addwfc  isr_xC+2,F
+
+    movf    isr_xC+1,W                  ; And sum into SENS
+    addwf   SENS+0,F
+    movf    isr_xC+2,W
+    addwfc  SENS+1,F
+
+    ; calculate amb_pressure = (sens * (d1-off))/2^12 + 1000
+    movf    OFF+0,W                      ; d1-off --> a
+    subwf   D1+0,W
+    movwf   isr_xA+0
+    movf    OFF+1,W
+    subwfb  D1+1,W
+    movwf   isr_xA+1
+
+	movff   SENS+0,isr_xB+0             ; sens --> b
+	movff   SENS+1,isr_xB+1
+	call    isr_signed_mult16x16
+    movlw   .12-.8                      ; a 12bit shift = 1 byte + 4 bits.
+    call    isr_shift_C31
+
+    movlw   LOW(.1000)                  ; add 1000
+    addwf   isr_xC+1,F
+    movlw   HIGH(.1000)
+    addwfc  isr_xC+2,F
+
+	banksel	common
+	btfss	simulatormode_active		; are we in simulator mode?
+	bra		calc_compensation_2			; no
+	banksel	isr_backup
+
+	movff	sim_pressure+0,isr_xC+1	    ; override readings with simulator values
+	movff	sim_pressure+1,isr_xC+2
+	
+calc_compensation_2:
+	banksel	isr_backup
+    movf    isr_xC+1,W                  ; Then sum_up to pressure averaging buffer.
+    addwf   amb_pressure_avg+0,F
+    movf    isr_xC+2,W
+    addwfc  amb_pressure_avg+1,F
+
+    ; calculate temp = 200 + dT*(C6+100)/2^11
+    movlw   LOW(.100)                   ; C6 + 100 --> A
+    addwf   C6+0,W
+    movwf   isr_xA+0
+    movlw   HIGH(.100)
+    addwfc  C6+1,W
+    movwf   isr_xA+1
+
+    movff   xdT2+0,isr_xB+0             ; dT2 --> B
+    movff   xdT2+1,isr_xB+1
+	call    isr_signed_mult16x16        ; A*B
+    movlw   .11-.8                      ; A 12bit shift = 1 byte + 3 bits.
+    call    isr_shift_C31
+
+    movlw   LOW(.200)                   ; Add 200
+    addwf   isr_xC+1,F
+    movlw   HIGH(.200)
+    addwfc  isr_xC+2,F
+
+    movf    isr_xC+1,W
+    addwf   temperature_avg+0,F
+    movf    isr_xC+2,W
+    addwfc  temperature_avg+1,F
+
+	return			                    ; Done.
+
+;=============================================================================
+	global	get_pressure_start
+get_pressure_start:
+	rcall	reset_MS5541
+	movlw	b'10100000'	;+3*high as start and 1+low as stop!
+get_pressure_start2:
+	movwf	isr1_temp
+	movlw	d'12'
+	rcall	send_data_MS5541
+	return
+
+	global get_pressure_value
+get_pressure_value:
+	btfsc	MS5541_miso				; Conversion done?
+	return							; No, Return
+	rcall	get_2bytes_MS5541
+	movff	dMSB,D1+1	
+	movff	dLSB,D1+0
+	return
+
+;=============================================================================
+	global	get_temperature_start
+get_temperature_start:
+	rcall	reset_MS5541
+	movlw	b'10010000'	;+3*high as start and 1+low as stop!
+	bra		get_pressure_start2	; continue in "get_pressure"
+
+	global	get_temperature_value
+get_temperature_value:
+	btfsc	MS5541_miso				; Conversion done?
+	return							; No, Return
+	rcall	get_2bytes_MS5541
+	movff	dMSB,D2+1
+	movff	dLSB,D2+0
+	return
+
+;=============================================================================
+	global	get_calibration_data
+get_calibration_data:
+	banksel	common
+	bsf		no_sensor_int			; disable sensor interrupts
+    banksel isr_backup              ; Back to Bank0 ISR data
+
+	rcall	reset_MS5541
+	movlw	b'01010100'	;+3*high as start and 1+low as stop!
+	movwf	isr1_temp
+	movlw	d'13'
+	rcall	send_data_MS5541
+	rcall	get_2bytes_MS5541
+	movff	dMSB,W1+1	
+	movff	dLSB,W1+0
+
+	movlw	b'01011000'	;+3*high as start and 1+low as stop!
+	movwf	isr1_temp
+	movlw	d'13'
+	rcall	send_data_MS5541
+	rcall	get_2bytes_MS5541
+	movff	dMSB,W2+1	
+	movff	dLSB,W2+0
+
+	movlw	b'01100100'	;+3*high as start and 1+low as stop!
+	movwf	isr1_temp
+	movlw	d'13'
+	rcall	send_data_MS5541
+	rcall	get_2bytes_MS5541
+	movff	dMSB,W3+1	
+	movff	dLSB,W3+0
+
+	movlw	b'01101000'	;+3*high as start and 1+low as stop!
+	movwf	isr1_temp
+	movlw	d'13'
+	rcall	send_data_MS5541
+	rcall	get_2bytes_MS5541
+	movff	dMSB,W4+1	
+	movff	dLSB,W4+0
+
+; calculate C1 (16Bit)
+	movff	W1+1, C1+1
+	bcf		STATUS,C
+	rrcf	C1+1
+	bcf		STATUS,C
+	rrcf	C1+1
+	bcf		STATUS,C
+	rrcf	C1+1
+	movff	W1+0, C1+0
+	bsf		STATUS,C
+	btfss	W1+1,0
+	bcf		STATUS,C
+	rrcf	C1+0
+	bsf		STATUS,C
+	btfss	W1+1,1
+	bcf		STATUS,C
+	rrcf	C1+0
+	bsf		STATUS,C
+	btfss	W1+1,2
+	bcf		STATUS,C
+	rrcf	C1+0
+
+; calculate C2 (16Bit)
+	movff	W2+0, C2+0
+	bsf		STATUS,C
+	btfss	W2+1,0
+	bcf		STATUS,C
+	rrcf	C2+0
+	bsf		STATUS,C
+	btfss	W2+1,1
+	bcf		STATUS,C
+	rrcf	C2+0
+	bsf		STATUS,C
+	btfss	W2+1,2
+	bcf		STATUS,C
+	rrcf	C2+0
+	bsf		STATUS,C
+	btfss	W2+1,3
+	bcf		STATUS,C
+	rrcf	C2+0
+	bsf		STATUS,C
+	btfss	W2+1,4
+	bcf		STATUS,C
+	rrcf	C2+0
+	bsf		STATUS,C
+	btfss	W2+1,5
+	bcf		STATUS,C
+	rrcf	C2+0
+
+	movff	W2+1, C2+1
+	bsf		STATUS,C
+	btfss	W1+0,0
+	bcf		STATUS,C
+	rrcf	C2+1
+	bsf		STATUS,C
+	btfss	W1+0,1
+	bcf		STATUS,C
+	rrcf	C2+1
+	bsf		STATUS,C
+	btfss	W1+0,2
+	bcf		STATUS,C
+	rrcf	C2+1
+	bcf		STATUS,C
+	rrcf	C2+1
+	bcf		STATUS,C
+	rrcf	C2+1
+	bcf		STATUS,C
+	rrcf	C2+1
+
+; calculate C3 (16Bit)
+	movff	W3+1,C3+0
+	bsf		STATUS,C
+	btfss	W3+0,7
+	bcf		STATUS,C
+	rlcf	C3+0
+	bsf		STATUS,C
+	btfss	W3+0,6
+	bcf		STATUS,C
+	rlcf	C3+0
+	clrf	C3+1
+	btfsc	W3+1,7
+	bsf		C3+1,1
+	btfsc	W3+1,6
+	bsf		C3+1,0
+	
+; calculate C4 (16Bit)	
+	movff	W4+1,C4+0
+	bsf		STATUS,C
+	btfss	W4+0,7
+	bcf		STATUS,C
+	rlcf	C4+0
+	clrf	C4+1
+	btfsc	W4+1,7
+	bsf		C4+1,0
+
+; C4=C4-250
+	movlw   LOW(-.250)                  ; C4 - 250 --> C4
+	addwf	C4+0,W
+	movwf   C4+0
+	movlw   -1                          ; HIGH(- .250) is not understood...
+	addwfc  C4+1,W
+	movwf   C4+1
+	
+; calculate C5 (16Bit)		
+	movff	W3+0,C5+0
+	bcf		C5+0,6
+	btfsc	W2+0,0
+	bsf		C5+0,6
+	bcf		C5+0,7
+	btfsc	W2+0,1
+	bsf		C5+0,7
+	clrf	C5+1
+	btfsc	W2+0,2
+	bsf		C5+1,0
+	btfsc	W2+0,3
+	bsf		C5+1,1
+	btfsc	W2+0,4
+	bsf		C5+1,2
+	btfsc	W2+0,5
+	bsf		C5+1,3
+
+    ; calculate C5 = UT1
+    ; C5 = 8*C5 + 10000 (u16 range 10.000 .. +42.760)
+	clrf	isr_xA+1
+	movlw	d'8'
+	movwf	isr_xA+0
+	movff	C5+0,isr_xB+0
+	movff	C5+1,isr_xB+1
+	call	isr_unsigned_mult16x16      ;isr_xA*isr_xB=isr_xC
+    movff   isr_xC+0,C5+0
+    movff   isr_xC+1,C5+1
+	movlw   LOW		d'10000'
+	addwf   C5+0,F
+	movlw   HIGH 	d'10000'
+	addwfc  C5+1,F    ; = 8*C5 + 10000
+
+; calculate C6 (16Bit)		
+	clrf	C6+1
+	movff	W4+0,C6+0
+	bcf		C6+0,7
+
+	banksel	common
+	bcf		no_sensor_int		; enable sensor interrupts
+	bcf		pressure_refresh 		; Clear flag
+    banksel isr_backup              ; Back to Bank0 ISR data
+
+	clrf		sensor_state_counter			; Then reset State counter
+
+	return
+
+;=============================================================================
+reset_MS5541_one:
+	bsf		MS5541_mosi
+    bra    send_clk_pulse  ; Send one high-low sequence on MS5541_clk  -> and return
+
+reset_MS5541_zero:
+	bcf		MS5541_mosi
+    bra    send_clk_pulse  ; Send one high-low sequence on MS5541_clk  -> and return
+
+reset_MS5541:
+	rcall	reset_MS5541_one			;0
+	rcall	reset_MS5541_zero
+	rcall	reset_MS5541_one
+	rcall	reset_MS5541_zero
+	rcall	reset_MS5541_one
+	rcall	reset_MS5541_zero
+	rcall	reset_MS5541_one
+	rcall	reset_MS5541_zero
+	rcall	reset_MS5541_one
+	rcall	reset_MS5541_zero
+	rcall	reset_MS5541_one
+	rcall	reset_MS5541_zero
+	rcall	reset_MS5541_one
+	rcall	reset_MS5541_zero
+	rcall	reset_MS5541_one
+	rcall	reset_MS5541_zero			;15
+	rcall	reset_MS5541_zero	
+	rcall	reset_MS5541_zero	
+	rcall	reset_MS5541_zero	
+	rcall	reset_MS5541_zero	
+	rcall	reset_MS5541_zero			;20
+	return
+
+get_2bytes_MS5541:
+	movlw	d'8'
+	movwf	clock_count
+	rcall	recieve_loop
+	movff	isr1_temp,dMSB
+
+	movlw	d'8'
+	movwf	clock_count
+	rcall	recieve_loop
+	movff	isr1_temp,dLSB
+    bra    send_clk_pulse  ; Send one high-low sequence on MS5541_clk  -> and return
+	;return
+
+recieve_loop:
+    rcall   send_clk_pulse  ; Send one high-low sequence on MS5541_clk
+	btfss	MS5541_miso	;MSB first
+	bcf		STATUS,C
+	btfsc	MS5541_miso	;MSB first
+	bsf		STATUS,C
+	rlcf	isr1_temp,F
+	decfsz	clock_count,F
+	bra		recieve_loop
+	return
+
+send_clk_pulse:
+	bsf		MS5541_clk
+	nop
+	nop
+	nop
+	nop
+	bcf		MS5541_clk
+	nop
+	nop
+    return
+
+send_data_MS5541:
+	movwf	clock_count     ; From WREG
+	; send three startbits first
+	bcf		MS5541_clk
+	bsf		MS5541_mosi
+	movlw	d'3'
+	subwf	clock_count,F	; total bit counter
+    rcall   send_clk_pulse  ; Send one high-low sequence on MS5541_clk
+    rcall   send_clk_pulse  ; Send one high-low sequence on MS5541_clk
+    rcall   send_clk_pulse  ; Send one high-low sequence on MS5541_clk
+	; now send 8 bytes from isr_temp1 and fill-up with zeros
+send_data_MS5541_2:
+	bcf		MS5541_clk
+
+	btfss	isr1_temp,7	;MSB first
+	bcf		MS5541_mosi
+	btfsc	isr1_temp,7	;MSB first
+	bsf		MS5541_mosi
+
+	bsf		MS5541_clk
+
+	bcf		STATUS,C
+	rlcf	isr1_temp,F
+;	nop
+;	nop
+;	nop
+;	nop
+;	nop
+;	nop
+;	bcf		MS5541_clk
+
+	decfsz	clock_count,F
+	bra		send_data_MS5541_2
+	bcf		MS5541_clk
+	return
+
+        END
\ No newline at end of file