;=============================================================================
;
;   File calibration.asm
;
;   o2 sensor calibration subroutines
;
;   Copyright (c) 2014, Heinrichs Weikamp, all right reserved.
;=============================================================================

#include    "hwos.inc"
#include    "shared_definitions.h"		; Mailbox between c and asm
#include    "math.inc"
#include    "adc_lightsensor.inc"
#include    "eeprom_rs232.inc"

calibrate     CODE

    global  check_sensors           ; Check O2 sensor thresholds for fallback and voting logic
check_sensors:
    ; Check min_mv
	movff	o2_mv_sensor1+0, sub_a+0
	movff	o2_mv_sensor1+1, sub_a+1
	movlw	LOW		min_mv
	movwf	sub_b+0
	movlw	HIGH	min_mv
	movwf	sub_b+1
	call	sub16			;  sub_c = sub_a - sub_b
    bsf     use_O2_sensor1  ;=1: Use this sensor for deco
	btfsc	neg_flag
	bcf     use_O2_sensor1  ;=1: Use this sensor for deco

	movff	o2_mv_sensor2+0, sub_a+0
	movff	o2_mv_sensor2+1, sub_a+1
	movlw	LOW		min_mv
	movwf	sub_b+0
	movlw	HIGH	min_mv
	movwf	sub_b+1
	call	sub16			;  sub_c = sub_a - sub_b
	bsf     use_O2_sensor2  ;=1: Use this sensor for deco
	btfsc	neg_flag
	bcf     use_O2_sensor2  ;=1: Use this sensor for deco

	movff	o2_mv_sensor3+0, sub_a+0
	movff	o2_mv_sensor3+1, sub_a+1
	movlw	LOW		min_mv
	movwf	sub_b+0
	movlw	HIGH	min_mv
	movwf	sub_b+1
	call	sub16			;  sub_c = sub_a - sub_b
	bsf     use_O2_sensor3  ;=1: Use this sensor for deco
	btfsc	neg_flag
	bcf     use_O2_sensor3  ;=1: Use this sensor for deco
    ; Check max_mv
	movff	o2_mv_sensor1+0, sub_a+0
	movff	o2_mv_sensor1+1, sub_a+1
	movlw	LOW		max_mv
	movwf	sub_b+0
	movlw	HIGH	max_mv
	movwf	sub_b+1
	call	sub16			;  sub_c = sub_a - sub_b
	btfss	neg_flag
	bcf     use_O2_sensor1  ;=1: Use this sensor for deco

	movff	o2_mv_sensor2+0, sub_a+0
	movff	o2_mv_sensor2+1, sub_a+1
	movlw	LOW		max_mv
	movwf	sub_b+0
	movlw	HIGH	max_mv
	movwf	sub_b+1
	call	sub16			;  sub_c = sub_a - sub_b
	btfss	neg_flag
	bcf     use_O2_sensor2  ;=1: Use this sensor for deco

	movff	o2_mv_sensor3+0, sub_a+0
	movff	o2_mv_sensor3+1, sub_a+1
	movlw	LOW		max_mv
	movwf	sub_b+0
	movlw	HIGH	max_mv
	movwf	sub_b+1
	call	sub16			;  sub_c = sub_a - sub_b
	btfss	neg_flag
	bcf     use_O2_sensor3  ;=1: Use this sensor for deco

    btfss   hud_connection_ok   ;=1: HUD connection ok
    bra     check_sensor2       ; No HUD/Digital data

    ; Copy disable flags from digital input
    btfss   sensor1_active
    bcf     use_O2_sensor1
    btfss   sensor2_active
    bcf     use_O2_sensor2
    btfss   sensor3_active
    bcf     use_O2_sensor3
    bra     check_sensor3           ; Check for voting logic

check_sensor2:
; Copy disable flags from internal calibration routine
    btfss   sensor1_calibrated_ok
    bcf     use_O2_sensor1
    btfss   sensor2_calibrated_ok
    bcf     use_O2_sensor2
    btfss   sensor3_calibrated_ok
    bcf     use_O2_sensor3
check_sensor3:                      ; Check for voting logic
    bsf     voting_logic_sensor1
    movff   o2_ppo2_sensor1,temp1
    rcall   check_sensor_voting_common
    incfsz  WREG                    ; Was Wreg=255?
    bcf     voting_logic_sensor1    ; Yes, ignore this sensor
    bsf     voting_logic_sensor2
    movff   o2_ppo2_sensor2,temp1
    rcall   check_sensor_voting_common
    incfsz  WREG                    ; Was Wreg=255?
    bcf     voting_logic_sensor2    ; Yes, ignore this sensor
    bsf     voting_logic_sensor3
    movff   o2_ppo2_sensor3,temp1
    rcall   check_sensor_voting_common
    incfsz  WREG                    ; Was Wreg=255?
    bcf     voting_logic_sensor3    ; Yes, ignore this sensor
    return


check_sensor_voting_common:
    movf    temp1,W
    cpfsgt  sensor_setpoint
    bra     check_sensor_voting_common2     ; temp1<sensor_setpoint
    ; temp1>sensor_setpoint
    movf    temp1,W
    subwf   sensor_setpoint,W
    movwf   temp1
check_sensor_voting_common1:
    movlw   sensor_voting_logic_threshold   ; Threshold in 0.01bar
    cpfsgt  temp1
    retlw   .255                            ; Within range
    retlw   .0                              ; Out of range
check_sensor_voting_common2:
    ; temp1<sensor_setpoint
    movf    sensor_setpoint,W
    subwf   temp1,F
    bra     check_sensor_voting_common1

	global	calibrate_mix
calibrate_mix:
    ; calibrate S8 HUD
    btfss   s8_digital          ; S8 Digital?
    bra     calibrate_mix2      ; No

    ; Yes, calibrate any S8-connected HUD
    clrf    temp1               ; Chksum
    movlw   0xAA                ; Start Byte
    addwf   temp1,F
    movff   WREG,TXREG2
    call    rs232_wait_tx2

    movlw   0x31                ; Calibrate
    addwf   temp1,F
    movff   WREG,TXREG2
    call    rs232_wait_tx2

    movff   opt_calibration_O2_ratio,WREG         ; Calibration gas %O2
    addwf   temp1,F
    movff   WREG,TXREG2
    call    rs232_wait_tx2

    movff   amb_pressure+0,WREG         ; Ambient pressure
    addwf   temp1,F
    movff   WREG,TXREG2
    call    rs232_wait_tx2
    movff   amb_pressure+1,WREG
    addwf   temp1,F
    movff   WREG,TXREG2
    call    rs232_wait_tx2

    movff   temp1,TXREG2                ; Chksum
    call    rs232_wait_tx2

calibrate_mix2:
    movff   opt_calibration_O2_ratio,WREG         ; Calibration gas %O2
    mullw   .100
	movff	PRODL,xA+0
	movff	PRODH,xA+1
; (%O2*100)*[ambient,mbar]/100 -> xC
	movff	amb_pressure+0,xB+0
	movff	amb_pressure+1,xB+1
	rcall	calibrate_mix2_helper
	movff	o2_mv_sensor1+0,xB+0
	movff	o2_mv_sensor1+1,xB+1
	call	div32x16	  ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder
	; xC= ppO2/mV
	movff	xC+0,opt_x_s1+0
	movff	xC+1,opt_x_s1+1				; Factor for Sensor1

    movff   opt_calibration_O2_ratio,WREG         ; Calibration gas %O2
    mullw   .100
	movff	PRODL,xA+0
	movff	PRODH,xA+1
; (%O2*100)*[ambient,mbar]/100 -> xC
	movff	amb_pressure+0,xB+0
	movff	amb_pressure+1,xB+1
	rcall	calibrate_mix2_helper
	movff	o2_mv_sensor2+0,xB+0
	movff	o2_mv_sensor2+1,xB+1
	call	div32x16	  ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder
	; xC= ppO2/mV
	movff	xC+0,opt_x_s2+0
	movff	xC+1,opt_x_s2+1				; Factor for Sensor2

    movff   opt_calibration_O2_ratio,WREG         ; Calibration gas %O2
    mullw   .100
	movff	PRODL,xA+0
	movff	PRODH,xA+1
; (%O2*100)*[ambient,mbar]/100 -> xC
	movff	amb_pressure+0,xB+0
	movff	amb_pressure+1,xB+1
	rcall	calibrate_mix2_helper
	movff	o2_mv_sensor3+0,xB+0
	movff	o2_mv_sensor3+1,xB+1
	call	div32x16	  ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder
	; xC= ppO2/mV
	movff	xC+0,opt_x_s3+0
	movff	xC+1,opt_x_s3+1				; Factor for Sensor3

    bsf     sensor1_calibrated_ok
    bsf     sensor2_calibrated_ok
    bsf     sensor3_calibrated_ok   ; Set flags prior check
    rcall   check_sensors           ; Check O2 sensor thresholds min_mv and max_mv and set use_02_sensorX flags
    ; initialise internal calibration flags
    btfss	use_O2_sensor1          ; Sensor out of range?
    bcf     sensor1_calibrated_ok   ; Yes, disable this sensor
    btfss	use_O2_sensor2          ; Sensor out of range?
    bcf     sensor2_calibrated_ok   ; Yes, disable this sensor
    btfss	use_O2_sensor3          ; Sensor out of range?
    bcf     sensor3_calibrated_ok   ; Yes, disable this sensor

	; When no sensor is found, enable all three to show error state
	btfsc	use_O2_sensor1
	return
	btfsc	use_O2_sensor2
	return
	btfsc	use_O2_sensor3
	return
	bsf		use_O2_sensor1
	bsf		use_O2_sensor2
	bsf		use_O2_sensor3
	; Clear factors
    banksel opt_x_s1+0
	clrf	opt_x_s1+0
	clrf	opt_x_s1+1
	clrf	opt_x_s2+0
	clrf	opt_x_s2+1
	clrf	opt_x_s3+0
	clrf	opt_x_s3+1
    banksel common
	return

calibrate_mix2_helper:	
	call	mult16x16		;xA*xB=xC
	movlw	LOW		.100
	movwf	xB+0
	movlw	HIGH	.100
	movwf	xB+1
	goto	div32x16	  ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder (And return)

	
compute_ppo2_analog:
    call    get_analog_inputs
    bra     compute_ppo2_common

	global	compute_ppo2			; compute mv_sensorX and ppo2_sensorX arrays
compute_ppo2:
    btfss   analog_o2_input             ; cR hardware?
    return                              ; No

    btfss   s8_digital                  ; =1: Digital I/O
    bra     compute_ppo2_analog         ; use analog

    ; use digital
    btfss   new_s8_data_available       ; =1: New data frame recieved
    return
	rcall	compute_mvolts_for_all_sensors

compute_ppo2_common:
	; o2_mv_sensor1:2 * opt_x_s1:2 = o2_ppo2_sensor1/10000
	movff	o2_mv_sensor1+0,xA+0
	movff	o2_mv_sensor1+1,xA+1
	movff	opt_x_s1+0,xB+0
	movff	opt_x_s1+1,xB+1
	rcall	compute_ppo2_common_helper
	movff	xC+0,o2_ppo2_sensor1		; result in 0.01bar
;    ; Set to zero if sensor is not active!
;	btfss	use_O2_sensor1
;	clrf	o2_ppo2_sensor1

	; o2_mv_sensor2:2 * opt_x_s1:2 = o2_ppo2_sensor2/10000
	movff	o2_mv_sensor2+0,xA+0
	movff	o2_mv_sensor2+1,xA+1
	movff	opt_x_s2+0,xB+0
	movff	opt_x_s2+1,xB+1
	rcall	compute_ppo2_common_helper
	movff	xC+0,o2_ppo2_sensor2		; result in 0.01bar
;	; Set to zero if sensor is not active!
;	btfss	use_O2_sensor2
;	clrf	o2_ppo2_sensor2

	; o2_mv_sensor3:2 * opt_x_s1:2 = o2_ppo2_sensor3/10000
	movff	o2_mv_sensor3+0,xA+0
	movff	o2_mv_sensor3+1,xA+1
	movff	opt_x_s3+0,xB+0
	movff	opt_x_s3+1,xB+1
	rcall	compute_ppo2_common_helper
	movff	xC+0,o2_ppo2_sensor3		; result in 0.01bar
;	; Set to zero if sensor is not active!
;	btfss	use_O2_sensor3
;	clrf	o2_ppo2_sensor3
	return							; Done.

compute_ppo2_common_helper:
	call	mult16x16		;xA:2*xB:2=xC:4
	movlw	LOW		.1000
	movwf	xB+0
	movlw	HIGH		.1000
	movwf	xB+1
	call	div32x16  ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder
	movlw	d'1'
	addwf	xC+0,F
	movlw	d'0'
	addwfc	xC+1,F
        tstfsz  xC+1                        ; ppO2 is higher then 2.55bar?
        setf    xC+0                        ; Yes.
        return

compute_mvolts_for_all_sensors:          ; Compute mV or all sensors (S8 Mode)
; compute AD results in 100µV steps (16bit/sensor)
; 24bit AD result is in 244,1406541nV
; Devide 24bit value through 409,5999512 -> 410 (0,01% error)
        #DEFINE	ad2mv_factor	.410
        ; Sensor 1
        clrf	xC+3
        movff	s8_rawdata_sensor1+2,xC+2
        movff	s8_rawdata_sensor1+1,xC+1
        movff	s8_rawdata_sensor1+0,xC+0
        movlw	LOW		ad2mv_factor
        movwf	xB+0
        movlw	HIGH	ad2mv_factor
        movwf	xB+1
        call	div32x16  ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder
        movff	xC+1,o2_mv_sensor1+1
        movff	xC+0,o2_mv_sensor1+0		; in 100uV steps
        ; Sensor 2
        clrf	xC+3
        movff	s8_rawdata_sensor2+2,xC+2
        movff	s8_rawdata_sensor2+1,xC+1
        movff	s8_rawdata_sensor2+0,xC+0
        movlw	LOW		ad2mv_factor
        movwf	xB+0
        movlw	HIGH	ad2mv_factor
        movwf	xB+1
        call	div32x16  ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder
        movff	xC+1,o2_mv_sensor2+1
        movff	xC+0,o2_mv_sensor2+0		; in 100uV steps
        ; Sensor 3
        clrf	xC+3
        movff	s8_rawdata_sensor3+2,xC+2
        movff	s8_rawdata_sensor3+1,xC+1
        movff	s8_rawdata_sensor3+0,xC+0
        movlw	LOW		ad2mv_factor
        movwf	xB+0
        movlw	HIGH	ad2mv_factor
        movwf	xB+1
        call	div32x16  ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder
        movff	xC+1,o2_mv_sensor3+1
        movff	xC+0,o2_mv_sensor3+0		; in 100uV steps

        bcf     new_s8_data_available       ; Clear flag
        return                  ; Done.

	global	transmit_setpoint           ; Transmit current setpoint from WREG (in cbar) to external electronics
transmit_setpoint:
    return
    btfss   s8_digital          ; S8 Digital?
    return                      ; No, ignore

    ; Yes, transmit setpoint from WREG
    movwf   temp2               ; Store setpoint
    clrf    temp1               ; Chksum
    movlw   0xAA                ; Start Byte
    addwf   temp1,F
    movff   WREG,TXREG2
    call    rs232_wait_tx2

    movlw   0x60                ; New SP
    addwf   temp1,F
    movff   WREG,TXREG2
    call    rs232_wait_tx2

    movff   temp2,WREG         ; SP in cbar
    addwf   temp1,F
    movff   WREG,TXREG2
    call    rs232_wait_tx2

    movff   temp1,TXREG2	; Chksum
    call    rs232_wait_tx2
    return


	END