1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
use crate::types::MagneticFieldData;
const MAG_OVERFLOW_XY: i16 = -4096;
const MAG_OVERFLOW_OUTPUT: i16 = -32768;
const MAG_OVERFLOW_ADCVAL_ZAXIS_HALL: i16 = -16384;
const MAG_NEGATIVE_SATURATION_Z: i32 = -32767;
const MAG_POSITIVE_SATURATION_Z: i32 = 32767;
impl MagneticFieldData {
/// Compensated X in micro tesla
#[inline]
pub fn x_compensated_ut(&self) -> i16 {
let mut retval: i16;
let process_comp_x0: u16;
let process_comp_x1: i32;
let process_comp_x2: u16;
let process_comp_x3: i32;
let process_comp_x4: i32;
let process_comp_x5: i32;
let process_comp_x6: i32;
let process_comp_x7: i32;
let process_comp_x8: i32;
let process_comp_x9: i32;
let process_comp_x10: i32;
let mag_data_x = self.x_unscaled();
let data_rhall = self.rhall_unscaled();
/* Overflow condition check */
if mag_data_x != MAG_OVERFLOW_XY {
if data_rhall != 0 {
/* Availability of valid data */
process_comp_x0 = data_rhall;
} else if self.trim_data.dig_xyz1 != 0 {
process_comp_x0 = self.trim_data.dig_xyz1;
} else {
process_comp_x0 = 0;
}
if process_comp_x0 != 0 {
/* Processing compensation equations */
process_comp_x1 = i32::from(self.trim_data.dig_xyz1) * 16384;
process_comp_x2 = u16::try_from(process_comp_x1 / i32::from(process_comp_x0))
.unwrap()
.wrapping_sub(0x4000);
let tmp_val: i16 = process_comp_x2 as i16;
process_comp_x3 = i32::from(tmp_val) * i32::from(tmp_val);
process_comp_x4 = i32::from(self.trim_data.dig_xy2) * (process_comp_x3 / 128);
process_comp_x5 = i32::from(i16::from(self.trim_data.dig_xy1) * 128);
process_comp_x6 = i32::from(tmp_val) * process_comp_x5;
process_comp_x7 = ((process_comp_x4 + process_comp_x6) / 512) + 0x100000;
process_comp_x8 = i32::from(i16::from(self.trim_data.dig_x2) + 0xA0);
process_comp_x9 = (process_comp_x7 * process_comp_x8) / 4096;
process_comp_x10 = i32::from(mag_data_x) * process_comp_x9;
retval = i16::try_from(process_comp_x10 / 8192).unwrap();
retval = (retval + (i16::from(self.trim_data.dig_x1) * 8)) / 16;
} else {
retval = MAG_OVERFLOW_OUTPUT;
}
} else {
/* Overflow condition */
retval = MAG_OVERFLOW_OUTPUT;
}
retval
}
/// Compensated Y in micro tesla
#[inline]
pub fn y_compensated_ut(&self) -> i16 {
let mut retval: i16;
let process_comp_y0: u16;
let process_comp_y1: i32;
let process_comp_y2: u16;
let process_comp_y3: i32;
let process_comp_y4: i32;
let process_comp_y5: i32;
let process_comp_y6: i32;
let process_comp_y7: i32;
let process_comp_y8: i32;
let process_comp_y9: i32;
let mag_data_y = self.y_unscaled();
let data_rhall = self.rhall_unscaled();
/* Overflow condition check */
if mag_data_y != MAG_OVERFLOW_XY {
if data_rhall != 0 {
/* Availability of valid data */
process_comp_y0 = data_rhall;
} else if self.trim_data.dig_xyz1 != 0 {
process_comp_y0 = self.trim_data.dig_xyz1;
} else {
process_comp_y0 = 0;
}
if process_comp_y0 != 0 {
/* Processing compensation equations */
process_comp_y1 =
((i32::from(self.trim_data.dig_xyz1)) * 16384) / i32::from(process_comp_y0);
process_comp_y2 = u16::try_from(process_comp_y1).unwrap().wrapping_sub(0x4000);
let tmp_val: i16 = process_comp_y2 as i16;
process_comp_y3 = i32::from(tmp_val) * i32::from(tmp_val);
process_comp_y4 = (i32::from(self.trim_data.dig_xy2)) * (process_comp_y3 / 128);
process_comp_y5 = i32::from(i16::from(self.trim_data.dig_xy1) * 128);
process_comp_y6 = (process_comp_y4 + (i32::from(tmp_val) * process_comp_y5)) / 512;
process_comp_y7 = i32::from((i16::from(self.trim_data.dig_y2)) + 0xA0);
process_comp_y8 = ((process_comp_y6 + 0x100000) * process_comp_y7) / 4096;
process_comp_y9 = i32::from(mag_data_y) * process_comp_y8;
retval = i16::try_from(process_comp_y9 / 8192).unwrap();
retval = (retval + (i16::from(self.trim_data.dig_y1) * 8)) / 16;
} else {
retval = MAG_OVERFLOW_OUTPUT;
}
} else {
/* Overflow condition */
retval = MAG_OVERFLOW_OUTPUT;
}
retval
}
/// Compensated Z in micro tesla
#[inline]
pub fn z_compensated_ut(&self) -> i16 {
let mut retval: i32;
let process_comp_z0: i16;
let process_comp_z1: i32;
let process_comp_z2: i32;
let process_comp_z3: i32;
let process_comp_z4: i16;
let mag_data_z = self.z_unscaled();
let data_rhall = self.rhall_unscaled();
if mag_data_z != MAG_OVERFLOW_ADCVAL_ZAXIS_HALL {
if (self.trim_data.dig_z2 != 0)
&& (self.trim_data.dig_z1 != 0)
&& (data_rhall != 0)
&& (self.trim_data.dig_xyz1 != 0)
{
/*Processing compensation equations */
process_comp_z0 = i16::try_from(data_rhall).unwrap()
- i16::try_from(self.trim_data.dig_xyz1).unwrap();
process_comp_z1 =
(i32::from(self.trim_data.dig_z3) * (i32::from(process_comp_z0))) / 4;
process_comp_z2 = (i32::from(mag_data_z - self.trim_data.dig_z4)) * 32768;
process_comp_z3 = (i32::from(self.trim_data.dig_z1)) * (i32::from(data_rhall) * 2);
process_comp_z4 = i16::try_from((process_comp_z3 + (32768)) / 65536).unwrap();
retval = (process_comp_z2 - process_comp_z1)
/ (i32::from(self.trim_data.dig_z2) + i32::from(process_comp_z4));
/* Saturate result to +/- 2 micro-tesla */
if retval > MAG_POSITIVE_SATURATION_Z {
retval = MAG_POSITIVE_SATURATION_Z;
} else if retval < MAG_NEGATIVE_SATURATION_Z {
retval = MAG_NEGATIVE_SATURATION_Z;
}
/* Conversion of LSB to micro-tesla */
retval /= 16;
} else {
retval = i32::from(MAG_OVERFLOW_OUTPUT);
}
} else {
/* Overflow condition */
retval = i32::from(MAG_OVERFLOW_OUTPUT);
}
retval.try_into().unwrap()
}
}