diff --git a/Cargo.toml b/Cargo.toml index 3f93359..7498529 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -2,6 +2,15 @@ name = "imu-fusion" version = "0.1.0" edition = "2021" +authors = ["Mark Berner "] +description = "A Rust library for fusing IMU data. Based on https://github.com/xioTechnologies/Fusion" +readme = "README.md" +repository = "https://github.com/mark2b/imu-fusion-rs" +license-file = "LICENSE" +keywords = ["imu", "fusion", "accelerometer", "gyroscope", "magnetometer"] +exclude = ["/tests", "/fusion-c"] +include = ["/src", "/examples"] + [features] fusion-use-normal-sqrt = [] diff --git a/src/fusion_ahrs_impl.rs b/src/fusion_ahrs_impl.rs index a7941c6..6d2aa4c 100644 --- a/src/fusion_ahrs_impl.rs +++ b/src/fusion_ahrs_impl.rs @@ -1,5 +1,5 @@ use libm::{atan2f, cosf, fabsf, sinf}; -use crate::{fusion_degrees_to_radians, FusionAhrs, FusionAhrsSettings, FusionConvention, FusionQuaternion, FusionVector, Quaternion}; +use crate::{fusion_degrees_to_radians, FusionAhrs, FusionAhrsSettings, FusionConvention, FusionQuaternion, FusionVector}; use crate::FusionConvention::NWU; /** @@ -73,21 +73,19 @@ impl FusionAhrs { } pub fn update_external_heading(&mut self, gyr: FusionVector, acc: FusionVector, heading: f32, dt: f32) { - unsafe { - let q = self.quaternion.element; - // Calculate roll - let roll = atan2f(q.w * q.x + q.y * q.z, 0.5f32 - q.y * q.y - q.x * q.x); - // Calculate magnetometer// Calculate magnetometer - let heading_radians = fusion_degrees_to_radians(heading); - let sin_heading_radians = sinf(heading_radians); - let magnetometer = FusionVector { - x: cosf(heading_radians), - y: -1.0f32 * cosf(roll) * sin_heading_radians, - z: sin_heading_radians * sinf(roll), - }; - // Update AHRS algorithm - self.update(gyr, acc, magnetometer, dt); - } + let q = self.quaternion; + // Calculate roll + let roll = atan2f(q.w * q.x + q.y * q.z, 0.5f32 - q.y * q.y - q.x * q.x); + // Calculate magnetometer// Calculate magnetometer + let heading_radians = fusion_degrees_to_radians(heading); + let sin_heading_radians = sinf(heading_radians); + let magnetometer = FusionVector { + x: cosf(heading_radians), + y: -1.0f32 * cosf(roll) * sin_heading_radians, + z: sin_heading_radians * sinf(roll), + }; + // Update AHRS algorithm + self.update(gyr, acc, magnetometer, dt); } pub fn update(&mut self, gyr: FusionVector, acc: FusionVector, mag: FusionVector, dt: f32) { @@ -188,20 +186,16 @@ impl FusionAhrs { } fn set_heading(&mut self, heading: f32) { - unsafe { - let q = self.quaternion.element; - let yaw = atan2f(q.w * q.z + q.x * q.y, 0.5f32 - q.y * q.y - q.z * q.z); - let half_yaw_minus_heading = 0.5f32 * (yaw - fusion_degrees_to_radians(heading)); - let rotation = FusionQuaternion { - element: Quaternion { - w: cosf(half_yaw_minus_heading), - x: 0.0f32, - y: 0.0f32, - z: -1.0f32 * sinf(half_yaw_minus_heading), - } - }; - self.quaternion = self.quaternion * rotation; - } + let q = self.quaternion; + let yaw = atan2f(q.w * q.z + q.x * q.y, 0.5f32 - q.y * q.y - q.z * q.z); + let half_yaw_minus_heading = 0.5f32 * (yaw - fusion_degrees_to_radians(heading)); + let rotation = FusionQuaternion { + w: cosf(half_yaw_minus_heading), + x: 0.0f32, + y: 0.0f32, + z: -1.0f32 * sinf(half_yaw_minus_heading), + }; + self.quaternion = self.quaternion * rotation; } pub fn reset(&mut self) { @@ -221,108 +215,100 @@ impl FusionAhrs { } pub fn calculate_half_gravity(&self) -> FusionVector { - unsafe { - let q = self.quaternion.element; - match self.settings.convention { - FusionConvention::ENU | FusionConvention::NWU => { - FusionVector { - x: q.x * q.z - q.w * q.y, - y: q.y * q.z + q.w * q.x, - z: q.w * q.w - 0.5f32 + q.z * q.z, - } + let q = self.quaternion; + match self.settings.convention { + FusionConvention::ENU | FusionConvention::NWU => { + FusionVector { + x: q.x * q.z - q.w * q.y, + y: q.y * q.z + q.w * q.x, + z: q.w * q.w - 0.5f32 + q.z * q.z, } - FusionConvention::NED => { - FusionVector { - x: q.w * q.y - q.x * q.z, - y: -1.0f32 * (q.y * q.z + q.w * q.x), - z: 0.5f32 - q.w * q.w - q.z * q.z, - } + } + FusionConvention::NED => { + FusionVector { + x: q.w * q.y - q.x * q.z, + y: -1.0f32 * (q.y * q.z + q.w * q.x), + z: 0.5f32 - q.w * q.w - q.z * q.z, } } } } pub fn calculate_half_magnetic(&self) -> FusionVector { - unsafe { - let q = self.quaternion.element; - match self.settings.convention { - FusionConvention::NWU => { - let half_magnetic = FusionVector { - x: q.x * q.y + q.w * q.z, - y: q.w * q.w - 0.5f32 + q.y * q.y, - z: q.y * q.z - q.w * q.x, - }; - half_magnetic - } - FusionConvention::ENU => { - let half_magnetic = FusionVector { - x: 0.5f32 - q.w * q.w - q.x * q.x, - y: q.w * q.z - q.x * q.y, - z: -1.0f32 * (q.x * q.z + q.w * q.y), - }; - half_magnetic - } - FusionConvention::NED => { - let half_magnetic = FusionVector { - x: -1.0f32 * (q.x * q.y + q.w * q.z), - y: 0.5f32 - q.w * q.w - q.y * q.y, - z: q.w * q.x - q.y * q.z, - }; - half_magnetic - } + let q = self.quaternion; + match self.settings.convention { + FusionConvention::NWU => { + let half_magnetic = FusionVector { + x: q.x * q.y + q.w * q.z, + y: q.w * q.w - 0.5f32 + q.y * q.y, + z: q.y * q.z - q.w * q.x, + }; + half_magnetic + } + FusionConvention::ENU => { + let half_magnetic = FusionVector { + x: 0.5f32 - q.w * q.w - q.x * q.x, + y: q.w * q.z - q.x * q.y, + z: -1.0f32 * (q.x * q.z + q.w * q.y), + }; + half_magnetic + } + FusionConvention::NED => { + let half_magnetic = FusionVector { + x: -1.0f32 * (q.x * q.y + q.w * q.z), + y: 0.5f32 - q.w * q.w - q.y * q.y, + z: q.w * q.x - q.y * q.z, + }; + half_magnetic } } } pub fn earth_acc(&self) -> FusionVector { - unsafe { - // Calculate accelerometer measurement in the Earth coordinate frame - let q = self.quaternion.element; - let a = self.acc; - let qwqw = q.w * q.w; // calculate common terms to avoid repeated operations - let qwqx = q.w * q.x; - let qwqy = q.w * q.y; - let qwqz = q.w * q.z; - let qxqy = q.x * q.y; - let qxqz = q.x * q.z; - let qyqz = q.y * q.z; - let mut accelerometer = FusionVector { - x: 2.0f32 * ((qwqw - 0.5f32 + q.x * q.x) * a.x + (qxqy - qwqz) * a.y + (qxqz + qwqy) * a.z), - y: 2.0f32 * ((qxqy + qwqz) * a.x + (qwqw - 0.5f32 + q.y * q.y) * a.y + (qyqz - qwqx) * a.z), - z: 2.0f32 * ((qxqz - qwqy) * a.x + (qyqz + qwqx) * a.y + (qwqw - 0.5f32 + q.z * q.z) * a.z), - }; - // Remove gravity from accelerometer measurement - match self.settings.convention { - FusionConvention::ENU | FusionConvention::NWU => { - accelerometer.z -= 1.0f32; - accelerometer - } - FusionConvention::NED => { - accelerometer.z += 1.0f32; - accelerometer - } + // Calculate accelerometer measurement in the Earth coordinate frame + let q = self.quaternion; + let a = self.acc; + let qwqw = q.w * q.w; // calculate common terms to avoid repeated operations + let qwqx = q.w * q.x; + let qwqy = q.w * q.y; + let qwqz = q.w * q.z; + let qxqy = q.x * q.y; + let qxqz = q.x * q.z; + let qyqz = q.y * q.z; + let mut accelerometer = FusionVector { + x: 2.0f32 * ((qwqw - 0.5f32 + q.x * q.x) * a.x + (qxqy - qwqz) * a.y + (qxqz + qwqy) * a.z), + y: 2.0f32 * ((qxqy + qwqz) * a.x + (qwqw - 0.5f32 + q.y * q.y) * a.y + (qyqz - qwqx) * a.z), + z: 2.0f32 * ((qxqz - qwqy) * a.x + (qyqz + qwqx) * a.y + (qwqw - 0.5f32 + q.z * q.z) * a.z), + }; + // Remove gravity from accelerometer measurement + match self.settings.convention { + FusionConvention::ENU | FusionConvention::NWU => { + accelerometer.z -= 1.0f32; + accelerometer + } + FusionConvention::NED => { + accelerometer.z += 1.0f32; + accelerometer } } } pub fn linear_acc(&self) -> FusionVector { - unsafe { - // Calculate accelerometer measurement in the Earth coordinate frame - let q = self.quaternion.element; - // Calculate gravity in the sensor coordinate frame - let gravity = FusionVector { - x: 2.0f32 * (q.x * q.z - q.w * q.y), - y: 2.0f32 * (q.y * q.z + q.w * q.x), - z: 2.0f32 * (q.w * q.w - 0.5f32 + q.z * q.z), - }; + // Calculate accelerometer measurement in the Earth coordinate frame + let q = self.quaternion; + // Calculate gravity in the sensor coordinate frame + let gravity = FusionVector { + x: 2.0f32 * (q.x * q.z - q.w * q.y), + y: 2.0f32 * (q.y * q.z + q.w * q.x), + z: 2.0f32 * (q.w * q.w - 0.5f32 + q.z * q.z), + }; - // Remove gravity from accelerometer measurement - match self.settings.convention { - FusionConvention::ENU | FusionConvention::NWU => { - self.acc - gravity - } - FusionConvention::NED => { - self.acc + gravity - } + // Remove gravity from accelerometer measurement + match self.settings.convention { + FusionConvention::ENU | FusionConvention::NWU => { + self.acc - gravity + } + FusionConvention::NED => { + self.acc + gravity } } } diff --git a/src/fusion_matrix_impl.rs b/src/fusion_matrix_impl.rs index df3fbd7..02a4722 100644 --- a/src/fusion_matrix_impl.rs +++ b/src/fusion_matrix_impl.rs @@ -47,28 +47,26 @@ impl ops::Mul for FusionMatrix { impl From for FusionMatrix { fn from(q: FusionQuaternion) -> Self { - unsafe { - let qwqw = q.element.w * q.element.w; - let qwqx = q.element.w * q.element.x; - let qwqy = q.element.w * q.element.y; - let qwqz = q.element.w * q.element.z; - let qxqx = q.element.x * q.element.x; - let qxqy = q.element.x * q.element.y; - let qxqz = q.element.x * q.element.z; - let qyqy = q.element.y * q.element.y; - let qyqz = q.element.y * q.element.z; - let qzqz = q.element.z * q.element.z; - Self { - xx: 2.0f32 * (qwqw - 0.5f32 + qxqx), - xy: 2.0f32 * (qxqy - qwqz), - xz: 2.0f32 * (qxqz + qwqy), - yx: 2.0f32 * (qxqy + qwqz), - yy: 2.0f32 * (qwqw - 0.5f32 + qyqy), - yz: 2.0f32 * (qyqz - qwqx), - zx: 2.0f32 * (qxqz - qwqy), - zy: 2.0f32 * (qyqz + qwqx), - zz: 2.0f32 * (qwqw - 0.5f32 + qzqz), - } + let qwqw = q.w * q.w; + let qwqx = q.w * q.x; + let qwqy = q.w * q.y; + let qwqz = q.w * q.z; + let qxqx = q.x * q.x; + let qxqy = q.x * q.y; + let qxqz = q.x * q.z; + let qyqy = q.y * q.y; + let qyqz = q.y * q.z; + let qzqz = q.z * q.z; + Self { + xx: 2.0f32 * (qwqw - 0.5f32 + qxqx), + xy: 2.0f32 * (qxqy - qwqz), + xz: 2.0f32 * (qxqz + qwqy), + yx: 2.0f32 * (qxqy + qwqz), + yy: 2.0f32 * (qwqw - 0.5f32 + qyqy), + yz: 2.0f32 * (qyqz - qwqx), + zx: 2.0f32 * (qxqz - qwqy), + zy: 2.0f32 * (qyqz + qwqx), + zz: 2.0f32 * (qwqw - 0.5f32 + qzqz), } } } diff --git a/src/fusion_quaternion_impl.rs b/src/fusion_quaternion_impl.rs index cda00bc..f4f4662 100644 --- a/src/fusion_quaternion_impl.rs +++ b/src/fusion_quaternion_impl.rs @@ -1,73 +1,71 @@ use core::ops; #[allow(unused_imports)] use libm::{asinf, atan2f, sqrtf}; -use crate::{Angle, asin_safe, fusion_fast_inverse_sqrt, fusion_radians_to_degrees, FusionEuler, FusionMatrix, FusionQuaternion, FusionVector, Quaternion}; +use crate::{Angle, asin_safe, fusion_fast_inverse_sqrt, fusion_radians_to_degrees, FusionEuler, FusionMatrix, FusionQuaternion, FusionVector}; impl FusionQuaternion { pub fn identity() -> Self { const VALUE: FusionQuaternion = FusionQuaternion { - data: [1.0f32, 0.0f32, 0.0f32, 0.0f32], + w: 1.0f32, + x: 0.0f32, + y: 0.0f32, + z: 0.0f32, }; VALUE } pub fn normalize(&self) -> Self { - unsafe { - #[cfg(feature = "fusion-use-normal-sqrt")] - { - *self * 1.0f32 / sqrtf(self.element.w * self.element.w + self.element.x * self.element.x + self.element.y * self.element.y + self.element.z * self.element.z) - } - #[cfg(not(feature = "fusion-use-normal-sqrt"))] - { - *self * fusion_fast_inverse_sqrt(self.element.w * self.element.w + self.element.x * self.element.x + self.element.y * self.element.y + self.element.z * self.element.z) - } + #[cfg(feature = "fusion-use-normal-sqrt")] + { + *self * 1.0f32 / sqrtf(self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z) + } + #[cfg(not(feature = "fusion-use-normal-sqrt"))] + { + *self * fusion_fast_inverse_sqrt(self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z) } } pub fn euler(self) -> FusionEuler { - unsafe { - // calculate common terms to avoid repeated operations - let q = self.element; - let half_minus_qy_squared = 0.5f32 - q.y * q.y; - let roll = fusion_radians_to_degrees(atan2f(q.w * q.x + q.y * q.z, half_minus_qy_squared - q.x * q.x)); - let pitch = fusion_radians_to_degrees(asin_safe(2.0f32 * (q.w * q.y - q.z * q.x))); - let yaw = fusion_radians_to_degrees(atan2f(q.w * q.z + q.x * q.y, half_minus_qy_squared - q.z * q.z)); + // calculate common terms to avoid repeated operations + let q = self; + let half_minus_qy_squared = 0.5f32 - q.y * q.y; + let roll = fusion_radians_to_degrees(atan2f(q.w * q.x + q.y * q.z, half_minus_qy_squared - q.x * q.x)); + let pitch = fusion_radians_to_degrees(asin_safe(2.0f32 * (q.w * q.y - q.z * q.x))); + let yaw = fusion_radians_to_degrees(atan2f(q.w * q.z + q.x * q.y, half_minus_qy_squared - q.z * q.z)); - FusionEuler { - angle: Angle { - roll, - pitch, - yaw, - } + FusionEuler { + angle: Angle { + roll, + pitch, + yaw, } } } pub fn rotation(self) -> FusionMatrix { - unsafe { - // calculate common terms to avoid repeated operations - let qwqw = self.element.w * self.element.w; - let qwqx = self.element.w * self.element.x; - let qwqy = self.element.w * self.element.y; - let qwqz = self.element.w * self.element.z; - let qxqx = self.element.x * self.element.x; - let qxqy = self.element.x * self.element.y; - let qxqz = self.element.x * self.element.z; - let qyqy = self.element.y * self.element.y; - let qyqz = self.element.y * self.element.z; - let qzqz = self.element.z * self.element.z; - FusionMatrix { - xx: 2.0f32 * (qwqw - 0.5f32 + qxqx), - xy: 2.0f32 * (qxqy - qwqz), - xz: 2.0f32 * (qxqz + qwqy), - yx: 2.0f32 * (qxqy + qwqz), - yy: 2.0f32 * (qwqw - 0.5f32 + qyqy), - yz: 2.0f32 * (qyqz - qwqx), - zx: 2.0f32 * (qxqz - qwqy), - zy: 2.0f32 * (qyqz + qwqx), - zz: 2.0f32 * (qwqw - 0.5f32 + qzqz), - } + // calculate common terms to avoid repeated operations + let qwqw = self.w * self.w; + let qwqx = self.w * self.x; + let qwqy = self.w * self.y; + let qwqz = self.w * self.z; + let qxqx = self.x * self.x; + let qxqy = self.x * self.y; + let qxqz = self.x * self.z; + let qyqy = self.y * self.y; + let qyqz = self.y * self.z; + let qzqz = self.z * self.z; + FusionMatrix { + xx: 2.0f32 * (qwqw - 0.5f32 + qxqx), + xy: 2.0f32 * (qxqy - qwqz), + xz: 2.0f32 * (qxqz + qwqy), + yx: 2.0f32 * (qxqy + qwqz), + yy: 2.0f32 * (qwqw - 0.5f32 + qyqy), + yz: 2.0f32 * (qyqz - qwqx), + zx: 2.0f32 * (qxqz - qwqy), + zy: 2.0f32 * (qyqz + qwqx), + zz: 2.0f32 * (qwqw - 0.5f32 + qzqz), + } } } @@ -76,15 +74,11 @@ impl ops::Add for FusionQuaternion { type Output = Self; fn add(self, rhs: Self) -> Self::Output { - unsafe { - Self { - element: Quaternion { - w: self.element.w + rhs.element.w, - x: self.element.x + rhs.element.x, - y: self.element.y + rhs.element.y, - z: self.element.z + rhs.element.z, - } - } + Self { + w: self.w + rhs.w, + x: self.x + rhs.x, + y: self.y + rhs.y, + z: self.z + rhs.z, } } } @@ -93,15 +87,11 @@ impl ops::Mul for FusionQuaternion { type Output = Self; fn mul(self, rhs: Self) -> Self::Output { - unsafe { - Self { - element: Quaternion { - w: self.element.w * rhs.element.w - self.element.x * rhs.element.x - self.element.y * rhs.element.y - self.element.z * rhs.element.z, - x: self.element.w * rhs.element.x + self.element.x * rhs.element.w + self.element.y * rhs.element.z - self.element.z * rhs.element.y, - y: self.element.w * rhs.element.y - self.element.x * rhs.element.z + self.element.y * rhs.element.w + self.element.z * rhs.element.x, - z: self.element.w * rhs.element.z + self.element.x * rhs.element.y - self.element.y * rhs.element.x + self.element.z * rhs.element.w, - } - } + Self { + w: self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z, + x: self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y, + y: self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x, + z: self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w, } } } @@ -109,15 +99,11 @@ impl ops::Mul for FusionQuaternion { impl ops::Mul for FusionQuaternion { type Output = Self; fn mul(self, rhs: FusionVector) -> Self::Output { - unsafe { - Self { - element: Quaternion { - w: -self.element.x * rhs.x - self.element.y * rhs.y - self.element.z * rhs.z, - x: self.element.w * rhs.x + self.element.y * rhs.z - self.element.z * rhs.y, - y: self.element.w * rhs.y - self.element.x * rhs.z + self.element.z * rhs.x, - z: self.element.w * rhs.z + self.element.x * rhs.y - self.element.y * rhs.x, - } - } + Self { + w: -self.x * rhs.x - self.y * rhs.y - self.z * rhs.z, + x: self.w * rhs.x + self.y * rhs.z - self.z * rhs.y, + y: self.w * rhs.y - self.x * rhs.z + self.z * rhs.x, + z: self.w * rhs.z + self.x * rhs.y - self.y * rhs.x, } } } @@ -125,15 +111,11 @@ impl ops::Mul for FusionQuaternion { impl ops::Mul for FusionQuaternion { type Output = Self; fn mul(self, rhs: f32) -> Self::Output { - unsafe { - Self { - element: Quaternion { - w: self.element.w * rhs, - x: self.element.x * rhs, - y: self.element.y * rhs, - z: self.element.z * rhs, - } - } + Self { + w: self.w * rhs, + x: self.x * rhs, + y: self.y * rhs, + z: self.z * rhs, } } } diff --git a/src/lib.rs b/src/lib.rs index 9fa1fa7..52a2283 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -14,7 +14,7 @@ pub enum FusionConvention { NWU, /* East-North-Up */ ENU, - /* East-North-Up */ + /* North-East-Down */ NED, } @@ -59,14 +59,6 @@ pub struct FusionAhrsSettings { pub recovery_trigger_period: i32, } -#[derive(Copy, Clone)] -pub struct Quaternion { - pub w: f32, - pub x: f32, - pub y: f32, - pub z: f32, -} - #[derive(Copy, Clone)] pub struct Angle { pub roll: f32, @@ -83,7 +75,6 @@ pub struct FusionVector { } #[derive(Copy, Clone)] -#[allow(dead_code)] pub struct FusionMatrix { pub xx: f32, pub xy: f32, @@ -97,10 +88,11 @@ pub struct FusionMatrix { } #[derive(Copy, Clone)] -#[allow(dead_code)] -pub union FusionQuaternion { - pub data: [f32; 4], - pub element: Quaternion, +pub struct FusionQuaternion { + pub w: f32, + pub x: f32, + pub y: f32, + pub z: f32, } #[derive(Copy, Clone)] diff --git a/tests/fusion-rs/test_1.rs b/tests/fusion-rs/test_1.rs index 18e090b..c890bb1 100644 --- a/tests/fusion-rs/test_1.rs +++ b/tests/fusion-rs/test_1.rs @@ -28,19 +28,18 @@ mod tests { let euler = fusion.euler(); let earth_acc = fusion.earth_acc(); earth_acc.get(&mut acc_x, &mut acc_y, &mut acc_z); - unsafe { - writer.write_record(&[format!("{:.8}", dt), format!("{:.8}", euler.angle.yaw), format!("{:.8}", euler.angle.pitch), format!("{:.8}", euler.angle.roll), format!("{:.8}", acc_x), format!("{:.8}", acc_y), format!("{:.8}", acc_z), format!("{:.8}", q.data[0]), format!("{:.8}", q.data[1]), format!("{:.8}", q.data[2]), format!("{:.8}", q.data[3])]).unwrap(); - } + writer.write_record(&[format!("{:.8}", dt), format!("{:.8}", euler.angle.yaw), format!("{:.8}", euler.angle.pitch), format!("{:.8}", euler.angle.roll), format!("{:.8}", acc_x), format!("{:.8}", acc_y), format!("{:.8}", acc_z), format!("{:.8}", q.w), format!("{:.8}", q.x), format!("{:.8}", q.y), format!("{:.8}", q.z)]).unwrap(); } _ = writer.flush(); compare_results(); } + fn compare_results() { let mut reader_c = csv::Reader::from_path("tests/fusion_c_out.csv").unwrap(); let mut reader_rs = csv::Reader::from_path("tests/fusion_rs_out.csv").unwrap(); loop { - let record_c = reader_c.deserialize::<[f32;3]>().next(); - let record_rs = reader_rs.deserialize::<[f32;3]>().next(); + let record_c = reader_c.deserialize::<[f32; 3]>().next(); + let record_rs = reader_rs.deserialize::<[f32; 3]>().next(); match record_c { Some(Ok(record_c)) => { match record_rs {