[ADD] Carrier Frequency Offset estimation

src/configs.rs

@@ -264,6 +264,14 @@ impl UwbChannel {
             UwbChannel::Channel9 => 0x0002AFB5,
         }
     }
+
+    /// Get center frequency in hertz
+    pub fn get_center_frequency(&self) -> u64 {
+        match self {
+            UwbChannel::Channel5 => 6489600000,
+            UwbChannel::Channel9 => 7987200000,
+        }
+    }
 }
 
 #[derive(Copy, Clone, Debug, Eq, PartialEq, Default)]

src/hl/carrier_freq_offset.rs

@@ -0,0 +1,49 @@
+use crate::hl::receiving::CarrierRecoveryIntegrator;
+use defmt::Format;
+
+/// A struct representing the carrier frequency offset of the received message.
+#[cfg_attr(feature = "defmt", derive(Format))]
+#[derive(Debug, serde::Serialize, serde::Deserialize)]
+pub struct CarrierFreqOffset {
+    /// Contains the carrier frequency offset in Hertz, as per
+    /// DW3000 user manual 8.2.7.6.
+    pub f_offset_hz: f64,
+
+    /// Contains the carrier frequency offset in PPM, giving similar
+    /// results as the `dwt_readclockoffset` function of the DWM3000 SDK.
+    /// NOTE: Positive value means that the local receiver’s clock is running
+    /// faster than that of the remote transmitter.
+    pub f_offset_ppm: f64,
+}
+
+impl CarrierFreqOffset {
+    /// This constructor calculates the carrier frequency offset from a CarrierRecoveryIntegrator and the center frequency of the channel
+    pub fn from_drx_car_int(
+        carrier_integrator: CarrierRecoveryIntegrator,
+        f_c: u64,
+    ) -> CarrierFreqOffset {
+        // Convert it to a f64
+        let drx_car_int: f64 = carrier_integrator.value() as f64;
+
+        // F_S / 2 / N_samples / 2^17   (N_samples = 1024 for DW3000)
+        const DW_FREQ_OFFSET_MULTIPLIER: f64 = 998.4e6 / 2.0 / 1024.0 / 131072.0;
+
+        // Compute f_offset in hertz
+        let f_offset_hz = drx_car_int * DW_FREQ_OFFSET_MULTIPLIER;
+
+        CarrierFreqOffset::from_f_offset_hz(f_offset_hz, f_c)
+    }
+
+    /// This constructor calculates the carrier frequency offset in ppm from a carrier_frequency_offset in Hertz and the center frequency of the channel
+    /// NOTE: The output has the same sign as the `dwt_readclockoffset` function of the DWM3000 SDK, and not as the computation in DW3000 user manual 8.2.7.6.
+    /// The computation follows from DW3000 user manual 8.2.7.6, without the negative sign.
+    pub fn from_f_offset_hz(f_offset_hz: f64, f_c: u64) -> CarrierFreqOffset {
+        // Compute f_offset in ppm w.r.t to channel frequency
+        let f_offset_ppm = 1e6 * f_offset_hz / (f_c as f64);
+
+        CarrierFreqOffset {
+            f_offset_hz,
+            f_offset_ppm,
+        }
+    }
+}

src/hl/mod.rs

@@ -24,6 +24,8 @@ pub use sleeping::*;
 pub use state_impls::*;
 #[allow(unused_imports)]
 pub use uninitialized::*;
+#[allow(unused_imports)]
+pub use carrier_freq_offset::*;
 
 use crate::ll;
 
@@ -35,6 +37,7 @@ mod sending;
 mod sleeping;
 mod state_impls;
 mod uninitialized;
+mod carrier_freq_offset;
 
 /// Entry point to the DW3000 driver API
 #[derive(Copy, Clone)]

src/hl/receiving.rs

@@ -33,6 +33,9 @@ pub struct Message<'l> {
     /// quality of the message received
     pub rx_quality: RxQuality,
 
+    /// carrier recovery integrator of the message received
+    pub carrier_integrator: CarrierRecoveryIntegrator,
+
     /// The MAC frame
     pub frame: Ieee802154Frame<&'l [u8]>,
 }
@@ -58,6 +61,32 @@ pub struct RxQuality {
     pub rssi: f32,
 }
 
+/// A struct representing the carrier recovery integrator of the received message.
+#[cfg_attr(feature = "defmt", derive(Format))]
+#[derive(Clone, Copy, Debug, serde::Deserialize, serde::Serialize)]
+#[repr(C)]
+pub struct CarrierRecoveryIntegrator(i32);
+
+impl CarrierRecoveryIntegrator {
+    /// Creates a new instance of `CarrierRecoveryIntegrator`
+    /// The value is represented as a signed fixed point, with the 17 LSBs being the
+    /// fractional part of the value
+    pub fn new(drx_car_int: u32) -> Self {
+        Self(((drx_car_int << 11) as i32) >> 11)
+    }
+
+    /// Wraps an i32 into a carrier recovery integrator value
+    pub fn from(carrier_integrator: i32) -> Self {
+        Self(carrier_integrator)
+    }
+
+    /// Returns the raw i32 drx_car_int
+    pub fn value(&self) -> i32 {
+        self.0
+    }
+}
+
+
 impl<SPI, RECEIVING> DW3000<SPI, RECEIVING>
 where
     SPI: spi::SpiDevice<u8>,
@@ -172,6 +201,16 @@ where
             rssi,
         };
 
+        // read the carrier recovery integrator from the dw3000
+        use crate::ll::drx_car_int;
+        let carrier_integrator = CarrierRecoveryIntegrator::new(
+            self.ll()
+                .drx_car_int()
+                .read()
+                .map_err(|error| nb::Error::Other(Error::Spi(error)))?
+                .value(),
+        );
+
         //  Reset status bits. This is not strictly necessary, but it helps, if
         // you have to inspect SYS_STATUS manually during debugging.
         self.ll()
@@ -226,6 +265,7 @@ where
         Ok(Message {
             rx_time,
             rx_quality,
+            carrier_integrator,
             frame,
         })
     }
@@ -245,7 +285,7 @@ where
     pub fn r_wait_buf(
         &mut self,
         buffer: &mut [u8],
-    ) -> nb::Result<(usize, Instant, RxQuality), Error<SPI>> {
+    ) -> nb::Result<(usize, Instant, RxQuality, CarrierRecoveryIntegrator), Error<SPI>> {
         // ATTENTION:
         // If you're changing anything about which SYS_STATUS flags are being
         // checked in this method, also make sure to update `enable_interrupts`.
@@ -310,6 +350,16 @@ where
             rssi,
         };
 
+        // read the carrier recovery integrator from the dw3000
+        use crate::ll::drx_car_int;
+        let carrier_integrator = CarrierRecoveryIntegrator::new(
+            self.ll()
+                .drx_car_int()
+                .read()
+                .map_err(|error| nb::Error::Other(Error::Spi(error)))?
+                .value(),
+        );
+
         // `rx_time` comes directly from the register, which should always
         // contain a 40-bit timestamp. Unless the hardware or its documentation
         // are buggy, the following should never panic.
@@ -362,7 +412,7 @@ where
 
         self.state.mark_finished();
 
-        Ok((len, rx_time, rx_quality))
+        Ok((len, rx_time, rx_quality, carrier_integrator))
     }
 
     /// DW3000 User Manual 4.7.1