用于 6 和 9 个自由度传感器的 Datafusion 库。

在此库中，您可以选择在两种类型的传感器上应用过滤器

• 一个具有 3 轴加速度计和 3 轴陀螺仪的 6 个自由度传感器 -> 模式 Dof6
• 一个具有 3 轴加速度计、3 轴陀螺仪和 3 轴磁力计的 9 个自由度传感器 -> 模式 Dof9

🔴 目前，传感器需要完全平坦才能工作。同时使用角度和距离测量不推荐使用 6Dof 传感器。
具体来说，对于距离来说，这不是很重要，因为高通滤波器会在几秒钟后自动删除角度偏移。
然而，对于角度（除磁力计外），传感器必须保持平坦，或者至少在初始化后 X 和 Y 轴的倾斜度相同。

在下面的示例中，我们使用自己的 Adafruit 传感器的驱动程序。当然，您可以使用任何其他您想要的驱动程序。

角度数据

当传感器平坦并且使用任何模式时，X 和 Y 角度是绝对滚动和俯仰值（以度为单位）。通过在加速度和角速度之间应用卡尔曼滤波器，产生可靠的输出。在使用 Dof6 传感器时，例如没有磁力计，Z 或偏航轴上的卡尔曼滤波器输出是每秒度的角速度。然后对其进行积分以产生绝对角度（以度为单位）。然而，这种方法并不太可靠，因为它严重依赖于传感器移动的速度。然而，当使用 Dof9 传感器时，Z 或偏航轴上的卡尔曼滤波器输出是绝对角度（以度为单位）。这是首选方法。只要没有磁性干扰，您将能够获得一个指向 2 到 3 度精度的航向角。在磁性干扰的情况下，如果它比地球磁场大得多，那么输出将不是磁北，而是相对于磁性干扰的角度。

距离数据

对于距离数据，无论使用哪种模式，算法都是相同的，因为它只使用 X 和 Y 轴上的加速度数据。它是一系列高通滤波器、低通滤波器和积分。在短距离内（例如 < 5cm）工作得非常好，并且在这个范围内非常准确。然而，超过这个范围，结果高度依赖于速度。这意味着，如果速度太低，距离将被低估。如果速度太高，距离将被高估。目前正在解决这个问题，如果找到解决方案，库将进行更新。

您可以通过调用disable_distance函数来禁用距离测量。

请注意，距离测量完全是实验性的，但它是一个起点，因为精度并不理想。

用法

以下示例使用rppal包，并在树莓派上运行。

请注意，此示例和库是与Adafruit的Dof9传感器驱动程序并行开发的。

use std::time::Instant;

use rppal::hal::Delay;
use rppal::i2c::I2c;

use embedded_hal::blocking::delay::*;

use adafruit::*;
use datafusion::{self as _, Fusion};

fn main() -> Result<(), SensorError<rppal::i2c::Error>> {

    // Init a delay used in certain functions and between each loop.
    let mut delay = Delay::new();

    // Setup the raspberry's I2C interface to create the sensor.
    let i2c = I2c::new().unwrap();

    // Create an Adafruit object
    let mut sensor = AdafruitNXP::new(0x8700A, 0x8700B, 0x0021002C, i2c);

    // Check if the sensor is ready to go
    let ready = sensor.begin()?;
    if !ready {
        std::eprintln!("Sensor not detected, check your wiring!");
        std::process::exit(1);
    }

    sensor.set_accel_range(config::AccelMagRange::Range8g)?;
    sensor.set_gyro_range(config::GyroRange::Range500dps)?;
    // etc...

    // Initialize the sensor
    sensor.read_data()?;

    let acc_x = sensor.accel_sensor.get_scaled_x();
    let acc_y = sensor.accel_sensor.get_scaled_y();
    let acc_z = sensor.accel_sensor.get_scaled_z();

    let gyro_x = sensor.gyro_sensor.get_scaled_x();
    let gyro_y = sensor.gyro_sensor.get_scaled_y();
    let gyro_z = sensor.gyro_sensor.get_scaled_z();

    let mag_rx = sensor.mag_sensor.get_scaled_x();
    let mag_ry = sensor.mag_sensor.get_scaled_y();
    let mag_rz = sensor.mag_sensor.get_scaled_z();

    // Create a datafusion object
    let mut fusion = Fusion::new(0.05, 20., 50);
    fusion.set_mode(datafusion::Mode::Dof9);

    // Set data to the fusion object
    fusion.set_data_dof9(acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, mag_rx, mag_ry, mag_rz);

    // Initialize the datafusion object
    fusion.init();

    // Set magnetic declination --> 1.39951° in Toulouse, France
    fusion.set_declination(1.39951);

    // Setting up the delta time
    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();

        // Update old values for the next loop
        fusion.set_old_values(acc_x, acc_y);

        sensor.read_data()?;

        let acc_x = sensor.accel_sensor.get_scaled_x();
        let acc_y = sensor.accel_sensor.get_scaled_y();
        let acc_z = sensor.accel_sensor.get_scaled_z();

        let gyro_x = sensor.gyro_sensor.get_scaled_x();
        let gyro_y = sensor.gyro_sensor.get_scaled_y();
        let gyro_z = sensor.gyro_sensor.get_scaled_z();

        let mag_rx = sensor.mag_sensor.get_scaled_x();
        let mag_ry = sensor.mag_sensor.get_scaled_y();
        let mag_rz = sensor.mag_sensor.get_scaled_z();

        // Set data to the fusion object
        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);

        // Collect outputs
        let angle_x = fusion.get_x_angle();
        let angle_y = fusion.get_y_angle();
        let angle_z = fusion.get_heading();
        let distance = fusion.get_final_distance();

        // Print data
        std::println!("Angle X: {} °", angle_x);
        std::println!("Angle Y: {} °", angle_y);
        std::println!("Angle Z: {} °", angle_z);
        std::println!("Total distance traveled: {} cm", distance);

        delay.delay_ms(5_u8);
    }
}