imu_fusion/fusion_impl.rs
1use crate::{Fusion, FusionAhrs, FusionAhrsSettings, FusionEuler, FusionGyrOffset, FusionMatrix, FusionQuaternion, FusionVector};
2
3impl Fusion {
4 pub fn new(sample_rate: u32, ahrs_settings: FusionAhrsSettings) -> Self {
5 let mut ahrs = FusionAhrs::new();
6 ahrs.update_settings(ahrs_settings);
7 Self {
8 gyr_misalignment: FusionMatrix::identity(),
9 gyr_sensitivity: FusionVector::ones(),
10 gyr_offset: FusionVector::zero(),
11 acc_misalignment: FusionMatrix::identity(),
12 acc_sensitivity: FusionVector::ones(),
13 acc_offset: FusionVector::zero(),
14 soft_iron_matrix: FusionMatrix::identity(),
15 hard_iron_offset: FusionVector::zero(),
16 ahrs,
17 offset: FusionGyrOffset::new(sample_rate),
18 last_timestamp: 0f32,
19 }
20 }
21
22 pub fn inertial_calibration(&self, uncalibrated: FusionVector, misalignment: FusionMatrix, sensitivity: FusionVector, offset: FusionVector) -> FusionVector {
23 misalignment * ((uncalibrated - offset) * sensitivity)
24 }
25
26 pub fn magnetic_calibration(&self, uncalibrated: FusionVector, soft_iron_matrix: FusionMatrix, hard_iron_offset: FusionVector) -> FusionVector {
27 soft_iron_matrix * (uncalibrated - hard_iron_offset)
28 }
29
30 /// Updates the AHRS algorithm based on gyroscope data in degrees/s and acceleration data in g force.
31 ///
32 /// The time is provided using an absolute timestamp in seconds since the first measurement.
33 /// Note that if you provide unix timestamps, the precision of f32 will not be enough to correctly compute the time difference.
34 ///
35 /// # Examples
36 /// ```no_run
37 /// use imu_fusion::{Fusion, FusionVector, FusionAhrsSettings};
38 ///
39 /// const SAMPLE_RATE_HZ: u32 = 100;
40 ///
41 /// let ahrs_settings = FusionAhrsSettings::new();
42 /// let mut fusion = imu_fusion::Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
43 /// let mut current_ts = 0f32;
44 ///
45 /// loop {
46 /// let gyr = FusionVector::new(0f32, 0f32, 0f32); // replace this with actual gyroscope data in degrees/s
47 /// let acc = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual accelerometer data in g
48 /// fusion.update_no_mag(gyr, acc, current_ts);
49 /// current_ts += 1.0 / SAMPLE_RATE_HZ as f32
50 /// }
51 ///
52 /// ```
53 pub fn update_no_mag(&mut self, gyr: FusionVector, acc: FusionVector, timestamp: f32) {
54 let delta_t = timestamp - self.last_timestamp;
55 self.update_no_mag_by_duration_seconds(gyr, acc, delta_t);
56 self.last_timestamp = timestamp;
57 }
58
59 /// Updates the AHRS algorithm based on gyroscope data in degrees/s and acceleration data in g force.
60 ///
61 /// The time is provided as a duration in seconds since the last measurement.
62 /// Note that this won't increase the internal timestamp and using the timestamp version of update functions will produce incorrect results.
63 ///
64 /// # Examples
65 /// ```no_run
66 /// use imu_fusion::{Fusion, FusionVector, FusionAhrsSettings};
67 /// use std::time::{Duration, Instant};
68 ///
69 /// const SAMPLE_RATE_HZ: u32 = 100;
70 ///
71 /// let ahrs_settings = FusionAhrsSettings::new();
72 /// let mut fusion = imu_fusion::Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
73 /// let mut last_ts = Instant::now();
74 ///
75 /// loop {
76 /// let gyr = FusionVector::new(0f32, 0f32, 0f32); // replace this with actual gyroscope data in degrees/s
77 /// let acc = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual accelerometer data in g
78 /// fusion.update_no_mag_by_duration_seconds(gyr, acc, last_ts.elapsed().as_secs_f32());
79 /// last_ts = Instant::now();
80 /// }
81 ///
82 /// ```
83 pub fn update_no_mag_by_duration_seconds(&mut self, gyr: FusionVector, acc: FusionVector, delta_t: f32) {
84 // Apply calibration
85 let mut gyr = self.inertial_calibration(gyr, self.gyr_misalignment, self.gyr_sensitivity, self.gyr_offset);
86 let acc = self.inertial_calibration(acc, self.acc_misalignment, self.acc_sensitivity, self.acc_offset);
87
88 // Update gyroscope offset correction algorithm
89 gyr = self.offset.update(gyr);
90
91 self.ahrs.update_no_mag(gyr, acc, delta_t);
92 }
93
94 /// Updates the AHRS algorithm based on gyroscope data in degrees/s, acceleration data in g force and a heading in degrees.
95 ///
96 /// The time is provided using an absolute timestamp in seconds since the first measurement.
97 /// Note that if you provide unix timestamps, the precision of f32 will not be enough to correctly compute the time difference.
98 ///
99 /// # Examples
100 /// ```no_run
101 /// use imu_fusion::{Fusion, FusionVector, FusionAhrsSettings};
102 ///
103 /// const SAMPLE_RATE_HZ: u32 = 100;
104 ///
105 /// let ahrs_settings = FusionAhrsSettings::new();
106 /// let mut fusion = Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
107 /// let mut current_ts = 0f32;
108 ///
109 /// loop {
110 /// let gyr = FusionVector::new(0f32, 0f32, 0f32); // replace this with actual gyroscope data in degrees/s
111 /// let acc = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual accelerometer data in g
112 /// let external_heading = 0f32; // replace this with actual heading in degrees
113 /// fusion.update_external_heading(gyr, acc, external_heading, current_ts);
114 /// current_ts += 1.0 / SAMPLE_RATE_HZ as f32
115 /// }
116 ///
117 /// ```
118 pub fn update_external_heading(
119 &mut self,
120 gyr: FusionVector,
121 acc: FusionVector,
122 heading: f32,
123 timestamp: f32,
124 ) {
125 let delta_t = timestamp - self.last_timestamp;
126 self.update_external_heading_by_duration_seconds(gyr, acc, heading, delta_t);
127 self.last_timestamp = timestamp;
128 }
129
130 /// Updates the AHRS algorithm based on gyroscope data in degrees/s, acceleration data in g force and a heading in degrees.
131 ///
132 /// The time is provided using a duration in seconds since the last measurement.
133 /// Note that this won't increase the internal timestamp and using the timestamp version of update functions will produce incorrect results.
134 ///
135 /// # Examples
136 /// ```no_run
137 /// use imu_fusion::{Fusion, FusionVector, FusionAhrsSettings};
138 /// use std::time::{Duration, Instant};
139 ///
140 /// const SAMPLE_RATE_HZ: u32 = 100;
141 ///
142 /// let ahrs_settings = FusionAhrsSettings::new();
143 /// let mut fusion = Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
144 /// let mut last_ts = Instant::now();
145 ///
146 /// loop {
147 /// let gyr = FusionVector::new(0f32, 0f32, 0f32); // replace this with actual gyroscope data in degrees/s
148 /// let acc = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual accelerometer data in g
149 /// let external_heading = 0f32; // replace this with actual heading in degrees
150 /// fusion.update_external_heading(gyr, acc, external_heading, last_ts.elapsed().as_secs_f32());
151 /// last_ts = Instant::now();
152 /// }
153 ///
154 /// ```
155 pub fn update_external_heading_by_duration_seconds(
156 &mut self,
157 gyr: FusionVector,
158 acc: FusionVector,
159 heading: f32,
160 delta_t: f32,
161 ) {
162 // Apply calibration
163 let mut gyr = self.inertial_calibration(gyr, self.gyr_misalignment, self.gyr_sensitivity, self.gyr_offset);
164 let acc = self.inertial_calibration(acc, self.acc_misalignment, self.acc_sensitivity, self.acc_offset);
165
166 // Update gyroscope offset correction algorithm
167 gyr = self.offset.update(gyr);
168
169 self.ahrs.update_external_heading(gyr, acc, heading, delta_t);
170 }
171
172 /// Updates the AHRS algorithm based on gyroscope data in degrees/s, acceleration data in g force and magnetic measurements in degrees.
173 ///
174 /// The time is provided using an absolute timestamp in seconds since the first measurement.
175 /// Note that if you provide unix timestamps, the precision of f32 will not be enough to correctly compute the time difference.
176 ///
177 /// # Examples
178 /// ```no_run
179 /// use imu_fusion::{Fusion, FusionAhrsSettings, FusionVector};
180 ///
181 /// const SAMPLE_RATE_HZ: u32 = 100;
182 ///
183 /// let ahrs_settings = FusionAhrsSettings::new();
184 /// let mut fusion = Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
185 /// let mut current_ts = 0f32;
186 ///
187 /// loop {
188 /// let gyr = FusionVector::new(0f32, 0f32, 0f32); // replace this with actual gyroscope data in degrees/s
189 /// let acc = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual accelerometer data in g
190 /// let mag = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual magnetic measurement in degrees
191 /// fusion.update(gyr, acc, mag, current_ts);
192 /// current_ts += 1.0 / SAMPLE_RATE_HZ as f32
193 /// }
194 ///
195 /// ```
196 pub fn update(
197 &mut self,
198 gyr: FusionVector,
199 acc: FusionVector,
200 mag: FusionVector,
201 timestamp: f32,
202 ) {
203 let delta_t = timestamp - self.last_timestamp;
204 self.update_by_duration_seconds(gyr, acc, mag, delta_t);
205 self.last_timestamp = timestamp;
206 }
207
208 /// Updates the AHRS algorithm based on gyroscope data in degrees/s, acceleration data in g force and magnetic measurements in degrees.
209 ///
210 /// The time is provided using a duration in seconds since the last measurement.
211 /// Note that this won't increase the internal timestamp and using the timestamp version of update functions will produce incorrect results.
212 /// # Examples
213 /// ```no_run
214 /// use imu_fusion::{Fusion, FusionAhrsSettings, FusionVector};
215 /// use std::time::{Duration, Instant};
216 ///
217 /// const SAMPLE_RATE_HZ: u32 = 100;
218 ///
219 /// let ahrs_settings = FusionAhrsSettings::new();
220 /// let mut fusion = Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
221 /// let mut last_ts = Instant::now();
222 ///
223 /// loop {
224 /// let gyr = FusionVector::new(0f32, 0f32, 0f32); // replace this with actual gyroscope data in degrees/s
225 /// let acc = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual accelerometer data in g
226 /// let mag = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual magnetic measurement in degrees
227 /// fusion.update_by_duration_seconds(gyr, acc, mag, last_ts.elapsed().as_secs_f32());
228 /// last_ts = Instant::now();
229 /// }
230 ///
231 /// ```
232 pub fn update_by_duration_seconds(
233 &mut self,
234 gyr: FusionVector,
235 acc: FusionVector,
236 mag: FusionVector,
237 delta_t: f32,
238 ) {
239 // Apply calibration
240 let mut gyr = self.inertial_calibration(gyr, self.gyr_misalignment, self.gyr_sensitivity, self.gyr_offset);
241 let acc = self.inertial_calibration(acc, self.acc_misalignment, self.acc_sensitivity, self.acc_offset);
242 let mag = self.magnetic_calibration(mag, self.soft_iron_matrix, self.hard_iron_offset);
243
244 // Update gyroscope offset correction algorithm
245 gyr = self.offset.update(gyr);
246
247 self.ahrs.update(gyr, acc, mag, delta_t);
248 }
249
250 /// Obtain euler angle current sensor position
251 ///
252 /// Euler angles are provided in degrees
253 ///
254 /// # Examples
255 ///
256 /// ```
257 /// use imu_fusion::{Fusion, FusionAhrsSettings};
258 ///
259 /// const SAMPLE_RATE_HZ: u32 = 100;
260 ///
261 /// let ahrs_settings = FusionAhrsSettings::new();
262 /// let mut fusion = Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
263 ///
264 /// // ...update sensor values
265 ///
266 /// let euler = fusion.euler();
267 /// println!("Roll {}, Pitch {}, Yaw {}", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
268 /// ```
269 pub fn euler(&self) -> FusionEuler {
270 self.ahrs.quaternion.euler()
271 }
272
273 /// Obtain acceleration of sensor in earth's frame of reference
274 ///
275 /// The values returned are provided in g force
276 ///
277 /// # Examples
278 ///
279 /// ```
280 /// use imu_fusion::{Fusion, FusionAhrsSettings, FusionVector};
281 ///
282 /// const SAMPLE_RATE_HZ: u32 = 100;
283 ///
284 /// let ahrs_settings = FusionAhrsSettings::new();
285 /// let mut fusion = Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
286 ///
287 /// // ...update sensor values
288 ///
289 /// let acc = fusion.earth_acc();
290 /// println!("x {}, y {}, z {}", acc.x, acc.y, acc.z);
291 /// ```
292 pub fn earth_acc(&self) -> FusionVector {
293 self.ahrs.earth_acc()
294 }
295
296 pub fn quaternion(&self) -> FusionQuaternion {
297 self.ahrs.quaternion
298 }
299}