diff --git a/examples/simple_by_duration.rs b/examples/simple_by_duration.rs new file mode 100644 index 0000000..7ed548f --- /dev/null +++ b/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(); + } +} \ No newline at end of file diff --git a/src/fusion_impl.rs b/src/fusion_impl.rs index f8f151a..58095df 100644 --- a/src/fusion_impl.rs +++ b/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