Merge pull request #4 from JanMH/update_by_duration

Update by duration

examples/simple_by_duration.rs

@@ -0,0 +1,22 @@
+use imu_fusion::FusionAhrsSettings;
+use std::time::{self, Instant};
+
+const SAMPLE_RATE_HZ: u32 = 100;
+
+fn main() {
+    let ahrs_settings = FusionAhrsSettings::new();
+    let mut fusion = imu_fusion::Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
+    let mut last_timestamp: time::Instant = time::Instant::now();
+    loop {
+        let gyr = imu_fusion::FusionVector::new(0f32, 0f32, 0f32); // replace this with actual gyroscope data in degrees/s
+        let acc = imu_fusion::FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual accelerometer data in g
+
+        // obtain a timestamp since the start of measurements as f32 in seconds
+        let delta_t = last_timestamp.elapsed().as_secs_f32();
+
+        fusion.update_no_mag_by_duration_seconds(gyr, acc, delta_t);
+        let euler = fusion.euler();
+        println!("Roll {}, Pitch {}, Yaw {}", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
+        last_timestamp = Instant::now();
+    }
+}

src/fusion_impl.rs

@@ -29,7 +29,7 @@ impl Fusion {
 
     /// Updates the AHRS algorithm based on gyroscope data in degrees/s and acceleration data in g force.
     ///
-    /// The time is provided using an absolute timestamp in seconds since the last measurement.
+    /// The time is provided using an absolute timestamp in seconds since the first measurement.
     /// Note that if you provide unix timestamps, the precision of f32 will not be enough to correctly compute the time difference.
     ///
     /// # Examples
@@ -52,6 +52,35 @@ impl Fusion {
     /// ```
     pub fn update_no_mag(&mut self, gyr: FusionVector, acc: FusionVector, timestamp: f32) {
         let delta_t = timestamp - self.last_timestamp;
+        self.update_no_mag_by_duration_seconds(gyr, acc, delta_t);
+        self.last_timestamp = timestamp;
+    }
+
+    /// Updates the AHRS algorithm based on gyroscope data in degrees/s and acceleration data in g force.
+    ///
+    /// The time is provided as a duration in seconds since the last measurement.
+    /// Note that this won't increase the internal timestamp and using the timestamp version of update functions will produce incorrect results.
+    ///
+    /// # Examples
+    /// ```no_run
+    /// use imu_fusion::{Fusion, FusionVector, FusionAhrsSettings};
+    /// use std::time::{Duration, Instant};
+    ///
+    /// const SAMPLE_RATE_HZ: u32 = 100;
+    ///
+    /// let ahrs_settings = FusionAhrsSettings::new();
+    /// let mut fusion = imu_fusion::Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
+    /// let mut last_ts = Instant::now();
+    ///
+    /// loop {
+    ///     let gyr = FusionVector::new(0f32, 0f32, 0f32); // replace this with actual gyroscope data in degrees/s
+    ///     let acc = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual accelerometer data in g
+    ///     fusion.update_no_mag_by_duration_seconds(gyr, acc, last_ts.elapsed().as_secs_f32());
+    ///     last_ts = Instant::now();
+    /// }
+    ///
+    /// ```
+    pub fn update_no_mag_by_duration_seconds(&mut self, gyr: FusionVector, acc: FusionVector, delta_t: f32) {
         // Apply calibration
         let mut gyr = self.inertial_calibration(gyr, self.gyr_misalignment, self.gyr_sensitivity, self.gyr_offset);
         let acc = self.inertial_calibration(acc, self.acc_misalignment, self.acc_sensitivity, self.acc_offset);
@@ -60,12 +89,11 @@ impl Fusion {
         gyr = self.offset.update(gyr);
 
         self.ahrs.update_no_mag(gyr, acc, delta_t);
-        self.last_timestamp = timestamp;
     }
 
     /// Updates the AHRS algorithm based on gyroscope data in degrees/s, acceleration data in g force and a heading in degrees.
     ///
-    /// The time is provided using an absolute timestamp in seconds since the last measurement.
+    /// The time is provided using an absolute timestamp in seconds since the first measurement.
     /// Note that if you provide unix timestamps, the precision of f32 will not be enough to correctly compute the time difference.
     ///
     /// # Examples
@@ -95,7 +123,42 @@ impl Fusion {
         timestamp: f32,
     ) {
         let delta_t = timestamp - self.last_timestamp;
+        self.update_external_heading_by_duration_seconds(gyr, acc, heading, delta_t);
+        self.last_timestamp = timestamp;
+    }
 
+    /// Updates the AHRS algorithm based on gyroscope data in degrees/s, acceleration data in g force and a heading in degrees.
+    ///
+    /// The time is provided using a duration in seconds since the last measurement.
+    /// Note that this won't increase the internal timestamp and using the timestamp version of update functions will produce incorrect results.
+    ///
+    /// # Examples
+    /// ```no_run
+    /// use imu_fusion::{Fusion, FusionVector, FusionAhrsSettings};
+    /// use std::time::{Duration, Instant};
+    ///
+    /// const SAMPLE_RATE_HZ: u32 = 100;
+    ///
+    /// let ahrs_settings = FusionAhrsSettings::new();
+    /// let mut fusion = Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
+    /// let mut last_ts = Instant::now();
+    ///
+    /// loop {
+    ///     let gyr = FusionVector::new(0f32, 0f32, 0f32); // replace this with actual gyroscope data in degrees/s
+    ///     let acc = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual accelerometer data in g
+    ///     let external_heading = 0f32; // replace this with actual heading in degrees
+    ///     fusion.update_external_heading(gyr, acc, external_heading, last_ts.elapsed().as_secs_f32());
+    ///     last_ts = Instant::now();
+    /// }
+    ///
+    /// ```
+    pub fn update_external_heading_by_duration_seconds(
+        &mut self,
+        gyr: FusionVector,
+        acc: FusionVector,
+        heading: f32,
+        delta_t: f32,
+    ) {
         // Apply calibration
         let mut gyr = self.inertial_calibration(gyr, self.gyr_misalignment, self.gyr_sensitivity, self.gyr_offset);
         let acc = self.inertial_calibration(acc, self.acc_misalignment, self.acc_sensitivity, self.acc_offset);
@@ -104,12 +167,11 @@ impl Fusion {
         gyr = self.offset.update(gyr);
 
         self.ahrs.update_external_heading(gyr, acc, heading, delta_t);
-        self.last_timestamp = timestamp;
     }
 
     /// Updates the AHRS algorithm based on gyroscope data in degrees/s, acceleration data in g force and magnetic measurements in degrees.
     ///
-    /// The time is provided using an absolute timestamp in seconds since the last measurement.
+    /// The time is provided using an absolute timestamp in seconds since the first measurement.
     /// Note that if you provide unix timestamps, the precision of f32 will not be enough to correctly compute the time difference.
     ///
     /// # Examples
@@ -139,6 +201,41 @@ impl Fusion {
         timestamp: f32,
     ) {
         let delta_t = timestamp - self.last_timestamp;
+        self.update_by_duration_seconds(gyr, acc, mag, delta_t);
+        self.last_timestamp = timestamp;
+    }
+
+    /// Updates the AHRS algorithm based on gyroscope data in degrees/s, acceleration data in g force and magnetic measurements in degrees.
+    ///
+    /// The time is provided using a duration in seconds since the last measurement.
+    /// Note that this won't increase the internal timestamp and using the timestamp version of update functions will produce incorrect results.
+    /// # Examples
+    /// ```no_run
+    /// use imu_fusion::{Fusion, FusionAhrsSettings, FusionVector};
+    /// use std::time::{Duration, Instant};
+    ///
+    /// const SAMPLE_RATE_HZ: u32 = 100;
+    ///
+    /// let ahrs_settings = FusionAhrsSettings::new();
+    /// let mut fusion = Fusion::new(SAMPLE_RATE_HZ, ahrs_settings);
+    /// let mut last_ts = Instant::now();
+    ///
+    /// loop {
+    ///     let gyr = FusionVector::new(0f32, 0f32, 0f32); // replace this with actual gyroscope data in degrees/s
+    ///     let acc = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual accelerometer data in g
+    ///     let mag = FusionVector::new(0f32, 0f32, 1.0f32); // replace this with actual magnetic measurement in degrees
+    ///     fusion.update_by_duration_seconds(gyr, acc, mag, last_ts.elapsed().as_secs_f32());
+    ///     last_ts = Instant::now();
+    /// }
+    ///
+    /// ```
+    pub fn update_by_duration_seconds(
+        &mut self,
+        gyr: FusionVector,
+        acc: FusionVector,
+        mag: FusionVector,
+        delta_t: f32,
+    ) {
         // Apply calibration
         let mut gyr = self.inertial_calibration(gyr, self.gyr_misalignment, self.gyr_sensitivity, self.gyr_offset);
         let acc = self.inertial_calibration(acc, self.acc_misalignment, self.acc_sensitivity, self.acc_offset);
@@ -148,7 +245,6 @@ impl Fusion {
         gyr = self.offset.update(gyr);
 
         self.ahrs.update(gyr, acc, mag, delta_t);
-        self.last_timestamp = timestamp;
     }
 
     /// Obtain euler angle current sensor position