add UBX-CFG-ESFALG msg

 - refactor EsfStatus msg field names
 - bump ublox version to differentiate from upstream

Signed-off-by: Andrei Gherghescu <[email protected]>

examples/adr-message-parsing/Cargo.toml

@@ -4,7 +4,7 @@ authors = [
     "Andrei Gherghescu <[email protected]>",
 ]
 edition = "2021"
-name = "example-basic-cli"
+name = "adr-status-messages"
 publish = false
 version = "0.1.0"
 

examples/adr-message-parsing/src/main.rs

@@ -121,6 +121,13 @@ fn main() {
         )
         .expect("Could not configure ports for UBX-ESF-INS");
 
+    device
+        .write_all(
+            &CfgMsgAllPortsBuilder::set_rate_for::<CfgEsfAlg>([0, 1, 0, 1, 0, 0])
+                .into_packet_bytes(),
+        )
+        .expect("Could not configure ports for UBX-CFG-ESFALG");
+
     device
         .write_all(
             &CfgMsgAllPortsBuilder::set_rate_for::<EsfAlg>([0, 1, 0, 1, 0, 0]).into_packet_bytes(),
@@ -151,6 +158,26 @@ fn main() {
         .write_all(&UbxPacketRequest::request_for::<MonVer>().into_packet_bytes())
         .expect("Unable to write request/poll for UBX-MON-VER message");
 
+    // Configure Auto Alignment on for IMU
+    let mut flags = CfgEsfAlgFlags::default();
+    flags.set_auto_imu_mount_alg(true);
+    device
+        .write_all(
+            &ublox::CfgEsfAlgBuilder {
+                flags,
+                yaw: 5.0,
+                roll: 1.0,
+                pitch: 2.0,
+            }
+            .into_packet_bytes(),
+        )
+        .expect("Could not write UBX-CFG-ESFALG msg due to: {e}");
+
+    // Send a packet request for the MonVer packet
+    device
+        .write_all(&UbxPacketRequest::request_for::<CfgEsfAlg>().into_packet_bytes())
+        .expect("Unable to write request/poll for CFG-ESFALG message");
+
     // Start reading data
     println!("Opened uBlox device, waiting for messages...");
     loop {

examples/send-receive-multithread/Cargo.toml

@@ -1,7 +1,7 @@
 [package]
 authors = ["Andrei Gherghescu <[email protected]>"]
 edition = "2021"
-name = "example-send-receive-multiple-threads"
+name = "send-receive-multiple-threads"
 publish = false
 version = "0.1.0"
 

ublox/Cargo.toml

@@ -7,7 +7,7 @@ name = "ublox"
 publish = false
 readme = "../README.md"
 repository = "https://github.com/lkolbly/ublox"
-version = "0.4.5"
+version = "0.5.0"
 
 [features]
 alloc = []
@@ -21,7 +21,7 @@ num-traits = { version = "0.2", default-features = false }
 serde = { version = "1.0", optional = true, default-features = false, features = [
     "derive",
 ] }
-ublox_derive = { path = "../ublox_derive", version = "=0.1.0" }
+ublox_derive = { path = "../ublox_derive", version = "=0.5.0" }
 
 [dev-dependencies]
 cpu-time = "1.0"

ublox/src/ubx_packets/packets.rs

@@ -2598,6 +2598,79 @@ struct MgaAck {
     msg_payload_start: [u8; 4],
 }
 
+/// Get/set IMU-mount misalignment configuration
+/// Only available for ADR products
+#[ubx_packet_recv_send]
+#[ubx(
+    class = 0x06,
+    id = 0x56,
+    fixed_payload_len = 12,
+    flags = "default_for_builder"
+)]
+struct CfgEsfAlg {
+    #[ubx(map_type = CfgEsfAlgFlags)]
+    flags: u32,
+
+    /// IMU mount yaw angle [0, 360]
+    #[ubx(map_type = f64, scale = 1e-2, alias = yaw)]
+    yaw: u32,
+
+    /// IMU mount pitch angle [-90, 90]
+    #[ubx(map_type = f64, scale = 1e-2, alias = pitch)]
+    pitch: i16,
+
+    /// IMU mount roll angle [-180, 180]
+    #[ubx(map_type = f64, scale = 1e-2, alias = roll)]
+    roll: i16,
+}
+
+#[derive(Copy, Clone, Default)]
+#[cfg_attr(feature = "serde", derive(serde::Serialize))]
+pub struct CfgEsfAlgFlags {
+    /// Not writable, only readable
+    version: u8,
+    /// Auto alignment status
+    auto_alignment: bool,
+}
+
+impl From<u32> for CfgEsfAlgFlags {
+    fn from(cfg: u32) -> Self {
+        let version = cfg.to_ne_bytes()[0];
+        let auto_alignment = ((cfg >> 8) & 0x01) == 1;
+        Self {
+            version,
+            auto_alignment,
+        }
+    }
+}
+
+impl CfgEsfAlgFlags {
+    pub fn auto_imu_mount_alg_on(self) -> bool {
+        self.auto_alignment
+    }
+
+    pub fn set_auto_imu_mount_alg(&mut self, enable: bool) {
+        self.auto_alignment = enable
+    }
+
+    pub fn version(self) -> u8 {
+        self.version
+    }
+
+    const fn into_raw(self) -> u32 {
+        (self.auto_alignment as u32) << 8
+    }
+}
+
+impl fmt::Debug for CfgEsfAlgFlags {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        f.debug_struct("flags")
+            .field("autoMntAlgOn", &self.auto_imu_mount_alg_on())
+            .field("version", &self.version())
+            .finish()
+    }
+}
+
 #[ubx_extend]
 #[ubx(from, rest_reserved)]
 #[repr(u8)]
@@ -3505,9 +3578,9 @@ struct EsfStatus {
     num_sens: u8,
 
     #[ubx(
-        map_type = EsfStatusDataIter,
-        from = EsfStatusDataIter::new,
-        is_valid = EsfStatusDataIter::is_valid,
+        map_type = EsfSensorStatusIter,
+        from = EsfSensorStatusIter::new,
+        is_valid = EsfSensorStatusIter::is_valid,
         may_fail,
     )]
     data: [u8; 0],
@@ -3639,17 +3712,17 @@ pub enum EsfImuInitStatus {
 
 #[derive(Debug)]
 #[cfg_attr(feature = "serde", derive(serde::Serialize))]
-pub struct EsfStatusData {
-    pub sens_status1: EsfStatusSensor1,
-    pub sens_status2: EsfStatusSensor2,
+pub struct EsfSensorStatus {
+    pub sens_status1: SensorStatus1,
+    pub sens_status2: SensorStatus2,
     pub freq: u8,
-    pub faults: EsfStatusFaults,
+    pub faults: SensorFaults,
 }
 
 #[derive(Clone, Debug)]
-pub struct EsfStatusDataIter<'a>(core::slice::ChunksExact<'a, u8>);
+pub struct EsfSensorStatusIter<'a>(core::slice::ChunksExact<'a, u8>);
 
-impl<'a> EsfStatusDataIter<'a> {
+impl<'a> EsfSensorStatusIter<'a> {
     fn new(bytes: &'a [u8]) -> Self {
         Self(bytes.chunks_exact(4))
     }
@@ -3659,13 +3732,13 @@ impl<'a> EsfStatusDataIter<'a> {
     }
 }
 
-impl core::iter::Iterator for EsfStatusDataIter<'_> {
-    type Item = EsfStatusData;
+impl core::iter::Iterator for EsfSensorStatusIter<'_> {
+    type Item = EsfSensorStatus;
 
     fn next(&mut self) -> Option<Self::Item> {
         let chunk = self.0.next()?;
         let data = u32::from_le_bytes(chunk[0..4].try_into().unwrap());
-        Some(EsfStatusData {
+        Some(EsfSensorStatus {
             sens_status1: ((data & 0xFF) as u8).into(),
             sens_status2: (((data >> 8) & 0xFF) as u8).into(),
             freq: ((data >> 16) & 0xFF).try_into().unwrap(),
@@ -3676,13 +3749,13 @@ impl core::iter::Iterator for EsfStatusDataIter<'_> {
 
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde", derive(serde::Serialize))]
-pub struct EsfStatusSensor1 {
+pub struct SensorStatus1 {
     pub sensor_type: EsfSensorType,
     pub used: bool,
     pub ready: bool,
 }
 
-impl From<u8> for EsfStatusSensor1 {
+impl From<u8> for SensorStatus1 {
     fn from(s: u8) -> Self {
         let sensor_type: EsfSensorType = (s & 0x3F).into();
         Self {
@@ -3739,15 +3812,15 @@ impl From<u8> for EsfSensorType {
 #[allow(dead_code)]
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde", derive(serde::Serialize))]
-pub struct EsfStatusSensor2 {
-    calibration_status: EsfStatusSensorCalibration,
-    time_status: EsfStatusSensorTime,
+pub struct SensorStatus2 {
+    calibration_status: SensorStatusCalibration,
+    time_status: SensorStatusTime,
 }
 
-impl From<u8> for EsfStatusSensor2 {
+impl From<u8> for SensorStatus2 {
     fn from(s: u8) -> Self {
-        let calibration_status: EsfStatusSensorCalibration = (s & 0x03).into();
-        let time_status: EsfStatusSensorTime = ((s >> 2) & 0x03).into();
+        let calibration_status: SensorStatusCalibration = (s & 0x03).into();
+        let time_status: SensorStatusTime = ((s >> 2) & 0x03).into();
         Self {
             calibration_status,
             time_status,
@@ -3757,54 +3830,54 @@ impl From<u8> for EsfStatusSensor2 {
 
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde", derive(serde::Serialize))]
-pub enum EsfStatusSensorCalibration {
+pub enum SensorStatusCalibration {
     NotCalibrated = 0,
     Calibrating = 1,
     Calibrated = 2,
 }
 
-impl From<u8> for EsfStatusSensorCalibration {
+impl From<u8> for SensorStatusCalibration {
     fn from(orig: u8) -> Self {
         match orig {
-            0 => EsfStatusSensorCalibration::NotCalibrated,
-            1 => EsfStatusSensorCalibration::Calibrating,
-            2 => EsfStatusSensorCalibration::Calibrated,
-            _ => EsfStatusSensorCalibration::Calibrated,
+            0 => SensorStatusCalibration::NotCalibrated,
+            1 => SensorStatusCalibration::Calibrating,
+            2 => SensorStatusCalibration::Calibrated,
+            _ => SensorStatusCalibration::Calibrated,
         }
     }
 }
 
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde", derive(serde::Serialize))]
-pub enum EsfStatusSensorTime {
+pub enum SensorStatusTime {
     NoData = 0,
     OnReceptionFirstByte = 1,
     OnEventInput = 2,
     TimeTagFromData = 3,
 }
 
-impl From<u8> for EsfStatusSensorTime {
+impl From<u8> for SensorStatusTime {
     fn from(orig: u8) -> Self {
         match orig {
-            0 => EsfStatusSensorTime::NoData,
-            1 => EsfStatusSensorTime::OnReceptionFirstByte,
-            2 => EsfStatusSensorTime::OnEventInput,
-            3 => EsfStatusSensorTime::TimeTagFromData,
-            _ => EsfStatusSensorTime::NoData,
+            0 => SensorStatusTime::NoData,
+            1 => SensorStatusTime::OnReceptionFirstByte,
+            2 => SensorStatusTime::OnEventInput,
+            3 => SensorStatusTime::TimeTagFromData,
+            _ => SensorStatusTime::NoData,
         }
     }
 }
 
 #[derive(Debug)]
 #[cfg_attr(feature = "serde", derive(serde::Serialize))]
-pub struct EsfStatusFaults {
+pub struct SensorFaults {
     pub bad_measurement: bool,
     pub bad_timetag: bool,
     pub missing_measurement: bool,
     pub noisy_measurement: bool,
 }
 
-impl From<u8> for EsfStatusFaults {
+impl From<u8> for SensorFaults {
     fn from(s: u8) -> Self {
         Self {
             bad_measurement: (s & 0x01) != 0,
@@ -4213,6 +4286,7 @@ define_recv_packets!(
         CfgTmode2,
         CfgTmode3,
         CfgTp5,
+        CfgEsfAlg,
         InfError,
         InfWarning,
         InfNotice,

ublox_derive/Cargo.toml

@@ -5,7 +5,7 @@ edition = "2021"
 license = "MIT"
 name = "ublox_derive"
 publish = false
-version = "0.1.0"
+version = "0.5.0"
 
 [lib]
 proc-macro = true