;***************************************************************
; Prof. Christopher Russel
; Remove zero level from each sensor
;    BXC = GX*BX - OX
;    BYC = GY*BY - OY
;    BZC = GZ*BZ - OZ
;    where BXC, BYC, BZC are the corrected values
;      BX, BY, BZ are the original valures
;      OX, OY, OZ are the readings of BX, BY, BZ when the fields is really (0, 0, 0)
; GX, GY and GZ are gains account for scale factors, defaulted values are set to 1
; OX, OY and OZ are offsets due to the unaccount calibration, defaulted values are set to 0
; the real offset values will be determined by sampling the incoming MAG data using correlation coefficient technique and also depends on the ranges set
; The above correction will be done in two steps, init zero calibration and scale factor calibration
; init zero calibration is implemented with function init_zero, while scale factor calibration is function scale_factor
;
;
;***************************************************************
function mag_init_zero, C, init_zero_cal
; This calibration function is used to make a correction to bring the ADC (Ci) to its effective "zero level" (Ciz)
; Ciz = Ci - Zi
; Ci is original ADC readings (counts) from the telemetry packet, in the form of CX, CY and CZ
; Zi is the correction matrix (3x1)
;
; Return values initialization
CXiz = 0
CYiz = 0
CZiz = 0
; Verify
if not !quiet then begin
    print,"Verify..."
    print,init_zero_cal
endif
    CXiz = C(0) - init_zero_cal(0)
    CYiz = C(1) - init_zero_cal(1)
    CZiz = C(2) - init_zero_cal(2)
Ciz = [CXiz, CYiz, CZiz]
return, Ciz

end; end of function