@@ 1,6 1,8 @@ 
 use std::time::{Duration, Instant};
 
-use glam::{DVec3, Vec3};
+pub mod util;
+
+use util::*;
 
 pub trait Sensor {
     type SensorData;

          
@@ 90,59 92,6 @@ impl Default for Sensors {
     }
 }
 
-/// Milliseconds from an arbitrary start time.
-#[derive(Copy, Clone, Debug, PartialEq, Eq, PartialOrd, Ord)]
-pub enum Timestamp {
-    Reset,
-    Time(u64),
-}
-
-impl Default for Timestamp {
-    fn default() -> Self {
-        Timestamp::Reset
-    }
-}
-
-impl Timestamp {
-    /// Increment the current timestamp by the given number
-    /// of seconds.
-    pub const fn incr(self, ms: u64) -> Self {
-        match self {
-            // TODO: Ponder checked_add?  wrapping_add?  idk.
-            // 2^64 ms is 584 million years, so, *probably* not a concern.
-            Timestamp::Time(t) => Timestamp::Time(t + ms),
-            // TODO: Does a Reset mean "restart at 0", or noop?
-            Timestamp::Reset => Timestamp::Time(0),
-        }
-    }
-}
-
-#[derive(Copy, Clone, Debug, PartialEq, Eq)]
-pub enum Frame {
-    Ecef,
-    Ltp,
-    Odom,
-    Robot,
-}
-
-#[derive(Copy, Clone, Debug, PartialEq, Eq)]
-pub struct Stamped<T> {
-    stamp: Timestamp,
-    data: T,
-}
-
-impl<T> Stamped<T> {
-    fn empty(data: T) -> Self {
-        Self {
-            stamp: Timestamp::Reset,
-            data,
-        }
-    }
-    fn new(t: Timestamp, data: T) -> Self {
-        Self { stamp: t, data }
-    }
-}
-
 /// All frames are ECEF
 #[derive(Copy, Clone, Debug, PartialEq)]
 pub struct Localization {

          
@@ 151,35 100,6 @@ pub struct Localization {
     accel: Stamped<Accel>,
 }
 
-#[derive(Default, Copy, Clone, Debug, PartialEq)]
-pub struct Pos(DVec3);
-#[derive(Default, Copy, Clone, Debug, PartialEq)]
-pub struct Vel(Vec3);
-#[derive(Default, Copy, Clone, Debug, PartialEq)]
-pub struct Accel(Vec3);
-
-/// A rate in hz
-#[derive(Default, Copy, Clone, Debug, PartialEq)]
-pub struct Rate(f32);
-
-impl Rate {
-    pub fn should_tick(&self, last_time: Timestamp, now: Timestamp) -> bool {
-        let ms = (1000.0 / self.0).round() as u64;
-        match (last_time, now) {
-            (Timestamp::Time(last_t), Timestamp::Time(now_t)) => {
-                // Time should never go backwards plz
-                assert!(now_t >= last_t);
-                let dt = now_t - last_t;
-                dt >= ms
-            }
-            (Timestamp::Reset, Timestamp::Time(_)) => true,
-            (Timestamp::Time(_), Timestamp::Reset) => true,
-            // I suppose?
-            (Timestamp::Reset, Timestamp::Reset) => false,
-        }
-    }
-}
-
 impl Localization {
     pub fn new() -> Self {
         Self {

          
@@ 230,7 150,7 @@ impl RtClock {
             last_timestamp: Timestamp::Reset,
             // We run our main loop at 250 hz
             // This might mean busy-waiting a fair bit, but for now that's okay.
-            rate: Rate(250.0),
+            rate: Rate::new(250.0),
         }
     }
 

          
@@ 240,7 160,7 @@ impl RtClock {
     }
 
     pub fn tick(&mut self) -> Timestamp {
-        let ms = (1000.0 / self.rate.0).round() as u64;
+        let ms = (1000.0 / self.rate.hz()).round() as u64;
         self.next_update += Duration::from_millis(ms);
         self.last_timestamp = self.last_timestamp.incr(ms);
         self.last_timestamp

          

@@ 0,0 1,90 @@ 
+use glam::{DVec3, Vec3};
+
+/// Milliseconds from an arbitrary start time.
+#[derive(Copy, Clone, Debug, PartialEq, Eq, PartialOrd, Ord)]
+pub enum Timestamp {
+    Reset,
+    Time(u64),
+}
+
+impl Default for Timestamp {
+    fn default() -> Self {
+        Timestamp::Reset
+    }
+}
+
+impl Timestamp {
+    /// Increment the current timestamp by the given number
+    /// of seconds.
+    pub const fn incr(self, ms: u64) -> Self {
+        match self {
+            // TODO: Ponder checked_add?  wrapping_add?  idk.
+            // 2^64 ms is 584 million years, so, *probably* not a concern.
+            Timestamp::Time(t) => Timestamp::Time(t + ms),
+            // TODO: Does a Reset mean "restart at 0", or noop?
+            Timestamp::Reset => Timestamp::Time(0),
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+pub enum Frame {
+    Ecef,
+    Ltp,
+    Odom,
+    Robot,
+}
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+pub struct Stamped<T> {
+    stamp: Timestamp,
+    data: T,
+}
+
+impl<T> Stamped<T> {
+    pub fn empty(data: T) -> Self {
+        Self {
+            stamp: Timestamp::Reset,
+            data,
+        }
+    }
+    pub fn new(t: Timestamp, data: T) -> Self {
+        Self { stamp: t, data }
+    }
+}
+
+#[derive(Default, Copy, Clone, Debug, PartialEq)]
+pub struct Pos(DVec3);
+#[derive(Default, Copy, Clone, Debug, PartialEq)]
+pub struct Vel(Vec3);
+#[derive(Default, Copy, Clone, Debug, PartialEq)]
+pub struct Accel(Vec3);
+
+/// A rate in hz
+#[derive(Default, Copy, Clone, Debug, PartialEq)]
+pub struct Rate(f32);
+
+impl Rate {
+    pub fn new(hz: f32) -> Self {
+        Rate(hz)
+    }
+
+    pub fn hz(&self) -> f32 {
+        self.0
+    }
+    pub fn should_tick(&self, last_time: Timestamp, now: Timestamp) -> bool {
+        let ms = (1000.0 / self.0).round() as u64;
+        match (last_time, now) {
+            (Timestamp::Time(last_t), Timestamp::Time(now_t)) => {
+                // Time should never go backwards plz
+                assert!(now_t >= last_t);
+                let dt = now_t - last_t;
+                dt >= ms
+            }
+            (Timestamp::Reset, Timestamp::Time(_)) => true,
+            (Timestamp::Time(_), Timestamp::Reset) => true,
+            // I suppose?
+            (Timestamp::Reset, Timestamp::Reset) => false,
+        }
+    }
+}