Struct Fusion 

pub struct Fusion { /* private fields */ }

Datafusion structure to compute distance traveled and angle.

This will allow you to:

• Chose which degrees of freedom to use.
• Performs a Kalman filter:
  • Angle for X and Y-axis
  • Angle for X-Axis (9Dof) or Angular velocity for Z-Axis (6Dof and 9Dof)
• Get the Z-axis angle traveled (6Dof)
• Perform a series of filters X and Y-axis to:
  • Get the velocity on each axis
  • Get the distance traveled

Implementations

impl Fusion

pub fn new(hp_fc: f32, lp_fc: f32, num_readings: usize) -> Self

Create a new Fusion object.

You will need to provide data from a sensor!

hp_fc and lp_fc are the cut-off frequencies of the high-pass and low-pass filters. We recommend 0.05 Hz for the high pass filter and 20.0 for the low pass filter. If you chose to disable distance computation, the first two parameters are not important and can be set to 0.0.

num_readings is the number of readings used to compute the moving average. As the number of readings increases, more information about the signal will be lost. On the other hand, the lower the number of readings, the rougher the signal and the more approximate the measurement. It is therefore necessary to find a middle ground, so we recommend a value between 25 and 50.

All variables are initialized to 0.0.

Usage:

use datafusion::{self as _, Fusion};
let mut fusion = Fusion::new(0.05, 20., 50);

pub fn init(&mut self)

Initialize the device.

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.

• Initialize the Kalman filter.
• The distance filters have been initialized with the default values within the creation of the device.

Usage (9Dof):

use datafusion::{self as _, Fusion};
let mut fusion = Fusion::new(0.05, 20., 50);
fusion.set_mode(datafusion::Mode::Dof9);
fusion.set_data_dof9(acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, mag_rx, mag_ry, mag_rz);
fusion.init();

Usage (6Dof):

use datafusion::{self as _, Fusion};
let mut fusion = Fusion::new(0.05, 20., 50);
fusion.set_data_dof6(acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z);
fusion.init();

pub fn step(&mut self, dt: f32)

Performs a step of all filters.

You must provide your own delta time in seconds.

The more accurate the delta time, the more accurate the results.

🔴 Always set raw data before calling this function.

• It updates the Kalman filters on all axis.
• Then, it updates the speed and distance filters on all axis.
• Finally, it updates the moving average filter on all axis.

Usage:

// Init a delay used in certain functions and between each loop.
let mut delay = Delay::new();
// Setting up the delta time within a std environment
let mut time = Instant::now();
loop {
    // Calculate delta time in seconds
   let dt = time.elapsed().as_micros() as f32 / 1_000_000.;
   time = Instant::now();
 
   // Set the raw data
   fusion.set_data_dof9(acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, mag_rx, mag_ry, mag_rz);
 
   // Perform a step of the algorithm
   fusion.step(dt);
 
   delay.delay_ms(5);
}

You can get all the results by calling the get_* functions.

NB: The results are not updated until you call the step function again.

pub fn set_accel_data(&mut self, acc_x: f32, acc_y: f32, acc_z: f32)

Set data from the accelerometer.

Should be in m/s².

pub fn set_gyro_data(&mut self, gyro_x: f32, gyro_y: f32, gyro_z: f32)

Set data from the gyroscope.

Should be in rad/s.

pub fn set_mag_data(&mut self, mag_x: f32, mag_y: f32, mag_z: f32)

Set data from the magnetometer.

Should be in uT.

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, )

Set data all in once for the accelerometer, gyroscope and magnetometer.

Should be in m/s², rad/s and uT.

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, )

Set data all in once for the accelerometer and gyroscope.

Should be in m/s² and rad/s.

pub fn set_old_values(&mut self, acc_x: f32, acc_y: f32)

Set old value for the High Pass Filter.

Should be in m/s².

This function must be called before reading data from the sensor.

pub fn get_mag_x(&self) -> f32

Returns the scaled value of the magnetic field on the X-Axis once computed with roll and pitch. Data is in microtesla (uT).

pub fn get_mag_y(&self) -> f32

Returns the scaled value of the magnetic field on the Y-Axis once computed with roll. Data is in microtesla (uT).

pub fn compute_roll(&self) -> f32

Computes roll from accelerometer data.

Computes and returns the roll in degrees.

pub fn compute_pitch(&self) -> f32

Computes pitch from accelerometer data.

computes and returns the pitch in degrees.

pub fn compute_yaw(&mut self) -> f32

Computes yaw from accelerometer data.

Computes and returns the yaw in degrees for Dof9 and the yaw velocity in degrees/s for Dof6.

pub fn get_heading(&self) -> f32

Get the heading from the magnetometer.

This is the heading of the sensor after the Kalman filter and the moving average. The heading is updated every time the step function is called.

Returns the heading in degrees.

In Dof9 mode the heading is computed from the magnetometer data. Thus, this is the real heading of the sensor. In Dof6 mode the heading is computed from the accelerometer data. Thus, this is the same as the yaw.

Note: This function is just a getter. If you want to compute and get the actual yaw, use compute_yaw() or run a step.

pub fn reset_distance(&mut self)

Set the measured distance to 0.

Acts like an artificial reset.

pub fn reset_angle_z(&mut self)

Set the measured angle to 0.

Acts like an artificial reset.

NB: This is only useful for Dof6.

pub fn reset_offset_distance(&mut self)

Set the distance offsets to the current state of the system.

Warning: you can’t go back to the previous state once changed.

Usually used when auto-resetting the distance variables. Must be called when the sensor does NOT move. Otherwise, next data won’t be accurate.

pub fn reset_offset_angle(&mut self)

Set the Kalman offsets to the current state of the system.

Warning: you can’t go back to the previous state once changed.

Usually used when auto-resetting the angle variables.
Must be called when the sensor does NOT move. Otherwise, next data won’t be accurate.

NB: This is only useful for Dof6.

pub fn get_x_angle(&self) -> f32

Get the Final X-Axis angle in degrees.

Use DEG2RAD const to convert to rad.

pub fn get_y_angle(&self) -> f32

Get the Final Y-Axis angle in degrees.

Use DEG2RAD const to convert to rad.

pub fn get_z_angle(&self) -> f32

Get the Final Z-Axis angle in degrees.

If the sensor is in Dof9 mode, the angle is the yaw angle in after the Kalman filter.

If the sensor is in Dof6 mode, this is the accumulated angle traveled by the sensor.

Use DEG2RAD const to convert to radians.

pub fn get_z_angular_velocity(&self) -> f32

Get the Z-axis angle velocity in degrees/s.

If the sensor is in Dof9 mode, the value is the derivative of the yaw angle.

If the sensor is in Dof6 mode, the value is the output of kalman filter.

Use DEG2RAD const to convert to rad/s.

pub fn get_x_speed(&self) -> f32

Get the current speed on the X-axis in cm/s.

pub fn get_y_speed(&self) -> f32

Get the current speed on the Y-axis in cm/s.

pub fn get_x_distance(&self) -> f32

Get the current distance traveled on the X-axis in cm.

pub fn get_y_distance(&self) -> f32

Get the current distance traveled on the Y-axis in cm.

pub fn get_final_distance(&self) -> f32

Get the current final distance traveled in cm.

pub fn get_mode(&self) -> Mode

Get the fusion’s mode.

pub fn set_mode(&mut self, mode: Mode)

Set the fusion’s mode.

Either Dof6 or Dof9, respectively for 6-axis and 9-axis degrees of freedom.

pub fn set_declination(&mut self, declination: f32)

Set the declination of the magnetic field.

Must be in degrees. (Dof9 only)

You can visit this website to get the declination.

pub fn get_declination(&self) -> f32

Get the declination of the magnetic field.

Returns the declination in degrees as a f32 (Dof9 only). The declination is the difference between the true north and the magnetic north.

pub fn set_buffer_zone_x(&mut self, buffer_zone: f32)

Set the buffer zone on the X-Axis.

This buffer zone is used while measuring distance. It’s made to avoid to constantly count the distance.

It must be a percentage of the scale factor (that defaults to 100).

Defaults to 0.13 (13% of the scale factor).

pub fn set_buffer_zone_y(&mut self, buffer_zone: f32)

Set the buffer zone on the Y-Axis.

This buffer zone is used while measuring distance. It’s made to avoid to constantly count the distance.

It must be a percentage of the scale factor (that defaults to 100).

Defaults to 0.10 (10% of the scale factor).

pub fn set_scale_factor(&mut self, scale_factor: f32)

Set the scale factor for the distance measurements.

This is the factor that is used to get a more accurate distance measurement and get a better readability of the distance while working in cm.

Defaults to 100.

pub fn set_correction_factor(&mut self, correction_factor: f32)

Set the correction factor for all Axis.

This is the factor that is used to correct the distance measurements. While filtering the data, we are losing some precision in amplitude. To avoid this, we are applying a correction factor to the data.

Defaults to G2 / SQRT_G, where G2 is the gravity constant squared and SQRT_G is the square root of G.

pub fn set_buffer_zone_kalman(&mut self, buffer_zone: f32)

Set the buffer zone on the Z-Axis for the Kalman filter.

DOF6 only. Defaults to 0.05.

pub fn set_counter_offset(&mut self, counter_offset: u32)

Set the offset counter for the distance measurements.

To measure the distance, we need the signal to be stable. So we ignore the first few measurements.

Defaults to 500.

pub fn disable_distance(&mut self, disable: bool)

Disable the distance measurements.

This allows to gain execution speed.

Defaults to true. (Distance measurements are enabled by default)