datafusion_imu/lib.rs
1//! # Datafusion library for 6 and 9 degrees of freedom sensors.
2//!
3//! [](https://www.rust-lang.org/)
4//! [](https://sii-group.com/fr-FR/sii-sud-ouest)
5//!
6//! You have the choice in this library to apply the filters on two types of sensors:
7//! - A 6 degrees of freedom sensor with a 3-axis accelerometer and 3-axis gyroscope -> Mode `Dof6`
8//! - A 9 degrees of freedom sensor with a 3-axis accelerometer, 3-axis gyroscope and 3-axis magnetometer -> Mode `Dof9`
9//!
10//! > 🔴 ***For now, the sensor needs to be perfectly flat to work. Also, using angle and distance measurement at the same time is not recommended with a 6Dof sensor.***
11//! > More specifically, for the distance it doesn't really matter as the high pass filter will automatically delete the angle offset after a few second.
12//! > However, for the angle (except with a magnetometer) the sensor has to be flat or at least stay within the same inclination on the X and Y-axis after initialization.
13//!
14//! In the example provided below, we use our own driver for an Adafruit sensor. So of course, you can use any other driver you want.
15//!
16//! ### Angle data
17//! When the sensor is flat and whatever the mode used, X and Y angles are absolute roll and pitch values in degrees.
18//! A Kalman filter is applied between acceleration and angular speed to produce a reliable output.
19//! While using a Dof6 sensor, e.g without a magnetometer, the Kalman filter output on the the Z or Yaw-axis is an angular speed in degrees per second.
20//! Which is then integrated to produce the absolute angle in degrees. But this method is not really reliable as it heavily rely on which speed the sensor is moving.
21//! However, when using a Dof9 sensor, the Kalman filter output on the Z or Yaw-axis is an absolute angle in degrees. This is the preferred method.
22//! As long as there is no magnetic interference, you we will be able a obtain a heading angle down to a 2 to 3 degree accuracy.
23//! In the case of magnetic interferences, if it is much greater than the earth magnetic field, then the output won't be the magnetic north but a relative angle to the magnetic interference.
24//!
25//! ### Distance data
26//! For the distance data, the algorithm is the same whatever the mode used as it only uses acceleration data on X and Y.
27//! It is a succession of high pass filters, low pass filters and integrations.
28//! It works perfectly fine on a short distance, e.g < 5cm, and it extremely accurate in this range.
29//! However, over this range the result highly rely on the speed.
30//! This means that, if the speed is too low, the distance will be under estimated.
31//! Same problem if the speed is too high, the distance will be over estimated.
32//! This problem is currently being worked on and the lib will be updated if a workaround is found.
33//!
34//! You have the ability to disable the distance measurement by calling the `disable_distance` function.
35//!
36//! ***Please note that distances measurements are purely experimental but it's a place to start as the accuracy is not optimal.***
37//!
38//! ### Usage
39//! The example below uses [rppal](https://crates.io/crates/rppal) crates and runs on a Raspberry Pi.
40//!
41//! Please note that this example and library have been developed in parallel with a driver for a Dof9 sensor from Adafruit.
42//!
43//! ```no_run
44//! use std::time::Instant;
45//!
46//! use rppal::hal::Delay;
47//! use rppal::i2c::I2c;
48//!
49//! use embedded_hal::blocking::delay::*;
50//!
51//! use adafruit::*;
52//! use datafusion::{self as _, Fusion};
53//!
54//! fn main() -> Result<(), SensorError<rppal::i2c::Error>> {
55//!
56//! // Init a delay used in certain functions and between each loop.
57//! let mut delay = Delay::new();
58//!
59//! // Setup the raspberry's I2C interface to create the sensor.
60//! let i2c = I2c::new().unwrap();
61//!
62//! // Create an Adafruit object
63//! let mut sensor = AdafruitNXP::new(0x8700A, 0x8700B, 0x0021002C, i2c);
64//!
65//! // Check if the sensor is ready to go
66//! let ready = sensor.begin()?;
67//! if !ready {
68//! std::eprintln!("Sensor not detected, check your wiring!");
69//! std::process::exit(1);
70//! }
71//!
72//! sensor.set_accel_range(config::AccelMagRange::Range8g)?;
73//! sensor.set_gyro_range(config::GyroRange::Range500dps)?;
74//! // etc...
75//!
76//! // Initialize the sensor
77//! sensor.read_data()?;
78//!
79//! let acc_x = sensor.accel_sensor.get_scaled_x();
80//! let acc_y = sensor.accel_sensor.get_scaled_y();
81//! let acc_z = sensor.accel_sensor.get_scaled_z();
82//!
83//! let gyro_x = sensor.gyro_sensor.get_scaled_x();
84//! let gyro_y = sensor.gyro_sensor.get_scaled_y();
85//! let gyro_z = sensor.gyro_sensor.get_scaled_z();
86//!
87//! let mag_rx = sensor.mag_sensor.get_scaled_x();
88//! let mag_ry = sensor.mag_sensor.get_scaled_y();
89//! let mag_rz = sensor.mag_sensor.get_scaled_z();
90//!
91//! // Create a datafusion object
92//! let mut fusion = Fusion::new(0.05, 20., 50);
93//! fusion.set_mode(datafusion::Mode::Dof9);
94//!
95//! // Set data to the fusion object
96//! fusion.set_data_dof9(acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, mag_rx, mag_ry, mag_rz);
97//!
98//! // Initialize the datafusion object
99//! fusion.init();
100//!
101//! // Set magnetic declination --> 1.39951° in Toulouse, France
102//! fusion.set_declination(1.39951);
103//!
104//! // Setting up the delta time
105//! let mut time = Instant::now();
106//!
107//! loop {
108//!
109//! // Calculate delta time in seconds
110//! let dt = time.elapsed().as_micros() as f32 / 1_000_000.;
111//! time = Instant::now();
112//!
113//! // Update old values for the next loop
114//! fusion.set_old_values(acc_x, acc_y);
115//!
116//! sensor.read_data()?;
117//!
118//! let acc_x = sensor.accel_sensor.get_scaled_x();
119//! let acc_y = sensor.accel_sensor.get_scaled_y();
120//! let acc_z = sensor.accel_sensor.get_scaled_z();
121//!
122//! let gyro_x = sensor.gyro_sensor.get_scaled_x();
123//! let gyro_y = sensor.gyro_sensor.get_scaled_y();
124//! let gyro_z = sensor.gyro_sensor.get_scaled_z();
125//!
126//! let mag_rx = sensor.mag_sensor.get_scaled_x();
127//! let mag_ry = sensor.mag_sensor.get_scaled_y();
128//! let mag_rz = sensor.mag_sensor.get_scaled_z();
129//!
130//! // Set data to the fusion object
131//! fusion.set_data_dof9(acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, mag_rx, mag_ry, mag_rz);
132//!
133//! // Perform a step of the algorithm
134//! fusion.step(dt);
135//!
136//! // Collect outputs
137//! let angle_x = fusion.get_x_angle();
138//! let angle_y = fusion.get_y_angle();
139//! let angle_z = fusion.get_heading();
140//! let distance = fusion.get_final_distance();
141//!
142//! // Print data
143//! std::println!("Angle X: {} °", angle_x);
144//! std::println!("Angle Y: {} °", angle_y);
145//! std::println!("Angle Z: {} °", angle_z);
146//! std::println!("Total distance traveled: {} cm", distance);
147//!
148//! delay.delay_ms(5_u8);
149//! }
150//! }
151//! ```
152
153#![no_std]
154#![deny(missing_docs)]
155
156#[macro_use]
157extern crate alloc;
158
159pub mod kalman;
160pub mod filters;
161use crate::kalman::Kalman;
162use crate::filters::{Smooth, HPFilter, LPFilter};
163
164#[allow(unused_imports)]
165use micromath::F32Ext;
166
167/// Gravity constant.
168pub const G: f32 = 9.80665;
169/// Squared G.
170pub const G2: f32 = G * G;
171/// PI. 3.14159274_f32
172pub const PI: f32 = core::f32::consts::PI;
173/// Constant to convert radians to degrees.
174pub const RAD2DEG: f32 = 180.0 / PI;
175/// Constant to convert degrees to radians.
176pub const DEG2RAD: f32 = PI / 180.0;
177/// Square root of g.
178pub const SQRT_G: f32 = 3.13155712;
179/// Tweaked square root of g. Used to correct the output value.
180pub const SQRT_G_KALMAN: f32 = SQRT_G * 3.0 / 4.0;
181/// Converts g to a scale value, here in squared centimeters.
182pub const G_TO_SCALE: f32 = 100.0;
183
184#[derive(Debug, PartialEq, Clone, Copy)]
185/// #### Enum used to define the mode of the datafusion algorithm.
186/// Use ***Dof9*** for 9 degrees of freedom ie, *Accelerometer, Gyroscope & Magnetometer*.
187/// Use ***Dof6*** for 6 degrees of freedom ie, *Accelerometer & Gyroscope*.
188pub enum Mode {
189 /// ***9*** degrees of freedom ie, *Accelerometer, Gyroscope & Magnetometer*.
190 Dof6,
191 /// ***6*** degrees of freedom ie, *Accelerometer & Gyroscope*.
192 Dof9
193}
194
195/// #### Datafusion structure to compute distance traveled and angle.
196///
197/// This will allow you to:
198/// - Chose which degrees of freedom to use.
199/// - Performs a Kalman filter:
200/// - Angle for X and Y-axis
201/// - Angle for X-Axis (9Dof) or Angular velocity for Z-Axis (6Dof and 9Dof)
202/// - Get the Z-axis angle traveled (6Dof)
203/// - Perform a series of filters X and Y-axis to:
204/// - Get the velocity on each axis
205/// - Get the distance traveled
206pub struct Fusion {
207 //Generic Variables
208 acc_x: f32,
209 acc_y: f32,
210 acc_z: f32,
211 gyro_x: f32,
212 gyro_y: f32,
213 gyro_z: f32,
214 mag_rx: f32,
215 mag_ry: f32,
216 mag_rz: f32,
217 mag_x: f32,
218 mag_y: f32,
219 roll: f32,
220 pitch: f32,
221 yaw: f32,
222 heading: f32,
223 /// Degrees of freedom.
224 mode: Mode,
225 //Kalman Variables
226 /// Final X-Axis angle.
227 kalman_x_angle: f32,
228 /// Final Y-Axis angle.
229 kalman_y_angle: f32,
230 /// Intermediate Z-Axis angle. When don't have a magnetometer, this is actually the angular velocity around the Z-axis.
231 /// When using a magnetometer, this is the angle between the magnetic field and the Z-axis.
232 kalman_z_angle: f32,
233 /// Kalman filter used to correct the output value on the X-axis.
234 kalman_x: Kalman,
235 /// Kalman filter used to correct the output value on the Y-axis.
236 kalman_y: Kalman,
237 /// Kalman filter used to correct the output value on the Z-axis.
238 kalman_z: Kalman,
239 /// Moving average filter used to smooth the output value of the heading.
240 ma_yaw: Smooth,
241 /// Corrects the final Z-axis angle. (6Dof only)
242 offset_kalman: f32,
243 /// In which position the point is relatively to the abscissa. -1 is left, 0 is neutral, 1 is right. (6Dof only)
244 sens: i8,
245 /// Last recorded sens. (6Dof only)
246 sens_prec: i8,
247 /// How many times the curve has crossed the abscissa. (6Dof only)
248 sens_count: i32,
249 /// Final Z-Axis angle. When we don't have a magnetometer, this is actually the angular velocity around the Z-axis.
250 /// When using a magnetometer, this variable is always 0.0. (6Dof only)
251 total_z_angle: f32,
252 /// Final angular velocity around the Z-axis. Only use this value when we use a magnetometer. (9Dof only)
253 angular_vel_z: f32,
254 /// Magnetic declination. (9Dof only)
255 declination: f32,
256 //Distance Variables
257 /// Enable or disable the distance computation.
258 enable_distance: bool,
259 //X-AXIS
260 //SPEED
261 /// High pass filter for the speed of the X-axis.
262 hp_vx: HPFilter,
263 /// High pass filter entry for the speed of the X-axis.
264 hp_e_vx: f32,
265 /// Old high pass filter entry for the speed of the X-axis.
266 old_hp_e_vx: f32,
267 /// High pass filter output for the speed of the X-axis.
268 hp_s_vx: f32,
269 /// Speed on the X-axis.
270 speed_x: f32,
271 /// Distance on the X-axis.
272 distance_x: f32,
273 /// Low pass filter for the speed of the X-axis.
274 lp_vx: LPFilter,
275 /// Low pass filter entry for the speed of the X-axis.
276 lp_e_vx: f32,
277 /// Low pass filter output for the speed of the X-axis.
278 lp_s_vx: f32,
279 //DISTANCE
280 /// High pass filter for the distance of the X-axis.
281 hp_dx: HPFilter,
282 /// High pass filter entry for the distance of the X-axis.
283 hp_e_dx: f32,
284 /// Old high pass filter entry for the distance of the X-axis.
285 old_hp_e_dx: f32,
286 /// High pass filter output for the distance of the X-axis.
287 hp_s_dx: f32,
288 /// Low pass filter for the distance of the X-axis.
289 lp_dx: LPFilter,
290 /// Low pass filter entry for the distance of the X-axis.
291 lp_e_dx: f32,
292 /// Low pass filter output for the distance of the X-axis.
293 lp_s_dx: f32,
294 //MOVING AVERAGE
295 /// Moving average filter of the X-axis.
296 ma_x: Smooth,
297 /// Moving average filter output value of the X-axis.
298 average_x: f32,
299 /// Offset of the X-axis.
300 offset_x: f32,
301 //Y-AXIS
302 //SPEED
303 /// High pass filter for the speed of the Y-axis.
304 hp_vy: HPFilter,
305 /// High pass filter entry for the speed of the Y-axis.
306 hp_e_vy : f32,
307 /// Old high pass filter entry for the speed of the Y-axis.
308 old_hp_e_vy: f32,
309 /// High pass filter output for the speed of the Y-axis.
310 hp_s_vy: f32,
311 /// Speed on the Y-axis.
312 speed_y: f32,
313 /// Distance on the Y-axis.
314 distance_y: f32,
315 /// Low pass filter for the speed of the Y-axis.
316 lp_vy: LPFilter,
317 /// Low pass filter entry for the speed of the Y-axis.
318 lp_e_vy: f32,
319 /// Low pass filter output for the speed of the Y-axis.
320 lp_s_vy: f32,
321 //DISTANCE
322 //High pass filter for the distance of the Y-axis.
323 hp_dy: HPFilter,
324 /// High pass filter entry for the distance of the Y-axis.
325 hp_e_dy: f32,
326 /// Old high pass filter entry for the distance of the Y-axis.
327 old_hp_e_dy: f32,
328 /// High pass filter output for the distance of the Y-axis.
329 hp_s_dy: f32,
330 /// Low pass filter for the distance of the Y-axis.
331 lp_dy: LPFilter,
332 /// Low pass filter entry for the distance of the Y-axis.
333 lp_e_dy: f32,
334 /// Low pass filter output for the distance of the Y-axis.
335 lp_s_dy: f32,
336 // MOVING AVERAGE
337 /// Moving average filter of the Y-axis.
338 ma_y: Smooth,
339 /// Moving average filter output value of the Y-axis.
340 average_y: f32,
341 /// Offset of the Y-axis.
342 offset_y: f32,
343 //Other variables
344 /// Counter used to set both X and Y-Axis offset.
345 counter: u32,
346 /// When the offsets are sets we start the filters.
347 ready_2_go: bool,
348 /// Final distance traveled by the sensor.
349 final_distance: f32,
350 // Useful constants that can be tweaked.
351 /// Factor used to scale up or down the tolerance. 1 is normal. 100 is 100 times the tolerance.
352 /// Defaults to 100 for better results. Tweaks BUFFER_ZONE_X and BUFFER_ZONE_Y.
353 scale_factor: f32,
354 /// Buffer zone used on the X-axis to compute the distance.
355 buffer_zone_x: f32,
356 /// Buffer zone used on the Y-axis to compute the distance.
357 buffer_zone_y: f32,
358 /// Correction factor to tweak the output value of our distance.
359 correction_factor: f32,
360 /// Buffer zone used with the Kalman filter (Dof6 only).
361 buffer_zone_kalman: f32,
362 /// Counter used to set the offset.
363 counter_offset: u32,
364}
365
366impl Fusion {
367 /// ### Create a new Fusion object.
368 /// ***You will need to provide data from a sensor!***
369 ///
370 /// *hp_fc* and *lp_fc* are the cut-off frequencies of the high-pass and low-pass filters.
371 /// We recommend 0.05 Hz for the high pass filter and 20.0 for the low pass filter.
372 /// If you chose to disable distance computation, the first two parameters are not important and can be set to 0.0.
373 ///
374 /// *num_readings* is the number of readings used to compute the moving average.
375 /// As the number of readings increases, more information about the signal will be lost.
376 /// On the other hand, the lower the number of readings, the rougher the signal and the more approximate the measurement.
377 /// It is therefore necessary to find a middle ground, so we recommend a value between 25 and 50.
378 ///
379 /// All variables are initialized to 0.0.
380 ///
381 /// ***Usage:***
382 /// ```no_run
383 /// use datafusion::{self as _, Fusion};
384 /// let mut fusion = Fusion::new(0.05, 20., 50);
385 /// ```
386 pub fn new(hp_fc: f32, lp_fc: f32, num_readings: usize) -> Self {
387 Fusion {
388 //General Variables
389 acc_x: 0.,
390 acc_y: 0.,
391 acc_z: 0.,
392 gyro_x: 0.,
393 gyro_y: 0.,
394 gyro_z: 0.,
395 mag_rx: 0.,
396 mag_ry: 0.,
397 mag_rz: 0.,
398 mag_x: 0.,
399 mag_y: 0.,
400 roll: 0.,
401 pitch: 0.,
402 yaw: 0.,
403 heading: 0.,
404 mode: Mode::Dof6,
405 //Kalman Variables
406 kalman_x_angle: 0.,
407 kalman_y_angle: 0.,
408 kalman_z_angle: 0.,
409 kalman_x: Kalman::new(),
410 kalman_y: Kalman::new(),
411 kalman_z: Kalman::new(),
412 ma_yaw: Smooth::new(num_readings),
413 offset_kalman: 0.,
414 sens: 0,
415 sens_prec: 0,
416 sens_count: 0,
417 total_z_angle: 0.,
418 angular_vel_z: 0.,
419 declination: 0.,
420 //Distance Variables
421 enable_distance: true,
422 //X-AXIS
423 //SPEED
424 //High pass filter for the speed of the X-axis
425 hp_vx: HPFilter::new(hp_fc),
426 hp_e_vx: 0.,
427 old_hp_e_vx: 0.,
428 hp_s_vx: 0.,
429 //We define speed and distance variables
430 speed_x: 0.,
431 distance_x: 0.,
432 //Low pass filter for the speed of the X-axis
433 lp_vx: LPFilter::new(lp_fc),
434 lp_e_vx: 0.,
435 lp_s_vx: 0.,
436 //DISTANCE
437 //High pass filter for the distance of the X-axis
438 hp_dx: HPFilter::new(hp_fc),
439 hp_e_dx: 0.,
440 old_hp_e_dx: 0.,
441 hp_s_dx: 0.,
442 //Low pass filter for the distance of the X-axis
443 lp_dx: LPFilter::new(lp_fc),
444 lp_e_dx: 0.,
445 lp_s_dx: 0.,
446 //MOVING AVERAGE
447 //Moving average filter of the X-axis
448 ma_x: Smooth::new(num_readings),
449 average_x: 0.,
450 offset_x: 0.,
451 //Y-AXIS
452 //SPEED
453 //High pass filter for the speed of the Y-axis
454 hp_vy: HPFilter::new(hp_fc),
455 hp_e_vy : 0.,
456 old_hp_e_vy: 0.,
457 hp_s_vy: 0.,
458 //We define speed and distance variables
459 speed_y: 0.,
460 distance_y: 0.,
461 //Low pass filter for the speed of the Y-axis
462 lp_vy: LPFilter::new(lp_fc),
463 lp_e_vy: 0.,
464 lp_s_vy: 0.,
465 //DISTANCE
466 //High pass filter for the distance of the Y-axis
467 hp_dy: HPFilter::new(hp_fc),
468 hp_e_dy: 0.,
469 old_hp_e_dy: 0.,
470 hp_s_dy: 0.,
471 //Low pass filter for the distance of the Y-axis
472 lp_dy: LPFilter::new(lp_fc),
473 lp_e_dy: 0.,
474 lp_s_dy: 0.,
475 //MOVING AVERAGE
476 //Moving average filter of the Y-axis
477 ma_y: Smooth::new(num_readings),
478 average_y: 0.,
479 offset_y: 0.,
480 //Other variables
481 counter: 0,
482 ready_2_go: false,
483 final_distance: 0.,
484 scale_factor: 100.,
485 buffer_zone_x: 13.,
486 buffer_zone_y: 10.,
487 correction_factor: G2 * SQRT_G,
488 buffer_zone_kalman: 0.05,
489 counter_offset: 500,
490 }
491 }
492
493 /// ### Initialize the device.
494 /// ***You must provide raw data before doing calling the function. Also, the mode defaults to a 6Dof sensor. So you might need to change the mode.***
495 ///
496 /// - Initialize the Kalman filter.
497 /// - The distance filters have been initialized with the default values within the creation of the device.
498 ///
499 /// ***Usage (9Dof):***
500 /// ```no_run
501 /// use datafusion::{self as _, Fusion};
502 /// let mut fusion = Fusion::new(0.05, 20., 50);
503 /// fusion.set_mode(datafusion::Mode::Dof9);
504 /// fusion.set_data_dof9(acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, mag_rx, mag_ry, mag_rz);
505 /// fusion.init();
506 /// ```
507 /// ***Usage (6Dof):***
508 /// ```no_run
509 /// use datafusion::{self as _, Fusion};
510 /// let mut fusion = Fusion::new(0.05, 20., 50);
511 /// fusion.set_data_dof6(acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z);
512 /// fusion.init();
513 /// ```
514 pub fn init(&mut self) {
515
516 // Initialize the Kalman filter
517 self.roll = self.compute_roll();
518 self.pitch = self.compute_pitch();
519 self.yaw = self.compute_yaw();
520
521 self.kalman_x.set_angle(self.roll);
522 self.kalman_y.set_angle(self.pitch);
523 self.kalman_z.set_angle(self.yaw);
524
525 self.offset_kalman = self.yaw;
526
527 }
528
529
530 /// ### Performs a step of all filters.
531 /// ***You must provide your own delta time in seconds.***
532 ///
533 /// The more accurate the delta time, the more accurate the results.
534 /// > 🔴 ***Always set raw data before calling this function.***
535 /// - It updates the Kalman filters on all axis.
536 /// - Then, it updates the speed and distance filters on all axis.
537 /// - Finally, it updates the moving average filter on all axis.
538 ///
539 /// ***Usage:***
540 /// ```no_run
541 /// // Init a delay used in certain functions and between each loop.
542 /// let mut delay = Delay::new();
543 /// // Setting up the delta time within a std environment
544 /// let mut time = Instant::now();
545 /// loop {
546 /// // Calculate delta time in seconds
547 /// let dt = time.elapsed().as_micros() as f32 / 1_000_000.;
548 /// time = Instant::now();
549 ///
550 /// // Set the raw data
551 /// fusion.set_data_dof9(acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, mag_rx, mag_ry, mag_rz);
552 ///
553 /// // Perform a step of the algorithm
554 /// fusion.step(dt);
555 ///
556 /// delay.delay_ms(5);
557 /// }
558 /// ```
559 /// ***You can get all the results by calling the get_\* functions.***
560 /// > ***NB: The results are not updated until you call the step function again.***
561 pub fn step(&mut self, dt: f32) {
562
563 // KALMAN
564 // Calculate roll, pitch and yaw
565 self.roll = self.compute_roll();
566 self.pitch = self.compute_pitch();
567 self.yaw = self.compute_yaw();
568
569 // Calculate the angular speed of the x, y, z axes
570 let mut gyro_x_rate = self.gyro_x * RAD2DEG;
571 let gyro_y_rate = self.gyro_y * RAD2DEG;
572 let gyro_z_rate = self.gyro_z * RAD2DEG;
573
574 // Restrict Pitch
575 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
576 if ((self.pitch < -90.0) && (self.kalman_y_angle > 90.0)) || ((self.pitch > 90.0) && (self.kalman_y_angle < -90.0)) {
577 self.kalman_y.set_angle(self.pitch);
578 self.kalman_y_angle = self.pitch;
579 } else {
580 self.kalman_y.compute_angle(self.pitch, gyro_y_rate, dt);
581 self.kalman_y_angle = self.kalman_y.get_angle();
582 }
583
584 // Invert rate, so it fits the restricted accelerometer reading
585 if self.kalman_y_angle.abs() > 90.0 {
586 gyro_x_rate = -gyro_x_rate;
587 }
588
589 // Update angles for x and z axis
590 self.kalman_x.compute_angle(self.roll, gyro_x_rate, dt);
591 self.kalman_x_angle = self.kalman_x.get_angle();
592
593 if self.mode == Mode::Dof6 {
594 self.kalman_z.compute_angle(self.yaw, gyro_z_rate, dt);
595 self.kalman_z_angle = self.kalman_z.get_angle();
596
597 let angle_z = self.kalman_z_angle - self.offset_kalman;
598
599 // Tries to correct the drift of the angle
600 // Acts like a buffer zone
601 if angle_z < -self.buffer_zone_kalman {
602 self.sens = -1;
603 } else if angle_z > self.buffer_zone_kalman {
604 self.sens = 1;
605 } else {
606 self.sens = 0;
607 }
608
609 // Counts the number of times the angle has changed
610 if (self.sens != self.sens_prec) && (self.sens != 0) {
611 self.sens_count += 1;
612 self.sens_prec = self.sens;
613 }
614
615 // Movement is done when the angle has changed more than 3 times
616 if self.sens_count > 3 {
617 self.sens_count = 0;
618 self.sens_prec = 0;
619 } else {
620 self.total_z_angle += self.compute_integral(angle_z, dt) * SQRT_G_KALMAN;
621 }
622 } else {
623 self.kalman_z.compute_angle(self.yaw, gyro_z_rate, dt);
624 self.kalman_z_angle = self.kalman_z.get_angle();
625
626 // We add a smoothing filter to get better results
627 self.ma_yaw.add_reading(self.kalman_z_angle);
628 self.heading = self.ma_yaw.get_average();
629
630 self.angular_vel_z = self.heading * dt;
631 }
632
633 if self.enable_distance {
634 // DISTANCE
635 // Offset management
636 if self.counter == self.counter_offset {
637 self.offset_x = self.average_x;
638 self.offset_y = self.average_y;
639 self.ready_2_go = true;
640 self.distance_x = 0.;
641 self.distance_y = 0.;
642 }
643 if self.counter < self.counter_offset + 1 {
644 self.counter += 1;
645 }
646
647 // Scale acceleration
648 let accel_x = self.acc_x * G_TO_SCALE;
649 let accel_y = self.acc_y * G_TO_SCALE;
650
651 // X-AXIS
652 // We update the old value of the speed HP filter
653 //self.old_hp_e_vx = self.hp_e_vx; // We don't need this anymore as it is handled with the function set_old_values.
654
655 // HP filter for the speed of the X-axis
656 self.hp_e_vx = accel_x;
657 self.hp_s_vx = self.hp_vx.compute(self.hp_e_vx, self.old_hp_e_vx, self.hp_s_vx, dt);
658
659 // LP filter for the speed of the X-axis
660 self.lp_e_vx = self.hp_s_vx;
661 self.lp_s_vx = self.lp_vx.compute(self.lp_e_vx, self.lp_s_vx, dt);
662
663 // We compute VX
664 self.speed_x = self.compute_integral(self.lp_s_vx, dt);
665
666 // we update the old value of the distance HP filter
667 self.old_hp_e_dx = self.hp_e_dx;
668
669 // HP filter for the distance of the X-axis
670 self.hp_e_dx = self.speed_x;
671 self.hp_s_dx = self.hp_dx.compute(self.hp_e_dx, self.old_hp_e_dx, self.hp_s_dx, dt);
672
673 // LP filter for the distance of the X-axis
674 self.lp_e_dx = self.hp_s_dx;
675 self.lp_s_dx = self.lp_dx.compute(self.lp_e_dx, self.lp_s_dx, dt);
676
677
678 // Y-AXIS
679 // We update the old value of the speed HP filter
680 //self.old_hp_e_vy = self.hp_e_vy; // We don't need this anymore as it is handled with the function set_old_values.
681
682 // HP filter for the speed of the Y-axis
683 self.hp_e_vy = accel_y;
684 self.hp_s_vy = self.hp_vy.compute(self.hp_e_vy, self.old_hp_e_vy, self.hp_s_vy, dt);
685
686 // LP filter for the speed of the Y-axis
687 self.lp_e_vy = self.hp_s_vy;
688 self.lp_s_vy = self.lp_vy.compute(self.lp_e_vy, self.lp_s_vy, dt);
689
690 // We compute VY
691 self.speed_y = self.compute_integral(self.lp_s_vy, dt);
692
693 // we update the old value of the distance HP filter
694 self.old_hp_e_dy = self.hp_e_dy;
695
696 // HP filter for the distance of the Y-axis
697 self.hp_e_dy = self.speed_y;
698 self.hp_s_dy = self.hp_dy.compute(self.hp_e_dy, self.old_hp_e_dy, self.hp_s_dy, dt);
699
700 // LP filter for the distance of the Y-axis
701 self.lp_e_dy = self.hp_s_dy;
702 self.lp_s_dy = self.lp_dy.compute(self.lp_e_dy, self.lp_s_dy, dt);
703
704 // Smoothing the curves
705 self.ma_x.add_reading(self.lp_s_dx);
706 self.average_x = self.ma_x.get_average() * self.scale_factor - self.offset_x;
707 self.ma_y.add_reading(self.lp_s_dy);
708 self.average_y = self.ma_y.get_average() * self.scale_factor - self.offset_y;
709
710 let factor = self.correction_factor / self.scale_factor;
711
712 // We compute the distance on the X-axis
713 if (self.average_x.abs() > self.buffer_zone_x) && self.ready_2_go {
714 self.distance_x += self.compute_integral(self.average_x, dt) * factor;
715 }
716
717 // We compute the distance on the Y-axis
718 if (self.average_y.abs() > self.buffer_zone_y) && self.ready_2_go {
719 self.distance_y += self.compute_integral(self.average_y, dt) * factor;
720 }
721
722 let dax = self.kalman_x_angle.sin() * G;
723 let day = self.kalman_y_angle.sin() * G;
724 let dpx = 0.5 * dax * dt * dt;
725 let dpy = 0.5 * day * dt * dt;
726
727 self.distance_x = (self.distance_x - dpx * self.distance_x).abs();
728 self.distance_y = (self.distance_y - dpy * self.distance_y).abs();
729
730 // We compute the final distance
731 self.final_distance = self.distance(self.distance_x, self.distance_y);
732 }
733 }
734
735 /// #### Set data from the accelerometer.
736 /// Should be in *m/s²*.
737 pub fn set_accel_data(&mut self, acc_x: f32, acc_y: f32, acc_z: f32) {
738 self.acc_x = acc_x;
739 self.acc_y = acc_y;
740 self.acc_z = acc_z;
741 }
742
743 /// #### Set data from the gyroscope.
744 /// Should be in *rad/s*.
745 pub fn set_gyro_data(&mut self, gyro_x: f32, gyro_y: f32, gyro_z: f32) {
746 self.gyro_x = gyro_x;
747 self.gyro_y = gyro_y;
748 self.gyro_z = gyro_z;
749 }
750
751 /// #### Set data from the magnetometer.
752 /// Should be in *uT*.
753 pub fn set_mag_data(&mut self, mag_x: f32, mag_y: f32, mag_z: f32) {
754 self.mag_rx = mag_x;
755 self.mag_ry = mag_y;
756 self.mag_rz = mag_z;
757 }
758
759 /// #### Set data all in once for the accelerometer, gyroscope and magnetometer.
760 /// Should be in *m/s²*, *rad/s* and *uT*.
761 pub fn set_data_dof9(&mut self, acc_x: f32, acc_y: f32, acc_z: f32, gyro_x: f32, gyro_y: f32, gyro_z: f32, mag_rx: f32, mag_ry: f32, mag_rz: f32) {
762 self.acc_x = acc_x;
763 self.acc_y = acc_y;
764 self.acc_z = acc_z;
765 self.gyro_x = gyro_x;
766 self.gyro_y = gyro_y;
767 self.gyro_z = gyro_z;
768 self.mag_rx = mag_rx;
769 self.mag_ry = mag_ry;
770 self.mag_rz = mag_rz;
771 }
772
773
774 /// #### Set data all in once for the accelerometer and gyroscope.
775 /// Should be in *m/s²* and *rad/s*.
776 pub fn set_data_dof6(&mut self, acc_x: f32, acc_y: f32, acc_z: f32, gyro_x: f32, gyro_y: f32, gyro_z: f32) {
777 self.acc_x = acc_x;
778 self.acc_y = acc_y;
779 self.acc_z = acc_z;
780 self.gyro_x = gyro_x;
781 self.gyro_y = gyro_y;
782 self.gyro_z = gyro_z;
783 }
784
785 /// #### Set old value for the High Pass Filter.
786 /// Should be in *m/s²*.
787 ///
788 /// ***This function must be called before reading data from the sensor.***
789 pub fn set_old_values(&mut self, acc_x: f32, acc_y: f32) {
790 self.old_hp_e_vx = acc_x * G_TO_SCALE;
791 self.old_hp_e_vy = acc_y * G_TO_SCALE;
792 }
793
794 /// Returns the scaled value of the magnetic field on the X-Axis once computed with roll and pitch.
795 /// Data is in *microtesla (uT)*.
796 pub fn get_mag_x(&self) -> f32 {
797 self.mag_x
798 }
799
800 /// Returns the scaled value of the magnetic field on the Y-Axis once computed with roll.
801 /// Data is in *microtesla (uT)*.
802 pub fn get_mag_y(&self) -> f32 {
803 self.mag_y
804 }
805
806 /// Computes the integral of the angular velocity.
807 /// Returns the integral in degrees.
808 /// Angle should be in degrees/second.
809 /// dt is the time in seconds since the last call to this function.
810 fn compute_integral(&self, value: f32, dt: f32) -> f32 {
811 value * dt
812 }
813
814 /// Computes the distance
815 fn distance(&self, x: f32, y: f32) -> f32 {
816 (x * x + y * y).sqrt()
817 }
818
819 /// #### Computes roll from accelerometer data.
820 /// Computes and returns the roll in *degrees*.
821 pub fn compute_roll(&self) -> f32 {
822 self.acc_y.atan2(self.acc_z) * RAD2DEG
823 }
824
825 /// #### Computes pitch from accelerometer data.
826 /// computes and returns the pitch in *degrees*.
827 pub fn compute_pitch(&self) -> f32 {
828 (-self.acc_x / (self.acc_y * self.acc_y + self.acc_z * self.acc_z).sqrt()).atan() * RAD2DEG
829 }
830
831 /// #### Computes yaw from accelerometer data.
832 /// Computes and returns the yaw in *degrees* for Dof9 and the yaw velocity in *degrees/s* for Dof6.
833 pub fn compute_yaw(&mut self) -> f32 {
834
835 if self.mode == Mode::Dof6 {
836 (self.acc_z / (self.acc_x * self.acc_x + self.acc_z * self.acc_z).sqrt()).atan() * RAD2DEG
837 } else {
838 let roll_rad = self.roll * DEG2RAD;
839 let pitch_rad = self.pitch * DEG2RAD;
840 // Tilt compensation
841 self.mag_x = self.mag_rx * pitch_rad.cos() + self.mag_ry * roll_rad.sin() * pitch_rad.sin() - self.mag_rz * self.roll.cos() * pitch_rad.sin();
842 self.mag_y = -self.mag_ry * roll_rad.cos() + self.mag_rz * roll_rad.sin();
843 (-self.mag_y.atan2(self.mag_x)) * RAD2DEG
844 }
845 }
846
847 /// #### Get the heading from the magnetometer.
848 /// This is the heading of the sensor after the Kalman filter and the moving average.
849 /// The heading is updated every time the `step` function is called.
850 ///
851 /// Returns the heading in *degrees*.
852 ///
853 /// In Dof9 mode the heading is computed from the magnetometer data.
854 /// Thus, this is the real heading of the sensor.
855 /// In Dof6 mode the heading is computed from the accelerometer data.
856 /// Thus, this is the same as the yaw.
857 ///
858 /// ***Note***: This function is just a getter. If you want to compute and get the actual yaw, use `compute_yaw()` or run a step.
859 pub fn get_heading(&self) -> f32 {
860 if self.mode == Mode::Dof9 {
861 self.heading + self.declination
862 } else {
863 self.yaw
864 }
865 }
866
867 /// #### Set the measured distance to 0.
868 /// Acts like an artificial reset.
869 pub fn reset_distance(&mut self) {
870 self.distance_x = 0.;
871 self.distance_y = 0.;
872 self.final_distance = 0.;
873 }
874
875 /// #### Set the measured angle to 0.
876 /// Acts like an artificial reset.
877 ///
878 /// ***NB: This is only useful for Dof6.***
879 pub fn reset_angle_z(&mut self) {
880 if self.mode == Mode::Dof6 {
881 self.total_z_angle = 0.;
882 }
883 }
884
885 /// #### Set the distance offsets to the current state of the system.
886 ///
887 /// ***Warning: you can't go back to the previous state once changed.***
888 ///
889 /// Usually used when auto-resetting the distance variables.
890 /// Must be called when the sensor does NOT move. Otherwise, next data won't be accurate.
891 pub fn reset_offset_distance(&mut self) {
892 self.offset_x = self.average_x;
893 self.offset_y = self.average_y;
894 }
895
896 /// #### Set the Kalman offsets to the current state of the system.
897 /// ***Warning: you can't go back to the previous state once changed.***
898 ///
899 /// Usually used when auto-resetting the angle variables.
900 /// Must be called when the sensor does ***NOT*** move. Otherwise, next data won't be accurate.
901 ///
902 /// ***NB: This is only useful for Dof6.***
903 pub fn reset_offset_angle(&mut self) {
904 if self.mode == Mode::Dof6 {
905 self.offset_kalman = self.yaw;
906 }
907 }
908
909 /// #### Get the Final X-Axis angle in *degrees*.
910 /// Use DEG2RAD const to convert to *rad*.
911 pub fn get_x_angle(&self) -> f32 {
912 self.kalman_x_angle
913 }
914
915 /// #### Get the Final Y-Axis angle in *degrees*.
916 /// Use DEG2RAD const to convert to *rad*.
917 pub fn get_y_angle(&self) -> f32 {
918 self.kalman_y_angle
919 }
920
921 /// #### Get the Final Z-Axis angle in *degrees*.
922 /// If the sensor is in Dof9 mode, the angle is the yaw angle in after the Kalman filter.
923 ///
924 /// If the sensor is in Dof6 mode, this is the accumulated angle traveled by the sensor.
925 ///
926 /// Use DEG2RAD const to convert to *radians*.
927 pub fn get_z_angle(&self) -> f32 {
928 if self.mode == Mode::Dof6 {
929 self.total_z_angle
930 } else {
931 self.kalman_z_angle
932 }
933
934 }
935
936 /// #### Get the Z-axis angle velocity in *degrees/s*.
937 /// If the sensor is in Dof9 mode, the value is the derivative of the yaw angle.
938 ///
939 /// If the sensor is in Dof6 mode, the value is the output of kalman filter.
940 ///
941 /// Use DEG2RAD const to convert to *rad/s*.
942 pub fn get_z_angular_velocity(&self) -> f32 {
943
944 if self.mode == Mode::Dof6 {
945 self.kalman_z_angle - self.offset_kalman
946 } else {
947 self.angular_vel_z
948 }
949 }
950
951 /// Get the current speed on the X-axis in *cm/s*.
952 pub fn get_x_speed(&self) -> f32 {
953 self.speed_x
954 }
955
956 /// Get the current speed on the Y-axis in *cm/s*.
957 pub fn get_y_speed(&self) -> f32 {
958 self.speed_y
959 }
960
961 /// Get the current distance traveled on the X-axis in *cm*.
962 pub fn get_x_distance(&self) -> f32 {
963 self.distance_x
964 }
965
966 /// Get the current distance traveled on the Y-axis in *cm*.
967 pub fn get_y_distance(&self) -> f32 {
968 self.distance_y
969 }
970
971 /// Get the current final distance traveled in *cm*.
972 pub fn get_final_distance(&self) -> f32 {
973 self.final_distance
974 }
975
976 /// Get the fusion's mode.
977 pub fn get_mode(&self) -> Mode {
978 self.mode
979 }
980
981 /// #### Set the fusion's mode.
982 /// Either Dof6 or Dof9, respectively for 6-axis and 9-axis degrees of freedom.
983 pub fn set_mode(&mut self, mode: Mode) {
984 self.mode = mode;
985 }
986
987 /// #### Set the declination of the magnetic field.
988 /// Must be in degrees. (Dof9 only)
989 ///
990 /// You can visit this [website](https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml?#declination) to get the declination.
991 pub fn set_declination(&mut self, declination: f32) {
992 self.declination = declination;
993 }
994
995 /// #### Get the declination of the magnetic field.
996 /// Returns the declination in degrees as a f32 (Dof9 only).
997 /// The declination is the difference between the true north and the magnetic north.
998 pub fn get_declination(&self) -> f32 {
999 self.declination
1000 }
1001
1002 /// #### Set the buffer zone on the X-Axis.
1003 /// This buffer zone is used while measuring distance.
1004 /// It's made to avoid to constantly count the distance.
1005 ///
1006 /// It must be a percentage of the scale factor (that defaults to 100).
1007 ///
1008 /// Defaults to 0.13 (13% of the scale factor).
1009 pub fn set_buffer_zone_x(&mut self, buffer_zone: f32) {
1010 self.buffer_zone_x = buffer_zone * self.scale_factor;
1011 }
1012
1013 /// #### Set the buffer zone on the Y-Axis.
1014 /// This buffer zone is used while measuring distance.
1015 /// It's made to avoid to constantly count the distance.
1016 ///
1017 /// It must be a percentage of the scale factor (that defaults to 100).
1018 ///
1019 /// Defaults to 0.10 (10% of the scale factor).
1020 pub fn set_buffer_zone_y(&mut self, buffer_zone: f32) {
1021 self.buffer_zone_y = buffer_zone * self.scale_factor;
1022 }
1023
1024 /// #### Set the scale factor for the distance measurements.
1025 /// This is the factor that is used to get a more accurate distance measurement
1026 /// and get a better readability of the distance while working in cm.
1027 ///
1028 /// Defaults to 100.
1029 pub fn set_scale_factor(&mut self, scale_factor: f32) {
1030 self.scale_factor = scale_factor;
1031 }
1032
1033 /// #### Set the correction factor for all Axis.
1034 /// This is the factor that is used to correct the distance measurements.
1035 /// While filtering the data, we are losing some precision in amplitude.
1036 /// To avoid this, we are applying a correction factor to the data.
1037 ///
1038 /// Defaults to G2 / SQRT_G, where G2 is the gravity constant squared and SQRT_G is the square root of G.
1039 pub fn set_correction_factor(&mut self, correction_factor: f32) {
1040 self.correction_factor = correction_factor;
1041 }
1042
1043 /// #### Set the buffer zone on the Z-Axis for the Kalman filter.
1044 /// DOF6 only.
1045 /// Defaults to 0.05.
1046 pub fn set_buffer_zone_kalman(&mut self, buffer_zone: f32) {
1047 self.buffer_zone_kalman = buffer_zone;
1048 }
1049
1050 /// #### Set the offset counter for the distance measurements.
1051 /// To measure the distance, we need the signal to be stable.
1052 /// So we ignore the first few measurements.
1053 ///
1054 /// Defaults to 500.
1055 pub fn set_counter_offset(&mut self, counter_offset: u32) {
1056 self.counter_offset = counter_offset;
1057 }
1058
1059 /// #### Disable the distance measurements.
1060 /// This allows to gain execution speed.
1061 ///
1062 /// Defaults to true. (Distance measurements are enabled by default)
1063 pub fn disable_distance(&mut self, disable: bool) {
1064 self.enable_distance = disable;
1065 }
1066
1067}