From b66957b67c8a5b6c6912a221b751c40216d5101d Mon Sep 17 00:00:00 2001
From: Cowboylaserkittenjetshark
 <82691052+Cowboylaserkittenjetshark@users.noreply.github.com>
Date: Mon, 9 Mar 2026 20:04:52 -0400
Subject: [PATCH 01/11] WIP: add support for passing different vehicle
 characteristics

---
 src/comms/control_board/mod.rs | 38 +++++++++++++++++++++++++++-------
 1 file changed, 30 insertions(+), 8 deletions(-)

diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs
index d7cd4ee..8781148 100644
--- a/src/comms/control_board/mod.rs
+++ b/src/comms/control_board/mod.rs
@@ -46,14 +46,16 @@ impl<T: AsyncWriteExt + Unpin> Deref for ControlBoard<T> {
 }
 
 impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
-    pub async fn new<U>(comm_out: T, comm_in: U, msg_id: Option<MessageId>) -> Result<Self>
+    pub async fn new<U>(
+        comm_out: T,
+        comm_in: U,
+        msg_id: Option<MessageId>,
+        thruster_inversions: Vec<bool>,
+        dof_speeds: &[f32; 6],
+    ) -> Result<Self>
     where
         U: 'static + AsyncRead + Unpin + Send,
     {
-        const THRUSTER_INVS: [bool; 8] = [true, true, false, false, true, false, false, true];
-        #[allow(clippy::approx_constant)]
-        const DOF_SPEEDS: [f32; 6] = [0.7071, 0.7071, 1.0, 0.4413, 1.0, 0.8139];
-
         let msg_id = msg_id.unwrap_or_default();
         let responses = ResponseMap::new(comm_in).await;
         let this = Self {
@@ -62,8 +64,8 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
         };
 
         this.init_matrices().await?;
-        this.thruster_inversion_set(&THRUSTER_INVS).await?;
-        this.relative_dof_speed_set_batch(&DOF_SPEEDS).await?;
+        this.thruster_inversion_set(thruster_inversions).await?;
+        this.relative_dof_speed_set_batch(dof_speeds).await?;
         this.bno055_imu_axis_config(BNO055AxisConfig::P6).await?;
 
         loop {
@@ -102,6 +104,26 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
         }
         Ok(this)
     }
+    pub async fn new_with_defaults<U>(
+        comm_out: T,
+        comm_in: U,
+        msg_id: Option<MessageId>,
+    ) -> Result<Self>
+    where
+        U: 'static + AsyncRead + Unpin + Send,
+    {
+        const THRUSTER_INVS: [bool; 8] = [true, true, false, false, true, false, false, true];
+        #[allow(clippy::approx_constant)]
+        const DOF_SPEEDS: [f32; 6] = [0.7071, 0.7071, 1.0, 0.4413, 1.0, 0.8139];
+        Self::new(
+            comm_out,
+            comm_in,
+            msg_id,
+            Vec::from(THRUSTER_INVS),
+            &DOF_SPEEDS,
+        )
+        .await
+    }
 
     async fn init_matrices(&self) -> Result<()> {
         self.motor_matrix_set(3, -1.0, -1.0, 0.0, 0.0, 0.0, 1.0)
@@ -219,7 +241,7 @@ impl<T: AsyncWrite + Unpin> ControlBoard<T> {
     ///
     /// # Arguments:
     /// * `inversions` - Array of invert statuses, with motor 1 at index 0
-    pub async fn thruster_inversion_set(&self, inversions: &[bool; 8]) -> Result<()> {
+    pub async fn thruster_inversion_set(&self, inversions: Vec<bool>) -> Result<()> {
         const THRUSTER_INVERSION_SET: [u8; 4] = *b"TINV";
         let mut message = Vec::from(THRUSTER_INVERSION_SET);
 

From dbaee61b0f0818376e79f6afe68f65774b3860a0 Mon Sep 17 00:00:00 2001
From: fekie <fekie@proton.me>
Date: Mon, 9 Mar 2026 21:13:25 -0400
Subject: [PATCH 02/11] chore: add DoFMatrix for use in matrix initialization

---
 src/comms/control_board/mod.rs | 120 +++++++++++++++++++++++++++------
 1 file changed, 101 insertions(+), 19 deletions(-)

diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs
index 8781148..3af3821 100644
--- a/src/comms/control_board/mod.rs
+++ b/src/comms/control_board/mod.rs
@@ -17,6 +17,7 @@ use self::{
 
 use super::auv_control_board::{AUVControlBoard, MessageId};
 use crate::logln;
+use std::marker::PhantomData;
 
 pub mod response;
 pub mod util;
@@ -45,6 +46,66 @@ impl<T: AsyncWriteExt + Unpin> Deref for ControlBoard<T> {
     }
 }
 
+/// Contains a *populated* DoF matrix. Intended for us in [`init_matrices`].
+/// Can onlly be created using DoFMatrixBuilder.
+pub struct DoFMatrix([MotorMatrixRowParams; 8]);
+
+// Making a builder where all rows of the matrix must be constructed for it to be
+// able to be built
+// forgot if this was the exact name, feel free to change it
+/// Builder for creating the parameters for [`init_matrices`]
+/// There will be 8 rows, each with 6 elements
+#[derive(Debug)]
+pub struct DoFMatrixBuilder(pub [Option<MotorMatrixRowParams>; 8]);
+
+impl DoFMatrixBuilder {
+    pub fn new() -> Self {
+        DoFMatrixBuilder([None, None, None, None, None, None, None, None])
+    }
+
+    // TODO: make thrust amount modifible. It is also probably a good idea to use
+    // a hashmap as thruster indexes do not start as 0, and this struct needs
+    // to be modified to have different thruster amounts.
+    pub fn set_row(mut self, thruster: u8, parameters: MotorMatrixRowParams) -> Self {
+        let thruster_index = thruster - 1;
+        self.0[thruster_index as usize] = Some(parameters);
+        self
+    }
+
+    pub fn build(self) -> DoFMatrix {
+        // Later we will pass up an error, but for now we just panic as I don't
+        // know the error ecosystem works yet
+
+        // Make sure that all of the arrays contain `Some()`
+        DoFMatrix(self.0.map(|x| x.unwrap()))
+    }
+}
+
+/// Each row corresponds to a line of self.motor_matrix_set(...);
+/// Can either be created manually, or the parameters can be dumped into [`Self::new`]
+/// for shorter code.
+pub struct MotorMatrixRowParams {
+    x: f32,
+    y: f32,
+    z: f32,
+    pitch: f32,
+    roll: f32,
+    yaw: f32,
+}
+
+impl MotorMatrixRowParams {
+    pub fn new(x: f32, y: f32, z: f32, pitch: f32, roll: f32, yaw: f32) -> Self {
+        Self {
+            x,
+            y,
+            z,
+            pitch,
+            roll,
+            yaw,
+        }
+    }
+}
+
 impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
     pub async fn new<U>(
         comm_out: T,
@@ -63,7 +124,21 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
             initial_angles: Arc::default(),
         };
 
-        this.init_matrices().await?;
+        let dof_matrix = DoFMatrixBuilder::new()
+            .set_row(1, MotorMatrixRowParams::new(-1.0, 1.0, 0.0, 0.0, 0.0, -1.0))
+            .set_row(2, MotorMatrixRowParams::new(1.0, 1.0, 0.0, 0.0, 0.0, 1.0))
+            .set_row(3, MotorMatrixRowParams::new(-1.0, -1.0, 0.0, 0.0, 0.0, 1.0))
+            .set_row(4, MotorMatrixRowParams::new(1.0, -1.0, 0.0, 0.0, 0.0, -1.0))
+            .set_row(5, MotorMatrixRowParams::new(0.0, 0.0, -1.0, 1.0, -1.0, 0.0))
+            .set_row(6, MotorMatrixRowParams::new(0.0, 0.0, -1.0, 1.0, 1.0, 0.0))
+            .set_row(
+                7,
+                MotorMatrixRowParams::new(0.0, 0.0, -1.0, -1.0, -1.0, 0.0),
+            )
+            .set_row(8, MotorMatrixRowParams::new(0.0, 0.0, -1.0, -1.0, 1.0, 0.0))
+            .build();
+
+        this.init_matrices(dof_matrix).await?;
         this.thruster_inversion_set(thruster_inversions).await?;
         this.relative_dof_speed_set_batch(dof_speeds).await?;
         this.bno055_imu_axis_config(BNO055AxisConfig::P6).await?;
@@ -125,25 +200,32 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
         .await
     }
 
-    async fn init_matrices(&self) -> Result<()> {
-        self.motor_matrix_set(3, -1.0, -1.0, 0.0, 0.0, 0.0, 1.0)
-            .await?;
-        self.motor_matrix_set(4, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0)
-            .await?;
-        self.motor_matrix_set(1, -1.0, 1.0, 0.0, 0.0, 0.0, -1.0)
-            .await?;
-        self.motor_matrix_set(2, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0)
-            .await?;
-        self.motor_matrix_set(7, 0.0, 0.0, -1.0, -1.0, -1.0, 0.0)
-            .await?;
-        self.motor_matrix_set(8, 0.0, 0.0, -1.0, -1.0, 1.0, 0.0)
-            .await?;
-        self.motor_matrix_set(5, 0.0, 0.0, -1.0, 1.0, -1.0, 0.0)
-            .await?;
-        self.motor_matrix_set(6, 0.0, 0.0, -1.0, 1.0, 1.0, 0.0)
-            .await?;
+    async fn init_matrices(&self, dof_matrix: DoFMatrix) -> Result<()> {
+        for (i, row) in dof_matrix.0.iter().enumerate() {
+            self.motor_matrix_set(i as u8, row.x, row.y, row.z, row.pitch, row.roll, row.yaw)
+                .await?;
+        }
 
-        self.motor_matrix_update().await
+        self.motor_matrix_update().await;
+
+        // self.motor_matrix_set(3, -1.0, -1.0, 0.0, 0.0, 0.0, 1.0)
+        //     .await?;
+        // self.motor_matrix_set(4, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0)
+        //     .await?;
+        // self.motor_matrix_set(1, -1.0, 1.0, 0.0, 0.0, 0.0, -1.0)
+        //     .await?;
+        // self.motor_matrix_set(2, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0)
+        //     .await?;
+        // self.motor_matrix_set(7, 0.0, 0.0, -1.0, -1.0, -1.0, 0.0)
+        //     .await?;
+        // self.motor_matrix_set(8, 0.0, 0.0, -1.0, -1.0, 1.0, 0.0)
+        //     .await?;
+        // self.motor_matrix_set(5, 0.0, 0.0, -1.0, 1.0, -1.0, 0.0)
+        //     .await?;
+        // self.motor_matrix_set(6, 0.0, 0.0, -1.0, 1.0, 1.0, 0.0)
+        //     .await?;
+
+        // self.motor_matrix_update().await
     }
 
     async fn stab_tune(&self) -> Result<()> {

From 5925faf014bf98f22a3e0b3e40ca295921355e3c Mon Sep 17 00:00:00 2001
From: fekie <fekie@proton.me>
Date: Tue, 10 Mar 2026 20:23:29 -0400
Subject: [PATCH 03/11] chore: make thruster amount configurable

---
 src/comms/control_board/mod.rs | 34 ++++++++++++++++++----------------
 1 file changed, 18 insertions(+), 16 deletions(-)

diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs
index 3af3821..6d17d35 100644
--- a/src/comms/control_board/mod.rs
+++ b/src/comms/control_board/mod.rs
@@ -48,24 +48,23 @@ impl<T: AsyncWriteExt + Unpin> Deref for ControlBoard<T> {
 
 /// Contains a *populated* DoF matrix. Intended for us in [`init_matrices`].
 /// Can onlly be created using DoFMatrixBuilder.
-pub struct DoFMatrix([MotorMatrixRowParams; 8]);
+pub struct DoFMatrix(pub(crate) [Option<MotorMatrixRowParams>; 8]);
 
-// Making a builder where all rows of the matrix must be constructed for it to be
-// able to be built
-// forgot if this was the exact name, feel free to change it
-/// Builder for creating the parameters for [`init_matrices`]
-/// There will be 8 rows, each with 6 elements
+/// A builder for DoFMatrix. This struct makes it cleaner to define thruster parameters,
+/// as well as validate that the correct amount of thrusters have a definition.
 #[derive(Debug)]
-pub struct DoFMatrixBuilder(pub [Option<MotorMatrixRowParams>; 8]);
+pub struct DoFMatrixBuilder {
+    pub(crate) params: [Option<MotorMatrixRowParams>; 8],
+    pub(crate) thrusters_in_use: u8,
+}
 
 impl DoFMatrixBuilder {
-    pub fn new() -> Self {
+    /// Creates an unpopulated builder.
+    pub fn new(thrusters_in_use: u8) -> Self {
         DoFMatrixBuilder([None, None, None, None, None, None, None, None])
     }
 
-    // TODO: make thrust amount modifible. It is also probably a good idea to use
-    // a hashmap as thruster indexes do not start as 0, and this struct needs
-    // to be modified to have different thruster amounts.
+    /// Sets the parameters for a specific thruster.
     pub fn set_row(mut self, thruster: u8, parameters: MotorMatrixRowParams) -> Self {
         let thruster_index = thruster - 1;
         self.0[thruster_index as usize] = Some(parameters);
@@ -73,11 +72,13 @@ impl DoFMatrixBuilder {
     }
 
     pub fn build(self) -> DoFMatrix {
-        // Later we will pass up an error, but for now we just panic as I don't
-        // know the error ecosystem works yet
+        // Make sure that we currently have parameters for `thrusters_in_use` thrusters.
+        let thrusters_defined = self.params.iter().filter(|x| x.is_some()).iter().count();
+
+        assert!(thrusters_defined, self.thrusters_in_use);
 
         // Make sure that all of the arrays contain `Some()`
-        DoFMatrix(self.0.map(|x| x.unwrap()))
+        DoFMatrix(self.0)
     }
 }
 
@@ -124,7 +125,8 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
             initial_angles: Arc::default(),
         };
 
-        let dof_matrix = DoFMatrixBuilder::new()
+        const THRUSTER_COUNT: u8 = 8;
+        let dof_matrix = DoFMatrixBuilder::new(THRUSTER_COUNT)
             .set_row(1, MotorMatrixRowParams::new(-1.0, 1.0, 0.0, 0.0, 0.0, -1.0))
             .set_row(2, MotorMatrixRowParams::new(1.0, 1.0, 0.0, 0.0, 0.0, 1.0))
             .set_row(3, MotorMatrixRowParams::new(-1.0, -1.0, 0.0, 0.0, 0.0, 1.0))
@@ -201,7 +203,7 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
     }
 
     async fn init_matrices(&self, dof_matrix: DoFMatrix) -> Result<()> {
-        for (i, row) in dof_matrix.0.iter().enumerate() {
+        for (i, row) in dof_matrix.0.iter().filter_map(|x| *x).enumerate() {
             self.motor_matrix_set(i as u8, row.x, row.y, row.z, row.pitch, row.roll, row.yaw)
                 .await?;
         }

From ca9c7e23b44c98d55e9aab108588681347e2bf39 Mon Sep 17 00:00:00 2001
From: fekie <fekie@proton.me>
Date: Tue, 10 Mar 2026 20:26:25 -0400
Subject: [PATCH 04/11] chore: fix init_matrices

---
 src/comms/control_board/mod.rs | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs
index 6d17d35..04913e9 100644
--- a/src/comms/control_board/mod.rs
+++ b/src/comms/control_board/mod.rs
@@ -203,9 +203,12 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
     }
 
     async fn init_matrices(&self, dof_matrix: DoFMatrix) -> Result<()> {
-        for (i, row) in dof_matrix.0.iter().filter_map(|x| *x).enumerate() {
-            self.motor_matrix_set(i as u8, row.x, row.y, row.z, row.pitch, row.roll, row.yaw)
-                .await?;
+        for (i, _row) in dof_matrix.0.iter().enumerate() {
+            // If the row is defined for the thruster, then set it
+            if let Some(row) = _row {
+                self.motor_matrix_set(i as u8, row.x, row.y, row.z, row.pitch, row.roll, row.yaw)
+                    .await?;
+            }
         }
 
         self.motor_matrix_update().await;

From c50e1a8941c1845f09b8c0c1c2b9f0231b3aed43 Mon Sep 17 00:00:00 2001
From: fekie <fekie@proton.me>
Date: Tue, 10 Mar 2026 20:39:38 -0400
Subject: [PATCH 05/11] chore: fix logic errors

---
 src/comms/control_board/mod.rs | 17 +++++++++++------
 1 file changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs
index 04913e9..c255f56 100644
--- a/src/comms/control_board/mod.rs
+++ b/src/comms/control_board/mod.rs
@@ -48,6 +48,7 @@ impl<T: AsyncWriteExt + Unpin> Deref for ControlBoard<T> {
 
 /// Contains a *populated* DoF matrix. Intended for us in [`init_matrices`].
 /// Can onlly be created using DoFMatrixBuilder.
+#[derive(Debug)]
 pub struct DoFMatrix(pub(crate) [Option<MotorMatrixRowParams>; 8]);
 
 /// A builder for DoFMatrix. This struct makes it cleaner to define thruster parameters,
@@ -61,30 +62,34 @@ pub struct DoFMatrixBuilder {
 impl DoFMatrixBuilder {
     /// Creates an unpopulated builder.
     pub fn new(thrusters_in_use: u8) -> Self {
-        DoFMatrixBuilder([None, None, None, None, None, None, None, None])
+        DoFMatrixBuilder {
+            params: [None, None, None, None, None, None, None, None],
+            thrusters_in_use,
+        }
     }
 
     /// Sets the parameters for a specific thruster.
     pub fn set_row(mut self, thruster: u8, parameters: MotorMatrixRowParams) -> Self {
         let thruster_index = thruster - 1;
-        self.0[thruster_index as usize] = Some(parameters);
+        self.params[thruster_index as usize] = Some(parameters);
         self
     }
 
     pub fn build(self) -> DoFMatrix {
         // Make sure that we currently have parameters for `thrusters_in_use` thrusters.
-        let thrusters_defined = self.params.iter().filter(|x| x.is_some()).iter().count();
+        let thrusters_defined = self.params.iter().filter(|x| x.is_some()).count();
 
-        assert!(thrusters_defined, self.thrusters_in_use);
+        assert_eq!(thrusters_defined, self.thrusters_in_use as usize);
 
         // Make sure that all of the arrays contain `Some()`
-        DoFMatrix(self.0)
+        DoFMatrix(self.params)
     }
 }
 
 /// Each row corresponds to a line of self.motor_matrix_set(...);
 /// Can either be created manually, or the parameters can be dumped into [`Self::new`]
 /// for shorter code.
+#[derive(Debug)]
 pub struct MotorMatrixRowParams {
     x: f32,
     y: f32,
@@ -211,7 +216,7 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
             }
         }
 
-        self.motor_matrix_update().await;
+        self.motor_matrix_update().await
 
         // self.motor_matrix_set(3, -1.0, -1.0, 0.0, 0.0, 0.0, 1.0)
         //     .await?;

From 6d6c489e59fc688cd97084405cb33de1d37062aa Mon Sep 17 00:00:00 2001
From: Cowboylaserkittenjetshark
 <82691052+Cowboylaserkittenjetshark@users.noreply.github.com>
Date: Wed, 8 Apr 2026 03:26:02 -0400
Subject: [PATCH 06/11] feat(wip): parameterized vehicle parameters for control
 board

---
 Cargo.lock                     |   1 +
 Cargo.toml                     |   1 +
 src/comms/control_board/mod.rs | 175 +++++++++++++++++++--------------
 src/main.rs                    |  19 +++-
 4 files changed, 118 insertions(+), 78 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index fb5e660..aa292a0 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -1926,6 +1926,7 @@ dependencies = [
  "ros2-interfaces-jazzy-serde",
  "serde",
  "serde_json",
+ "thiserror 2.0.18",
  "tokio",
  "tokio-serial",
  "tokio-util",
diff --git a/Cargo.toml b/Cargo.toml
index 25820a6..c85b9b9 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -46,3 +46,4 @@ serde_json = "1.0.140"
 tokio-util = { version = "0.7.15" }
 ros2-client = "0.8.2"
 ros2-interfaces-jazzy-serde = { version = "0.0.4", features = ["sensor_msgs"] }
+thiserror = "2.0.18"
diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs
index c255f56..057dcb3 100644
--- a/src/comms/control_board/mod.rs
+++ b/src/comms/control_board/mod.rs
@@ -2,6 +2,7 @@ use core::fmt::Debug;
 use std::{ops::Deref, sync::Arc, time::Duration};
 
 use anyhow::{anyhow, bail, Result};
+use thiserror::Error;
 use tokio::{
     io::{self, AsyncRead, AsyncWrite, AsyncWriteExt, WriteHalf},
     net::TcpStream,
@@ -17,7 +18,6 @@ use self::{
 
 use super::auv_control_board::{AUVControlBoard, MessageId};
 use crate::logln;
-use std::marker::PhantomData;
 
 pub mod response;
 pub mod util;
@@ -46,23 +46,62 @@ impl<T: AsyncWriteExt + Unpin> Deref for ControlBoard<T> {
     }
 }
 
-/// Contains a *populated* DoF matrix. Intended for us in [`init_matrices`].
-/// Can onlly be created using DoFMatrixBuilder.
+type ThrusterInversions = Vec<bool>;
+type DofSpeeds = [f32; 6];
+
+pub struct VehicleDefinition {
+    pub(crate) motor_matrix: MotorMatrix,
+    pub(crate) thruster_inversions: ThrusterInversions,
+    pub(crate) dof_speeds: DofSpeeds,
+}
+
+impl VehicleDefinition {
+    pub fn new(
+        motor_matrix: MotorMatrix,
+        thruster_inversions: ThrusterInversions,
+        dof_speeds: DofSpeeds,
+    ) -> Result<Self, ControlBoardError> {
+        use ControlBoardError::*;
+
+        let rows = motor_matrix.0.iter().map(|r| r.is_some() as u8).sum();
+        let inversions = thruster_inversions.len();
+
+        if rows != inversions as u8 {
+            Err(ThrusterMismatch { rows, inversions })
+        } else {
+            Ok(Self {
+                motor_matrix,
+                thruster_inversions,
+                dof_speeds,
+            })
+        }
+    }
+}
+
+/// Contains a *populated* motor matrix.
+/// Can only be created using [`MotorMatrixBuilder`].
 #[derive(Debug)]
-pub struct DoFMatrix(pub(crate) [Option<MotorMatrixRowParams>; 8]);
+pub struct MotorMatrix(pub(crate) [Option<MotorMatrixRowParams>; 8]);
+
+impl MotorMatrix {
+    /// Creates a new [`MotorMatrixBuilder`]
+    pub fn builder(thrusters_in_use: u8) -> MotorMatrixBuilder {
+        MotorMatrixBuilder::new(thrusters_in_use)
+    }
+}
 
-/// A builder for DoFMatrix. This struct makes it cleaner to define thruster parameters,
+/// A builder for MotorMatrix. This struct makes it cleaner to define thruster parameters,
 /// as well as validate that the correct amount of thrusters have a definition.
 #[derive(Debug)]
-pub struct DoFMatrixBuilder {
+pub struct MotorMatrixBuilder {
     pub(crate) params: [Option<MotorMatrixRowParams>; 8],
     pub(crate) thrusters_in_use: u8,
 }
 
-impl DoFMatrixBuilder {
+impl MotorMatrixBuilder {
     /// Creates an unpopulated builder.
     pub fn new(thrusters_in_use: u8) -> Self {
-        DoFMatrixBuilder {
+        MotorMatrixBuilder {
             params: [None, None, None, None, None, None, None, None],
             thrusters_in_use,
         }
@@ -75,14 +114,15 @@ impl DoFMatrixBuilder {
         self
     }
 
-    pub fn build(self) -> DoFMatrix {
+    /// Build the populated [`MotorMatrix`]
+    pub fn build(self) -> MotorMatrix {
         // Make sure that we currently have parameters for `thrusters_in_use` thrusters.
         let thrusters_defined = self.params.iter().filter(|x| x.is_some()).count();
 
         assert_eq!(thrusters_defined, self.thrusters_in_use as usize);
 
         // Make sure that all of the arrays contain `Some()`
-        DoFMatrix(self.params)
+        MotorMatrix(self.params)
     }
 }
 
@@ -112,13 +152,31 @@ impl MotorMatrixRowParams {
     }
 }
 
+impl From<[f32; 6]> for MotorMatrixRowParams {
+    fn from(vals: [f32; 6]) -> Self {
+        Self {
+            x: vals[0],
+            y: vals[1],
+            z: vals[2],
+            pitch: vals[3],
+            roll: vals[4],
+            yaw: vals[5],
+        }
+    }
+}
+
+#[derive(Error, Debug)]
+pub enum ControlBoardError {
+    #[error("number of motor matrix rows and number of thruster inversions do not match: {rows:?} rows, {inversions:?} inversions")]
+    ThrusterMismatch { rows: u8, inversions: usize },
+}
+
 impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
     pub async fn new<U>(
         comm_out: T,
         comm_in: U,
         msg_id: Option<MessageId>,
-        thruster_inversions: Vec<bool>,
-        dof_speeds: &[f32; 6],
+        vehicle_defintion: VehicleDefinition,
     ) -> Result<Self>
     where
         U: 'static + AsyncRead + Unpin + Send,
@@ -130,24 +188,23 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
             initial_angles: Arc::default(),
         };
 
-        const THRUSTER_COUNT: u8 = 8;
-        let dof_matrix = DoFMatrixBuilder::new(THRUSTER_COUNT)
-            .set_row(1, MotorMatrixRowParams::new(-1.0, 1.0, 0.0, 0.0, 0.0, -1.0))
-            .set_row(2, MotorMatrixRowParams::new(1.0, 1.0, 0.0, 0.0, 0.0, 1.0))
-            .set_row(3, MotorMatrixRowParams::new(-1.0, -1.0, 0.0, 0.0, 0.0, 1.0))
-            .set_row(4, MotorMatrixRowParams::new(1.0, -1.0, 0.0, 0.0, 0.0, -1.0))
-            .set_row(5, MotorMatrixRowParams::new(0.0, 0.0, -1.0, 1.0, -1.0, 0.0))
-            .set_row(6, MotorMatrixRowParams::new(0.0, 0.0, -1.0, 1.0, 1.0, 0.0))
-            .set_row(
-                7,
-                MotorMatrixRowParams::new(0.0, 0.0, -1.0, -1.0, -1.0, 0.0),
-            )
-            .set_row(8, MotorMatrixRowParams::new(0.0, 0.0, -1.0, -1.0, 1.0, 0.0))
-            .build();
-
-        this.init_matrices(dof_matrix).await?;
-        this.thruster_inversion_set(thruster_inversions).await?;
-        this.relative_dof_speed_set_batch(dof_speeds).await?;
+        // const THRUSTER_COUNT: u8 = 8;
+        // let motor_matrix = MotorMatrixBuilder::new(THRUSTER_COUNT)
+        //     .set_row(1, MotorMatrixRowParams::new(-1.0, 1.0, 0.0, 0.0, 0.0, -1.0))
+        //     .set_row(2, MotorMatrixRowParams::new(1.0, 1.0, 0.0, 0.0, 0.0, 1.0))
+        //     .set_row(3, MotorMatrixRowParams::new(-1.0, -1.0, 0.0, 0.0, 0.0, 1.0))
+        //     .set_row(4, MotorMatrixRowParams::new(1.0, -1.0, 0.0, 0.0, 0.0, -1.0))
+        //     .set_row(5, MotorMatrixRowParams::new(0.0, 0.0, -1.0, 1.0, -1.0, 0.0))
+        //     .set_row(6, MotorMatrixRowParams::new(0.0, 0.0, -1.0, 1.0, 1.0, 0.0))
+        //     .set_row(7, MotorMatrixRowParams::new(0.0, 0.0, -1.0, -1.0, -1.0, 0.0))
+        //     .set_row(8, MotorMatrixRowParams::new(0.0, 0.0, -1.0, -1.0, 1.0, 0.0))
+        //     .build();
+
+        this.init_matrices(vehicle_defintion.motor_matrix).await?;
+        this.thruster_inversion_set(vehicle_defintion.thruster_inversions)
+            .await?;
+        this.relative_dof_speed_set_batch(&vehicle_defintion.dof_speeds)
+            .await?;
         this.bno055_imu_axis_config(BNO055AxisConfig::P6).await?;
 
         loop {
@@ -186,29 +243,9 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
         }
         Ok(this)
     }
-    pub async fn new_with_defaults<U>(
-        comm_out: T,
-        comm_in: U,
-        msg_id: Option<MessageId>,
-    ) -> Result<Self>
-    where
-        U: 'static + AsyncRead + Unpin + Send,
-    {
-        const THRUSTER_INVS: [bool; 8] = [true, true, false, false, true, false, false, true];
-        #[allow(clippy::approx_constant)]
-        const DOF_SPEEDS: [f32; 6] = [0.7071, 0.7071, 1.0, 0.4413, 1.0, 0.8139];
-        Self::new(
-            comm_out,
-            comm_in,
-            msg_id,
-            Vec::from(THRUSTER_INVS),
-            &DOF_SPEEDS,
-        )
-        .await
-    }
-
-    async fn init_matrices(&self, dof_matrix: DoFMatrix) -> Result<()> {
-        for (i, _row) in dof_matrix.0.iter().enumerate() {
+
+    async fn init_matrices(&self, motor_matrix: MotorMatrix) -> Result<()> {
+        for (i, _row) in motor_matrix.0.iter().enumerate() {
             // If the row is defined for the thruster, then set it
             if let Some(row) = _row {
                 self.motor_matrix_set(i as u8, row.x, row.y, row.z, row.pitch, row.roll, row.yaw)
@@ -217,25 +254,6 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
         }
 
         self.motor_matrix_update().await
-
-        // self.motor_matrix_set(3, -1.0, -1.0, 0.0, 0.0, 0.0, 1.0)
-        //     .await?;
-        // self.motor_matrix_set(4, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0)
-        //     .await?;
-        // self.motor_matrix_set(1, -1.0, 1.0, 0.0, 0.0, 0.0, -1.0)
-        //     .await?;
-        // self.motor_matrix_set(2, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0)
-        //     .await?;
-        // self.motor_matrix_set(7, 0.0, 0.0, -1.0, -1.0, -1.0, 0.0)
-        //     .await?;
-        // self.motor_matrix_set(8, 0.0, 0.0, -1.0, -1.0, 1.0, 0.0)
-        //     .await?;
-        // self.motor_matrix_set(5, 0.0, 0.0, -1.0, 1.0, -1.0, 0.0)
-        //     .await?;
-        // self.motor_matrix_set(6, 0.0, 0.0, -1.0, 1.0, 1.0, 0.0)
-        //     .await?;
-
-        // self.motor_matrix_update().await
     }
 
     async fn stab_tune(&self) -> Result<()> {
@@ -251,7 +269,7 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
 }
 
 impl ControlBoard<WriteHalf<SerialStream>> {
-    pub async fn serial(port_name: &str) -> Result<Self> {
+    pub async fn serial(port_name: &str, vehicle_defintion: VehicleDefinition) -> Result<Self> {
         const BAUD_RATE: u32 = 9600;
         const DATA_BITS: DataBits = DataBits::Eight;
         const PARITY: Parity = Parity::None;
@@ -262,14 +280,19 @@ impl ControlBoard<WriteHalf<SerialStream>> {
             .parity(PARITY)
             .stop_bits(STOP_BITS);
         let (comm_in, comm_out) = io::split(SerialStream::open(&port_builder)?);
-        Self::new(comm_out, comm_in, None).await
+        Self::new(comm_out, comm_in, None, vehicle_defintion).await
     }
 }
 
 impl ControlBoard<WriteHalf<TcpStream>> {
     /// Both connections are necessary for the simulator to run,
     /// but the one that doesn't feed forward to control board is unnecessary
-    pub async fn tcp(host: &str, port: &str, dummy_port: String) -> Result<Self> {
+    pub async fn tcp(
+        host: &str,
+        port: &str,
+        dummy_port: String,
+        vehicle_defintion: VehicleDefinition,
+    ) -> Result<Self> {
         let host = host.to_string();
         let host_clone = host.clone();
         tokio::spawn(async move {
@@ -284,7 +307,7 @@ impl ControlBoard<WriteHalf<TcpStream>> {
 
         let stream = TcpStream::connect(host.to_string() + ":" + port).await?;
         let (comm_in, comm_out) = io::split(stream);
-        Self::new(comm_out, comm_in, None).await
+        Self::new(comm_out, comm_in, None, vehicle_defintion).await
     }
 }
 
diff --git a/src/main.rs b/src/main.rs
index e6ee280..04fb0fa 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -6,7 +6,7 @@ use std::process::exit;
 use std::time::Duration;
 use sw9s_lib::{
     comms::{
-        control_board::{ControlBoard, SensorStatuses},
+        control_board::{ControlBoard, MotorMatrix, SensorStatuses, VehicleDefinition},
         meb::MainElectronicsBoard,
         zed_ros2::ZedRos2,
     },
@@ -62,7 +62,22 @@ async fn control_board() -> &'static ControlBoard<WriteHalf<SerialStream>> {
     let config = config().await;
     CONTROL_BOARD_CELL
         .get_or_init(|| async {
-            let board = ControlBoard::serial(config.control_board_path.as_str()).await;
+            let motor_matrix = MotorMatrix::builder(8)
+                .set_row(1, [-1.0, 1.0, 0.0, 0.0, 0.0, -1.0].into())
+                .set_row(2, [1.0, 1.0, 0.0, 0.0, 0.0, 1.0].into())
+                .set_row(3, [-1.0, -1.0, 0.0, 0.0, 0.0, 1.0].into())
+                .set_row(4, [1.0, -1.0, 0.0, 0.0, 0.0, -1.0].into())
+                .set_row(5, [0.0, 0.0, -1.0, 1.0, -1.0, 0.0].into())
+                .set_row(6, [0.0, 0.0, -1.0, 1.0, 1.0, 0.0].into())
+                .set_row(7, [0.0, 0.0, -1.0, -1.0, -1.0, 0.0].into())
+                .set_row(8, [0.0, 0.0, -1.0, -1.0, 1.0, 0.0].into())
+                .build();
+            let vehicle_def = VehicleDefinition::new(
+                motor_matrix,
+                [true, true, false, false, true, false, false, true].into(),
+                [0.7071, 0.7071, 1.0, 0.4413, 1.0, 0.8139],
+            )
+            let board = ControlBoard::serial(config.control_board_path.as_str(), vehicle_def).await;
             match board {
                 Ok(x) => x,
                 Err(e) => {

From 02feac47fe57d930a8825a60bde70a338db2813b Mon Sep 17 00:00:00 2001
From: Cowboylaserkittenjetshark
 <82691052+Cowboylaserkittenjetshark@users.noreply.github.com>
Date: Thu, 9 Apr 2026 03:25:45 -0400
Subject: [PATCH 07/11] feat(wip): configurable vehicle defintion and improved
 errors - Pass VehicleDefinition to control board constructors - Replace
 anyhow with color eyre - Replace usages of anyhow in the comms module with
 custom errors

---
 Cargo.lock                              | 97 ++++++++++++++++++++++++-
 Cargo.toml                              |  2 +-
 src/comms/auv_control_board/mod.rs      |  2 +-
 src/comms/auv_control_board/response.rs |  2 +-
 src/comms/auv_control_board/util.rs     | 17 ++---
 src/comms/control_board/mod.rs          | 50 ++++++++-----
 src/comms/control_board/util.rs         |  4 +-
 src/comms/meb/mod.rs                    | 16 +++-
 src/comms/zed_ros2.rs                   |  2 +-
 src/config/mod.rs                       |  2 +-
 src/main.rs                             | 24 +++---
 src/missions/action.rs                  |  2 +-
 src/missions/comms.rs                   |  7 +-
 src/missions/extra.rs                   | 53 +++++++-------
 src/missions/movement.rs                | 46 ++++++------
 src/missions/vision.rs                  | 14 ++--
 src/video_source/appsink.rs             |  2 +-
 src/vision/bin.rs                       |  2 +-
 src/vision/gate.rs                      |  2 +-
 src/vision/gate_cv.rs                   |  3 +-
 src/vision/gate_poles.rs                |  2 +-
 src/vision/image_prep.rs                |  2 +-
 src/vision/mod.rs                       |  4 +-
 src/vision/nn_cv2.rs                    |  2 +-
 src/vision/octagon.rs                   |  3 +-
 src/vision/path_cv.rs                   |  7 +-
 src/vision/slalom.rs                    |  3 +-
 src/vision/slalom_yolo.rs               |  2 +-
 src/vision/yolo_model.rs                |  2 +-
 29 files changed, 252 insertions(+), 124 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index aa292a0..dd6d053 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -2,6 +2,21 @@
 # It is not intended for manual editing.
 version = 4
 
+[[package]]
+name = "addr2line"
+version = "0.25.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1b5d307320b3181d6d7954e663bd7c774a838b8220fe0593c86d9fb09f498b4b"
+dependencies = [
+ "gimli",
+]
+
+[[package]]
+name = "adler2"
+version = "2.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "320119579fcad9c21884f5c4861d16174d0e06250625266f50fe6898340abefa"
+
 [[package]]
 name = "aho-corasick"
 version = "1.1.4"
@@ -103,6 +118,21 @@ version = "1.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8"
 
+[[package]]
+name = "backtrace"
+version = "0.3.76"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bb531853791a215d7c62a30daf0dde835f381ab5de4589cfe7c649d2cbe92bd6"
+dependencies = [
+ "addr2line",
+ "cfg-if 1.0.4",
+ "libc",
+ "miniz_oxide",
+ "object",
+ "rustc-demangle",
+ "windows-link 0.2.1",
+]
+
 [[package]]
 name = "base64"
 version = "0.22.1"
@@ -320,6 +350,19 @@ version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "3a822ea5bc7590f9d40f1ba12c0dc3c2760f3482c6984db1573ad11031420831"
 
+[[package]]
+name = "color-eyre"
+version = "0.6.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e5920befb47832a6d61ee3a3a846565cfa39b331331e68a3b1d1116630f2f26d"
+dependencies = [
+ "backtrace",
+ "eyre",
+ "indenter",
+ "once_cell",
+ "owo-colors",
+]
+
 [[package]]
 name = "colorchoice"
 version = "1.0.4"
@@ -575,6 +618,16 @@ dependencies = [
  "pin-project-lite",
 ]
 
+[[package]]
+name = "eyre"
+version = "0.6.12"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7cd915d99f24784cdc19fd37ef22b97e3ff0ae756c7e492e9fbfe897d61e2aec"
+dependencies = [
+ "indenter",
+ "once_cell",
+]
+
 [[package]]
 name = "find-msvc-tools"
 version = "0.1.9"
@@ -734,6 +787,12 @@ dependencies = [
  "syn 2.0.117",
 ]
 
+[[package]]
+name = "gimli"
+version = "0.32.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e629b9b98ef3dd8afe6ca2bd0f89306cec16d43d907889945bc5d6687f2f13c7"
+
 [[package]]
 name = "glob"
 version = "0.3.3"
@@ -819,6 +878,12 @@ dependencies = [
  "windows-sys 0.59.0",
 ]
 
+[[package]]
+name = "indenter"
+version = "0.3.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "964de6e86d545b246d84badc0fef527924ace5134f30641c203ef52ba83f58d5"
+
 [[package]]
 name = "indexmap"
 version = "1.9.3"
@@ -1032,6 +1097,15 @@ dependencies = [
  "autocfg",
 ]
 
+[[package]]
+name = "miniz_oxide"
+version = "0.8.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1fa76a2c86f704bdb222d66965fb3d63269ce38518b83cb0575fca855ebb6316"
+dependencies = [
+ "adler2",
+]
+
 [[package]]
 name = "mio"
 version = "0.6.23"
@@ -1216,6 +1290,15 @@ dependencies = [
  "autocfg",
 ]
 
+[[package]]
+name = "object"
+version = "0.37.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ff76201f031d8863c38aa7f905eca4f53abbfa15f609db4277d44cd8938f33fe"
+dependencies = [
+ "memchr",
+]
+
 [[package]]
 name = "once_cell"
 version = "1.21.3"
@@ -1263,6 +1346,12 @@ dependencies = [
  "shlex",
 ]
 
+[[package]]
+name = "owo-colors"
+version = "4.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d211803b9b6b570f68772237e415a029d5a50c65d382910b879fb19d3271f94d"
+
 [[package]]
 name = "parking"
 version = "2.2.1"
@@ -1611,6 +1700,12 @@ dependencies = [
  "serde_with",
 ]
 
+[[package]]
+name = "rustc-demangle"
+version = "0.1.27"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b50b8869d9fc858ce7266cce0194bd74df58b9d0e3f6df3a9fc8eb470d95c09d"
+
 [[package]]
 name = "rustdds"
 version = "0.11.8"
@@ -1913,10 +2008,10 @@ checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f"
 name = "sw9s"
 version = "0.1.0"
 dependencies = [
- "anyhow",
  "bluerobotics-ping",
  "bytes",
  "chrono",
+ "color-eyre",
  "derive-getters",
  "futures",
  "itertools 0.13.0",
diff --git a/Cargo.toml b/Cargo.toml
index c85b9b9..8ce384f 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -31,7 +31,6 @@ annotated_streams = ["logging"]
 opencv = { version = "0.94.2", default-features = false, features = ["dnn", "imgcodecs", "imgproc", "videoio", "cudaimgproc", "cudafilters"], optional = true } # Vision processing
 tokio-serial = "5.4.1" # Async serial comms
 tokio = { version = "1.38.0", features = ["full"] } # Async runtime
-anyhow = "1.0.86" # Error handling
 itertools = "0.13.0" # Enhance iterators
 num-traits = "0.2.19" # Numeric generics
 derive-getters = "0.4.0" # Getter macro
@@ -47,3 +46,4 @@ tokio-util = { version = "0.7.15" }
 ros2-client = "0.8.2"
 ros2-interfaces-jazzy-serde = { version = "0.0.4", features = ["sensor_msgs"] }
 thiserror = "2.0.18"
+color-eyre = { version = "0.6.5", default-features = false }
diff --git a/src/comms/auv_control_board/mod.rs b/src/comms/auv_control_board/mod.rs
index ab47c40..c658e68 100644
--- a/src/comms/auv_control_board/mod.rs
+++ b/src/comms/auv_control_board/mod.rs
@@ -1,12 +1,12 @@
 use core::fmt::Debug;
 use std::sync::Arc;
 
-use anyhow::Result;
 use tokio::{io::AsyncWriteExt, sync::Mutex};
 
 use self::util::{crc_itt16_false, AcknowledgeErr};
 
 use super::auv_control_board::util::{END_BYTE, ESCAPE_BYTE, START_BYTE};
+use super::control_board::Result;
 
 pub mod response;
 pub mod util;
diff --git a/src/comms/auv_control_board/response.rs b/src/comms/auv_control_board/response.rs
index 5d89d7e..85f9d89 100644
--- a/src/comms/auv_control_board/response.rs
+++ b/src/comms/auv_control_board/response.rs
@@ -194,7 +194,7 @@ mod tests {
             )
             .collect::<Vec<Vec<u8>>>()
             .await,
-            vec![vec![]]
+            Vec::<Vec<u8>>::new()
         );
 
         assert_eq!(
diff --git a/src/comms/auv_control_board/util.rs b/src/comms/auv_control_board/util.rs
index 1e4fc32..8e713cd 100644
--- a/src/comms/auv_control_board/util.rs
+++ b/src/comms/auv_control_board/util.rs
@@ -1,5 +1,5 @@
 // Implementing <https://mb3hel.github.io/AUVControlBoard/user_guide/comm_protocol/>
-use std::{error::Error, fmt::Display};
+use thiserror::Error;
 
 pub const START_BYTE: u8 = 253;
 pub const END_BYTE: u8 = 254;
@@ -38,23 +38,20 @@ pub fn crc_itt16_false(bytes: &[u8]) -> u16 {
     crc
 }
 
-#[derive(Debug)]
+#[derive(Error, Debug)]
 pub enum AcknowledgeErr {
+    #[error("unknown message")]
     UnknownMsg,
+    #[error("invalid arguments")]
     InvalidArguments,
+    #[error("invalid command")]
     InvalidCommand,
+    #[error("reserved")]
     Reserved,
+    #[error("undefined: '{0}'")]
     Undefined(u8),
 }
 
-impl Display for AcknowledgeErr {
-    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
-        write!(f, "{self:?}")
-    }
-}
-
-impl Error for AcknowledgeErr {}
-
 impl From<u8> for AcknowledgeErr {
     fn from(value: u8) -> Self {
         match value {
diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs
index 057dcb3..7366833 100644
--- a/src/comms/control_board/mod.rs
+++ b/src/comms/control_board/mod.rs
@@ -1,7 +1,7 @@
 use core::fmt::Debug;
 use std::{ops::Deref, sync::Arc, time::Duration};
 
-use anyhow::{anyhow, bail, Result};
+// use anyhow::{anyhow, bail, Result};
 use thiserror::Error;
 use tokio::{
     io::{self, AsyncRead, AsyncWrite, AsyncWriteExt, WriteHalf},
@@ -28,6 +28,26 @@ pub enum SensorStatuses {
     AllGood,
 }
 
+#[derive(Error, Debug)]
+pub enum ControlBoardError {
+    #[error("number of motor matrix rows and number of thruster inversions do not match: {rows:?} rows, {inversions:?} inversions")]
+    ThrusterMismatch { rows: u8, inversions: usize },
+    #[error("thruster index '{0}' is outside of the allowed range 1-8")]
+    ThrusterIndexing(u8),
+    #[error("failed to initialize serial comms with control board")]
+    InitSerial(#[from] tokio_serial::Error),
+    #[error("tcp connect failed")]
+    TcpConnect(#[from] io::Error),
+    #[error("failed to set initial yaw")]
+    InitialYawSet,
+    #[error("{0} is not a valid PID tune, pick from [X, Y, Z, D]")]
+    InvalidPidTuneAxis(char),
+    #[error("acknowledge failed")]
+    AcknowledgeErr(#[from] super::auv_control_board::util::AcknowledgeErr),
+}
+
+pub type Result<T, E = ControlBoardError> = core::result::Result<T, E>;
+
 pub static LAST_YAW: std::sync::Mutex<Option<f32>> = std::sync::Mutex::new(None);
 
 #[derive(Debug)]
@@ -60,7 +80,7 @@ impl VehicleDefinition {
         motor_matrix: MotorMatrix,
         thruster_inversions: ThrusterInversions,
         dof_speeds: DofSpeeds,
-    ) -> Result<Self, ControlBoardError> {
+    ) -> Result<Self> {
         use ControlBoardError::*;
 
         let rows = motor_matrix.0.iter().map(|r| r.is_some() as u8).sum();
@@ -165,18 +185,12 @@ impl From<[f32; 6]> for MotorMatrixRowParams {
     }
 }
 
-#[derive(Error, Debug)]
-pub enum ControlBoardError {
-    #[error("number of motor matrix rows and number of thruster inversions do not match: {rows:?} rows, {inversions:?} inversions")]
-    ThrusterMismatch { rows: u8, inversions: usize },
-}
-
 impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
     pub async fn new<U>(
         comm_out: T,
         comm_in: U,
         msg_id: Option<MessageId>,
-        vehicle_defintion: VehicleDefinition,
+        vehicle_defintion: &VehicleDefinition,
     ) -> Result<Self>
     where
         U: 'static + AsyncRead + Unpin + Send,
@@ -200,8 +214,8 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
         //     .set_row(8, MotorMatrixRowParams::new(0.0, 0.0, -1.0, -1.0, 1.0, 0.0))
         //     .build();
 
-        this.init_matrices(vehicle_defintion.motor_matrix).await?;
-        this.thruster_inversion_set(vehicle_defintion.thruster_inversions)
+        this.init_matrices(&vehicle_defintion.motor_matrix).await?;
+        this.thruster_inversion_set(&vehicle_defintion.thruster_inversions)
             .await?;
         this.relative_dof_speed_set_batch(&vehicle_defintion.dof_speeds)
             .await?;
@@ -244,7 +258,7 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
         Ok(this)
     }
 
-    async fn init_matrices(&self, motor_matrix: MotorMatrix) -> Result<()> {
+    async fn init_matrices(&self, motor_matrix: &MotorMatrix) -> Result<()> {
         for (i, _row) in motor_matrix.0.iter().enumerate() {
             // If the row is defined for the thruster, then set it
             if let Some(row) = _row {
@@ -269,7 +283,7 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
 }
 
 impl ControlBoard<WriteHalf<SerialStream>> {
-    pub async fn serial(port_name: &str, vehicle_defintion: VehicleDefinition) -> Result<Self> {
+    pub async fn serial(port_name: &str, vehicle_defintion: &VehicleDefinition) -> Result<Self> {
         const BAUD_RATE: u32 = 9600;
         const DATA_BITS: DataBits = DataBits::Eight;
         const PARITY: Parity = Parity::None;
@@ -307,7 +321,7 @@ impl ControlBoard<WriteHalf<TcpStream>> {
 
         let stream = TcpStream::connect(host.to_string() + ":" + port).await?;
         let (comm_in, comm_out) = io::split(stream);
-        Self::new(comm_out, comm_in, None, vehicle_defintion).await
+        Self::new(comm_out, comm_in, None, &vehicle_defintion).await
     }
 }
 
@@ -336,7 +350,7 @@ impl<T: AsyncWrite + Unpin> ControlBoard<T> {
         message.extend(MOTOR_MATRIX_SET);
 
         if !(1..=8).contains(&thruster) {
-            bail!("{thruster} is outside the allowed range 1-8.")
+            return Err(ControlBoardError::ThrusterIndexing(thruster));
         };
 
         message.extend(thruster.to_le_bytes());
@@ -356,7 +370,7 @@ impl<T: AsyncWrite + Unpin> ControlBoard<T> {
     ///
     /// # Arguments:
     /// * `inversions` - Array of invert statuses, with motor 1 at index 0
-    pub async fn thruster_inversion_set(&self, inversions: Vec<bool>) -> Result<()> {
+    pub async fn thruster_inversion_set(&self, inversions: &Vec<bool>) -> Result<()> {
         const THRUSTER_INVERSION_SET: [u8; 4] = *b"TINV";
         let mut message = Vec::from(THRUSTER_INVERSION_SET);
 
@@ -488,7 +502,7 @@ impl<T: AsyncWrite + Unpin> ControlBoard<T> {
             None => {
                 self.set_initial_angle().await?;
                 let angle =
-                    (*self.initial_angles.lock().await).ok_or(anyhow!("Initial Yaw set Error"))?;
+                    (*self.initial_angles.lock().await).ok_or(ControlBoardError::InitialYawSet)?;
                 *angle.yaw()
             }
         };
@@ -556,7 +570,7 @@ impl<T: AsyncWrite + Unpin> ControlBoard<T> {
         message.extend(STAB_TUNE);
 
         if !['X', 'Y', 'Z', 'D'].contains(&which) {
-            bail!("{which} is not a valid PID tune, pick from [X, Y, Z, D]")
+            return Err(ControlBoardError::InvalidPidTuneAxis(which));
         }
         message.push(which as u8);
 
diff --git a/src/comms/control_board/util.rs b/src/comms/control_board/util.rs
index f3b7686..98a13a4 100644
--- a/src/comms/control_board/util.rs
+++ b/src/comms/control_board/util.rs
@@ -1,6 +1,6 @@
 use std::f32::consts::PI;
 
-use anyhow::bail;
+use color_eyre::eyre::bail;
 use derive_getters::Getters;
 
 /// See <https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf>,
@@ -18,7 +18,7 @@ pub enum BNO055AxisConfig {
 }
 
 impl TryFrom<u8> for BNO055AxisConfig {
-    type Error = anyhow::Error;
+    type Error = color_eyre::eyre::Error;
 
     fn try_from(value: u8) -> Result<Self, Self::Error> {
         match value {
diff --git a/src/comms/meb/mod.rs b/src/comms/meb/mod.rs
index aa3298b..818da6e 100644
--- a/src/comms/meb/mod.rs
+++ b/src/comms/meb/mod.rs
@@ -1,6 +1,6 @@
 use std::sync::Arc;
 
-use anyhow::Result;
+use thiserror::Error;
 use tokio::{
     io::{AsyncReadExt, AsyncWrite, AsyncWriteExt, WriteHalf},
     sync::Mutex,
@@ -13,6 +13,16 @@ use super::auv_control_board::{AUVControlBoard, MessageId};
 
 pub mod response;
 
+#[derive(Error, Debug)]
+pub enum MebError {
+    #[error("failed to initialized serial comms with MEB")]
+    InitSerial(#[from] tokio_serial::Error),
+    #[error("sending message failed")]
+    SendMessage(#[from] super::control_board::ControlBoardError),
+}
+
+pub type Result<T, E = MebError> = core::result::Result<T, E>;
+
 #[derive(Debug)]
 pub struct MainElectronicsBoard<C: AsyncWrite + Unpin> {
     board: AUVControlBoard<C, Statuses>,
@@ -83,8 +93,8 @@ pub enum MebCmd {
 }
 
 impl<C: AsyncWriteExt + Unpin> MainElectronicsBoard<C> {
-    pub async fn send_msg(&self, cmd: MebCmd) -> anyhow::Result<()> {
+    pub async fn send_msg(&self, cmd: MebCmd) -> Result<()> {
         let formatted_cmd: [u8; 4] = [b'M', b'S', b'B', cmd as u8];
-        self.board.write_out_basic(formatted_cmd.to_vec()).await
+        Ok(self.board.write_out_basic(formatted_cmd.to_vec()).await?)
     }
 }
diff --git a/src/comms/zed_ros2.rs b/src/comms/zed_ros2.rs
index 8cd16c4..bc95d5f 100644
--- a/src/comms/zed_ros2.rs
+++ b/src/comms/zed_ros2.rs
@@ -1,4 +1,4 @@
-use anyhow::Result;
+use color_eyre::eyre::Result;
 use futures::StreamExt;
 use ros2_client::{
     Context, MessageTypeName, Name, NodeName, NodeOptions, DEFAULT_SUBSCRIPTION_QOS,
diff --git a/src/config/mod.rs b/src/config/mod.rs
index af1a1c4..45b940f 100644
--- a/src/config/mod.rs
+++ b/src/config/mod.rs
@@ -10,7 +10,7 @@ pub mod spin;
 use std::fs::read_to_string;
 
 use crate::vision::Yuv;
-use anyhow::Result;
+use color_eyre::eyre::Result;
 use serde::{Deserialize, Serialize};
 use std::collections::HashMap;
 use std::ops::RangeInclusive;
diff --git a/src/main.rs b/src/main.rs
index 04fb0fa..4a39554 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,4 +1,4 @@
-use anyhow::{bail, Result};
+use color_eyre::eyre::{bail, Result};
 use std::env::temp_dir;
 
 use std::env;
@@ -50,7 +50,7 @@ async fn config() -> &'static Config {
     CONFIG_CELL
         .get_or_init(|| async {
             Config::new().unwrap_or_else(|e| {
-                logln!("Error getting config file: {:#?}\nUsing default config", e);
+                logln!("Error getting config file: {e}\nUsing default config");
                 Config::default()
             })
         })
@@ -77,17 +77,22 @@ async fn control_board() -> &'static ControlBoard<WriteHalf<SerialStream>> {
                 [true, true, false, false, true, false, false, true].into(),
                 [0.7071, 0.7071, 1.0, 0.4413, 1.0, 0.8139],
             )
-            let board = ControlBoard::serial(config.control_board_path.as_str(), vehicle_def).await;
+            .inspect_err(|e| logln!("Invalid vehicle definition: {:#?}", e))
+            .unwrap();
+            let board =
+                ControlBoard::serial(config.control_board_path.as_str(), &vehicle_def).await;
             match board {
                 Ok(x) => x,
                 Err(e) => {
-                    logln!("Error initializing control board: {:#?}", e);
-                    let backup_board =
-                        ControlBoard::serial(config.control_board_backup_path.as_str())
-                            .await
-                            .unwrap();
+                    logln!("Error initializing control board: {e}");
+                    let backup_board = ControlBoard::serial(
+                        config.control_board_backup_path.as_str(),
+                        &vehicle_def,
+                    )
+                    .await
+                    .unwrap();
                     backup_board.reset().await.unwrap();
-                    ControlBoard::serial(config.control_board_path.as_str())
+                    ControlBoard::serial(config.control_board_path.as_str(), &vehicle_def)
                         .await
                         .unwrap()
                 }
@@ -166,6 +171,7 @@ static SHUTDOWN_GUARD: Semaphore = Semaphore::const_new(1);
 
 #[tokio::main]
 async fn main() {
+    color_eyre::install().unwrap();
     let (shutdown_tx, mission_ct) = shutdown_handler().await;
 
     let orig_hook = std::panic::take_hook();
diff --git a/src/missions/action.rs b/src/missions/action.rs
index 2e81436..7d1bb60 100644
--- a/src/missions/action.rs
+++ b/src/missions/action.rs
@@ -1,4 +1,4 @@
-use anyhow::Result;
+use color_eyre::eyre::Result;
 
 use core::fmt::Debug;
 use std::{marker::PhantomData, sync::Arc, thread};
diff --git a/src/missions/comms.rs b/src/missions/comms.rs
index 5321fb2..db9d167 100644
--- a/src/missions/comms.rs
+++ b/src/missions/comms.rs
@@ -1,4 +1,4 @@
-use anyhow::Result;
+use color_eyre::eyre::Result;
 use tokio::io::WriteHalf;
 use tokio_serial::SerialStream;
 
@@ -22,9 +22,10 @@ impl<T> Action for StartBno055<'_, T> {}
 
 impl<T: GetControlBoard<WriteHalf<SerialStream>>> ActionExec<Result<()>> for StartBno055<'_, T> {
     async fn execute(&mut self) -> Result<()> {
-        self.context
+        Ok(self
+            .context
             .get_control_board()
             .bno055_periodic_read(true)
-            .await
+            .await?)
     }
 }
diff --git a/src/missions/extra.rs b/src/missions/extra.rs
index 3146c95..4809d49 100644
--- a/src/missions/extra.rs
+++ b/src/missions/extra.rs
@@ -1,7 +1,7 @@
 use std::fmt::Debug;
 use std::marker::PhantomData;
 
-use anyhow::{anyhow, bail};
+use color_eyre::eyre::{anyhow, bail, Result};
 
 use crate::logln;
 
@@ -112,8 +112,8 @@ impl<T: Send + Sync> ActionMod<T> for AlwaysTrue {
     fn modify(&mut self, _input: &T) {}
 }
 
-impl ActionExec<anyhow::Result<()>> for AlwaysTrue {
-    async fn execute(&mut self) -> anyhow::Result<()> {
+impl ActionExec<Result<()>> for AlwaysTrue {
+    async fn execute(&mut self) -> Result<()> {
         Ok(())
     }
 }
@@ -159,8 +159,8 @@ impl<T: Send + Sync> ActionMod<T> for AlwaysFalse {
     fn modify(&mut self, _input: &T) {}
 }
 
-impl ActionExec<anyhow::Result<()>> for AlwaysFalse {
-    async fn execute(&mut self) -> anyhow::Result<()> {
+impl ActionExec<Result<()>> for AlwaysFalse {
+    async fn execute(&mut self) -> Result<()> {
         bail!("")
     }
 }
@@ -197,7 +197,7 @@ impl<T: ActionMod<U>, U: Send + Sync> ActionMod<U> for UnwrapAction<T> {
     }
 }
 
-impl<T: ActionExec<anyhow::Result<U>>, U: Send + Sync> ActionExec<U> for UnwrapAction<T> {
+impl<T: ActionExec<Result<U>>, U: Send + Sync> ActionExec<U> for UnwrapAction<T> {
     async fn execute(&mut self) -> U {
         self.action.execute().await.unwrap()
     }
@@ -217,8 +217,8 @@ impl CountTrue {
 
 impl Action for CountTrue {}
 
-impl<T: Send + Sync> ActionMod<anyhow::Result<T>> for CountTrue {
-    fn modify(&mut self, input: &anyhow::Result<T>) {
+impl<T: Send + Sync> ActionMod<Result<T>> for CountTrue {
+    fn modify(&mut self, input: &Result<T>) {
         if input.is_ok() {
             self.count += 1;
             if self.count > self.target {
@@ -259,8 +259,8 @@ impl ActionMod<bool> for CountTrue {
     }
 }
 
-impl ActionExec<anyhow::Result<()>> for CountTrue {
-    async fn execute(&mut self) -> anyhow::Result<()> {
+impl ActionExec<Result<()>> for CountTrue {
+    async fn execute(&mut self) -> Result<()> {
         logln!("Check true count: {} ? {}", self.count, self.target);
         if self.count < self.target {
             Ok(())
@@ -284,8 +284,8 @@ impl CountFalse {
 
 impl Action for CountFalse {}
 
-impl<T: Send + Sync> ActionMod<anyhow::Result<T>> for CountFalse {
-    fn modify(&mut self, input: &anyhow::Result<T>) {
+impl<T: Send + Sync> ActionMod<Result<T>> for CountFalse {
+    fn modify(&mut self, input: &Result<T>) {
         if input.is_err() {
             self.count += 1;
             if self.count > self.target {
@@ -326,8 +326,8 @@ impl ActionMod<bool> for CountFalse {
     }
 }
 
-impl ActionExec<anyhow::Result<()>> for CountFalse {
-    async fn execute(&mut self) -> anyhow::Result<()> {
+impl ActionExec<Result<()>> for CountFalse {
+    async fn execute(&mut self) -> Result<()> {
         logln!("Check false count: {} ? {}", self.count, self.target);
         if self.count < self.target {
             Ok(())
@@ -363,13 +363,10 @@ impl<T: ActionMod<V>, U: ActionMod<V>, V: Send + Sync> ActionMod<V> for InOrderF
     }
 }
 
-impl<
-        T: ActionExec<anyhow::Result<V>>,
-        U: ActionExec<anyhow::Result<V>>,
-        V: Send + Sync + Default,
-    > ActionExec<anyhow::Result<V>> for InOrderFail<T, U>
+impl<T: ActionExec<Result<V>>, U: ActionExec<Result<V>>, V: Send + Sync + Default>
+    ActionExec<Result<V>> for InOrderFail<T, U>
 {
-    async fn execute(&mut self) -> anyhow::Result<V> {
+    async fn execute(&mut self) -> Result<V> {
         if !self.finished_first {
             let ret = self.first.execute().await;
             self.finished_first = ret.is_err();
@@ -453,10 +450,10 @@ impl<T: Send + Sync + Debug, U: IntoIterator<Item = T> + Send + Sync + Clone> Ac
     }
 }
 
-impl<T: Send + Sync, U: IntoIterator<Item = T> + Send + Sync + Clone> ActionMod<anyhow::Result<U>>
+impl<T: Send + Sync, U: IntoIterator<Item = T> + Send + Sync + Clone> ActionMod<Result<U>>
     for ToVec<T>
 {
-    fn modify(&mut self, input: &anyhow::Result<U>) {
+    fn modify(&mut self, input: &Result<U>) {
         if let Ok(input) = input {
             self.value = input.clone().into_iter().collect();
         } else {
@@ -504,10 +501,10 @@ impl<T: Send + Sync + Debug, U: IntoIterator<Item = T> + Send + Sync + Clone> Ac
     }
 }
 
-impl<T: Send + Sync, U: IntoIterator<Item = T> + Send + Sync + Clone> ActionMod<anyhow::Result<U>>
+impl<T: Send + Sync, U: IntoIterator<Item = T> + Send + Sync + Clone> ActionMod<Result<U>>
     for IsSome<T>
 {
-    fn modify(&mut self, input: &anyhow::Result<U>) {
+    fn modify(&mut self, input: &Result<U>) {
         if let Ok(input) = input {
             self.value = input.clone().into_iter().collect();
         } else {
@@ -543,8 +540,8 @@ impl<T: Send + Sync> ActionMod<T> for AlwaysBetterTrue {
     fn modify(&mut self, _input: &T) {}
 }
 
-impl ActionExec<anyhow::Result<()>> for AlwaysBetterTrue {
-    async fn execute(&mut self) -> anyhow::Result<()> {
+impl ActionExec<Result<()>> for AlwaysBetterTrue {
+    async fn execute(&mut self) -> Result<()> {
         Ok(())
     }
 }
@@ -570,8 +567,8 @@ impl<T: Send + Sync> ActionMod<T> for AlwaysBetterFalse {
     fn modify(&mut self, _input: &T) {}
 }
 
-impl ActionExec<anyhow::Result<()>> for AlwaysBetterFalse {
-    async fn execute(&mut self) -> anyhow::Result<()> {
+impl ActionExec<Result<()>> for AlwaysBetterFalse {
+    async fn execute(&mut self) -> Result<()> {
         bail!("")
     }
 }
diff --git a/src/missions/movement.rs b/src/missions/movement.rs
index 10c143d..b65e728 100644
--- a/src/missions/movement.rs
+++ b/src/missions/movement.rs
@@ -6,7 +6,7 @@ use crate::vision::Offset2D;
 use crate::vision::RelPos;
 use crate::vision::RelPosAngle;
 
-use anyhow::Result;
+use color_eyre::eyre::Result;
 use core::fmt::Debug;
 use derive_getters::Getters;
 use num_traits::abs;
@@ -132,7 +132,7 @@ impl<T: GetControlBoard<WriteHalf<SerialStream>>> ActionExec<Result<()>>
             cur_angles = cntrl_board.responses().get_angles().await;
         }
 
-        cntrl_board
+        Ok(cntrl_board
             .stability_2_speed_set(
                 0.0,
                 speed,
@@ -141,7 +141,7 @@ impl<T: GetControlBoard<WriteHalf<SerialStream>>> ActionExec<Result<()>>
                 *cur_angles.unwrap().yaw(),
                 self.target_depth,
             )
-            .await
+            .await?)
     }
 }
 
@@ -163,10 +163,11 @@ impl<'a, T> ZeroMovement<'a, T> {
 
 impl<T: GetControlBoard<WriteHalf<SerialStream>>> ActionExec<Result<()>> for ZeroMovement<'_, T> {
     async fn execute(&mut self) -> Result<()> {
-        self.context
+        Ok(self
+            .context
             .get_control_board()
             .stability_2_speed_set_initial_yaw(0.0, 0.0, 0.0, 0.0, self.target_depth)
-            .await
+            .await?)
     }
 }
 
@@ -214,10 +215,11 @@ where
 
 impl<T: GetControlBoard<WriteHalf<SerialStream>>> ActionExec<Result<()>> for AdjustMovement<'_, T> {
     async fn execute(&mut self) -> Result<()> {
-        self.context
+        Ok(self
+            .context
             .get_control_board()
             .stability_2_speed_set_initial_yaw(self.x, 0.5, 0.0, 0.0, self.target_depth)
-            .await
+            .await?)
     }
 }
 
@@ -335,10 +337,11 @@ impl<T: GetControlBoard<WriteHalf<SerialStream>>> ActionExec<Result<()>>
         }
         logln!("Setting x to {x}");
 
-        self.context
+        Ok(self
+            .context
             .get_control_board()
             .stability_2_speed_set(x, 0.5, 0.0, 0.0, yaw, self.target_depth)
-            .await
+            .await?)
     }
 }
 
@@ -410,10 +413,11 @@ impl<T: GetControlBoard<WriteHalf<SerialStream>>> ActionExec<Result<()>> for Cen
             y = 0.0;
         }
 
-        self.context
+        Ok(self
+            .context
             .get_control_board()
             .stability_2_speed_set(x, y, 0.0, 0.0, yaw, self.target_depth)
-            .await
+            .await?)
     }
 }
 
@@ -570,7 +574,7 @@ impl Stability2Pos {
 
         //logln!("Stability 2 speed set: {:#?}", self);
 
-        board
+        Ok(board
             .stability_2_speed_set(
                 self.x,
                 self.y,
@@ -579,7 +583,7 @@ impl Stability2Pos {
                 self.target_yaw.unwrap(),
                 self.target_depth,
             )
-            .await
+            .await?)
     }
 
     /// Sets speed, bounded to [-1, 1]
@@ -1350,8 +1354,8 @@ impl<T: Send + Sync + Clone + Default> ActionMod<Option<T>> for OffsetToPose<T>
     }
 }
 
-impl<T: Send + Sync + Clone + Default> ActionMod<anyhow::Result<T>> for OffsetToPose<T> {
-    fn modify(&mut self, input: &anyhow::Result<T>) {
+impl<T: Send + Sync + Clone + Default> ActionMod<color_eyre::eyre::Result<T>> for OffsetToPose<T> {
+    fn modify(&mut self, input: &color_eyre::eyre::Result<T>) {
         if let Ok(input) = input {
             self.offset = input.clone();
         } else {
@@ -1417,8 +1421,8 @@ impl<T: Send + Sync + Clone + Default> ActionMod<Option<T>> for BoxToPose<T> {
     }
 }
 
-impl<T: Send + Sync + Clone + Default> ActionMod<anyhow::Result<T>> for BoxToPose<T> {
-    fn modify(&mut self, input: &anyhow::Result<T>) {
+impl<T: Send + Sync + Clone + Default> ActionMod<color_eyre::eyre::Result<T>> for BoxToPose<T> {
+    fn modify(&mut self, input: &color_eyre::eyre::Result<T>) {
         if let Ok(input) = input {
             self.input = input.clone();
         } else {
@@ -1575,7 +1579,7 @@ impl Stability1Pos {
     pub async fn exec(&mut self, board: &ControlBoard<WriteHalf<SerialStream>>) -> Result<()> {
         logln!("Stability 1 speed set: {:#?}", self);
 
-        board
+        Ok(board
             .stability_1_speed_set(
                 self.x,
                 self.y,
@@ -1584,7 +1588,7 @@ impl Stability1Pos {
                 self.yaw_speed,
                 self.target_depth,
             )
-            .await
+            .await?)
     }
 
     /// Sets speed, bounded to [-1, 1]
@@ -2450,7 +2454,7 @@ impl GlobalPos {
 
     /// Executes the position in stability assist
     pub async fn exec(&mut self, board: &ControlBoard<WriteHalf<SerialStream>>) -> Result<()> {
-        board
+        Ok(board
             .global_speed_set(
                 self.x,
                 self.y,
@@ -2459,7 +2463,7 @@ impl GlobalPos {
                 self.roll_speed,
                 self.yaw_speed,
             )
-            .await
+            .await?)
     }
 
     pub const fn const_default() -> Self {
diff --git a/src/missions/vision.rs b/src/missions/vision.rs
index d5f83e2..d83cbba 100644
--- a/src/missions/vision.rs
+++ b/src/missions/vision.rs
@@ -11,7 +11,7 @@ use crate::vision::{
     Angle2D, Draw, DrawRect2d, Offset2D, RelPos, RelPosAngle, VisualDetection, VisualDetector,
 };
 
-use anyhow::{anyhow, Result};
+use color_eyre::eyre::{anyhow, Result};
 use num_traits::{Float, FromPrimitive, Num};
 use opencv::core::{Mat, Rect2d};
 use uuid::Uuid;
@@ -783,9 +783,9 @@ impl<
 }
 
 impl<T: Display, U: Send + Sync + Clone, V: Send + Sync + Clone>
-    ActionMod<anyhow::Result<Vec<VisualDetection<U, V>>>> for DetectTarget<T, U, V>
+    ActionMod<Result<Vec<VisualDetection<U, V>>>> for DetectTarget<T, U, V>
 {
-    fn modify(&mut self, input: &anyhow::Result<Vec<VisualDetection<U, V>>>) {
+    fn modify(&mut self, input: &Result<Vec<VisualDetection<U, V>>>) {
         #[allow(clippy::all)]
         {
             self.results = input.as_ref().map(|valid| valid.clone()).ok()
@@ -846,8 +846,8 @@ impl<T: Send + Sync + Clone> ActionMod<Option<Vec<T>>> for Average<T> {
     }
 }
 
-impl<T: Send + Sync + Clone> ActionMod<anyhow::Result<Vec<T>>> for Average<T> {
-    fn modify(&mut self, input: &anyhow::Result<Vec<T>>) {
+impl<T: Send + Sync + Clone> ActionMod<Result<Vec<T>>> for Average<T> {
+    fn modify(&mut self, input: &Result<Vec<T>>) {
         if let Ok(input) = input {
             self.values.clone_from(input);
         } else {
@@ -991,8 +991,8 @@ impl<T: Send + Sync + Clone> ActionMod<Option<Vec<T>>> for MidPoint<T> {
     }
 }
 
-impl<T: Send + Sync + Clone> ActionMod<anyhow::Result<Vec<T>>> for MidPoint<T> {
-    fn modify(&mut self, input: &anyhow::Result<Vec<T>>) {
+impl<T: Send + Sync + Clone> ActionMod<Result<Vec<T>>> for MidPoint<T> {
+    fn modify(&mut self, input: &Result<Vec<T>>) {
         if let Ok(input) = input {
             self.values.clone_from(input);
         } else {
diff --git a/src/video_source/appsink.rs b/src/video_source/appsink.rs
index a261ed4..a158376 100644
--- a/src/video_source/appsink.rs
+++ b/src/video_source/appsink.rs
@@ -1,4 +1,4 @@
-use anyhow::{anyhow, Result};
+use color_eyre::eyre::{anyhow, Result};
 use opencv::{
     prelude::Mat,
     videoio::{VideoCapture, VideoCaptureAPIs, VideoCaptureTrait},
diff --git a/src/vision/bin.rs b/src/vision/bin.rs
index 8238cab..d3324fc 100644
--- a/src/vision/bin.rs
+++ b/src/vision/bin.rs
@@ -1,4 +1,4 @@
-use anyhow::Result;
+use color_eyre::eyre::Result;
 use derive_getters::Getters;
 use opencv::{core::Size, prelude::Mat};
 
diff --git a/src/vision/gate.rs b/src/vision/gate.rs
index f855ea5..a2b3c0d 100644
--- a/src/vision/gate.rs
+++ b/src/vision/gate.rs
@@ -1,4 +1,4 @@
-use anyhow::Result;
+use color_eyre::eyre::Result;
 use opencv::{core::Size, prelude::Mat};
 
 use crate::load_onnx;
diff --git a/src/vision/gate_cv.rs b/src/vision/gate_cv.rs
index fa0c77e..d327bbe 100644
--- a/src/vision/gate_cv.rs
+++ b/src/vision/gate_cv.rs
@@ -1,6 +1,7 @@
 use crate::config::ColorProfile;
 
 use super::{image_prep::resize, MatWrapper, PosVector, VisualDetection, VisualDetector, Yuv};
+use color_eyre::eyre::Result;
 use opencv::{
     core::{in_range, Point, Scalar, Size, Vector},
     imgproc::{
@@ -76,7 +77,7 @@ impl VisualDetector<f64> for GateCV {
     fn detect(
         &mut self,
         input_image: &Mat,
-    ) -> anyhow::Result<Vec<VisualDetection<Self::ClassEnum, Self::Position>>> {
+    ) -> Result<Vec<VisualDetection<Self::ClassEnum, Self::Position>>> {
         self.image = resize(input_image, &self.size)?.into();
         let mut yuv_image = Mat::default();
 
diff --git a/src/vision/gate_poles.rs b/src/vision/gate_poles.rs
index 39ac59d..8dd483b 100644
--- a/src/vision/gate_poles.rs
+++ b/src/vision/gate_poles.rs
@@ -1,4 +1,4 @@
-use anyhow::Result;
+use color_eyre::eyre::Result;
 use derive_getters::Getters;
 use opencv::core::{multiply, BORDER_CONSTANT, CV_8U};
 use opencv::imgproc::{dilate, morphology_default_border_value};
diff --git a/src/vision/image_prep.rs b/src/vision/image_prep.rs
index 411b3ac..f1643ce 100644
--- a/src/vision/image_prep.rs
+++ b/src/vision/image_prep.rs
@@ -11,7 +11,7 @@ use opencv::{
     prelude::{Mat, MatSizeTraitConst, MatTrait, MatTraitConst, MatTraitConstManual},
 };
 
-use anyhow::Result;
+use color_eyre::eyre::Result;
 
 /// Creates a new Mat with the specified size
 ///
diff --git a/src/vision/mod.rs b/src/vision/mod.rs
index a9e8c16..e65a119 100644
--- a/src/vision/mod.rs
+++ b/src/vision/mod.rs
@@ -1,4 +1,4 @@
-use anyhow::Result;
+use color_eyre::eyre::Result;
 use derive_getters::Getters;
 use itertools::Itertools;
 use num_traits::{zero, FromPrimitive, Num};
@@ -480,7 +480,7 @@ impl Mul<&Mat> for PosVector {
 }
 
 impl Draw for VisualDetection<bool, Offset2D<f64>> {
-    fn draw(&self, canvas: &mut Mat) -> anyhow::Result<()> {
+    fn draw(&self, canvas: &mut Mat) -> Result<()> {
         let color = if self.class {
             Scalar::from((0.0, 255.0, 0.0))
         } else {
diff --git a/src/vision/nn_cv2.rs b/src/vision/nn_cv2.rs
index 07ed464..fbdd1c0 100644
--- a/src/vision/nn_cv2.rs
+++ b/src/vision/nn_cv2.rs
@@ -1,4 +1,4 @@
-use anyhow::Result;
+use color_eyre::eyre::Result;
 use derive_getters::Getters;
 use opencv::{
     core::{Rect2d, Scalar, Size, VecN, Vector, CV_32F},
diff --git a/src/vision/octagon.rs b/src/vision/octagon.rs
index 327db21..87884b4 100644
--- a/src/vision/octagon.rs
+++ b/src/vision/octagon.rs
@@ -1,4 +1,5 @@
 use super::{image_prep::resize, MatWrapper, Offset2D, VisualDetection, VisualDetector};
+use color_eyre::eyre::Result;
 use opencv::{
     core::{in_range, Point, Size, VecN, Vector},
     imgproc::{find_contours, CHAIN_APPROX_SIMPLE, RETR_TREE},
@@ -90,7 +91,7 @@ impl VisualDetector<f64> for Octagon {
     fn detect(
         &mut self,
         input_image: &Mat,
-    ) -> anyhow::Result<Vec<VisualDetection<Self::ClassEnum, Self::Position>>> {
+    ) -> Result<Vec<VisualDetection<Self::ClassEnum, Self::Position>>> {
         let image = resize(input_image, &self.size)?;
         self.image.0 = image.clone();
 
diff --git a/src/vision/path_cv.rs b/src/vision/path_cv.rs
index e16334a..0846560 100644
--- a/src/vision/path_cv.rs
+++ b/src/vision/path_cv.rs
@@ -1,5 +1,6 @@
 use super::{image_prep::resize, MatWrapper, PosVector, VisualDetection, VisualDetector, Yuv};
 use crate::{config::ColorProfile, vision::Draw};
+use color_eyre::eyre::Result;
 use opencv::{
     core::{in_range, Point, Scalar, Size, Vector},
     imgproc::{
@@ -11,7 +12,7 @@ use opencv::{
 use std::ops::RangeInclusive;
 
 impl Draw for VisualDetection<bool, PosVector> {
-    fn draw(&self, canvas: &mut Mat) -> anyhow::Result<()> {
+    fn draw(&self, canvas: &mut Mat) -> Result<()> {
         let color = if self.class {
             logln!("Drawing true: {:#?}", self.position());
             Scalar::from((0.0, 255.0, 0.0))
@@ -106,7 +107,7 @@ impl VisualDetector<i32> for PathCV {
     fn detect(
         &mut self,
         input_image: &Mat,
-    ) -> anyhow::Result<Vec<VisualDetection<Self::ClassEnum, Self::Position>>> {
+    ) -> Result<Vec<VisualDetection<Self::ClassEnum, Self::Position>>> {
         self.image = resize(input_image, &self.size)?.into();
         let mut yuv_image = Mat::default();
 
@@ -215,7 +216,7 @@ impl VisualDetector<f64> for PathCV {
     fn detect(
         &mut self,
         input_image: &Mat,
-    ) -> anyhow::Result<Vec<VisualDetection<Self::ClassEnum, Self::Position>>> {
+    ) -> Result<Vec<VisualDetection<Self::ClassEnum, Self::Position>>> {
         self.image = resize(input_image, &self.size)?.into();
         let mut yuv_image = Mat::default();
 
diff --git a/src/vision/slalom.rs b/src/vision/slalom.rs
index 2eb2375..c0ac82e 100644
--- a/src/vision/slalom.rs
+++ b/src/vision/slalom.rs
@@ -1,6 +1,7 @@
 use crate::config::ColorProfile;
 
 use super::{image_prep::resize, MatWrapper, PosVector, VisualDetection, VisualDetector, Yuv};
+use color_eyre::eyre::Result;
 use opencv::{
     core::{in_range, Point, Scalar, Size, Vector},
     imgproc::{
@@ -71,7 +72,7 @@ impl VisualDetector<f64> for Slalom {
     fn detect(
         &mut self,
         input_image: &Mat,
-    ) -> anyhow::Result<Vec<VisualDetection<Self::ClassEnum, Self::Position>>> {
+    ) -> Result<Vec<VisualDetection<Self::ClassEnum, Self::Position>>> {
         let areas = self.area_bounds.clone();
         let min_area = areas.start();
         let max_area = areas.end();
diff --git a/src/vision/slalom_yolo.rs b/src/vision/slalom_yolo.rs
index e9a52c9..aba1280 100644
--- a/src/vision/slalom_yolo.rs
+++ b/src/vision/slalom_yolo.rs
@@ -1,4 +1,4 @@
-use anyhow::Result;
+use color_eyre::eyre::Result;
 use derive_getters::Getters;
 use opencv::{core::Size, prelude::Mat};
 
diff --git a/src/vision/yolo_model.rs b/src/vision/yolo_model.rs
index d85837a..af10278 100644
--- a/src/vision/yolo_model.rs
+++ b/src/vision/yolo_model.rs
@@ -7,7 +7,7 @@ use super::{
     nn_cv2::{YoloClass, YoloDetection},
     Draw, DrawRect2d, RelPos, VisualDetection, VisualDetector,
 };
-use anyhow::Result;
+use color_eyre::eyre::Result;
 use opencv::{
     core::{Point, Rect2d, Scalar, Size},
     imgproc::{self, LINE_AA},

From 4ebe14b16dcac8b843822bf53688372acb707bf8 Mon Sep 17 00:00:00 2001
From: Cowboylaserkittenjetshark
 <82691052+Cowboylaserkittenjetshark@users.noreply.github.com>
Date: Fri, 10 Apr 2026 18:16:27 -0400
Subject: [PATCH 08/11] wip: configurable pid params

---
 src/comms/auv_control_board/mod.rs            |  13 +-
 src/comms/auv_control_board/response.rs       |   1 +
 src/comms/auv_control_board/util.rs           |   2 +
 src/comms/control_board/mod.rs                | 188 +++---------------
 src/comms/control_board/response.rs           |   1 +
 src/comms/control_board/util.rs               |  38 ++++
 src/comms/control_board/vehicle_definition.rs | 158 +++++++++++++++
 src/comms/mod.rs                              |   1 +
 8 files changed, 236 insertions(+), 166 deletions(-)
 create mode 100644 src/comms/control_board/vehicle_definition.rs

diff --git a/src/comms/auv_control_board/mod.rs b/src/comms/auv_control_board/mod.rs
index c658e68..92bd26c 100644
--- a/src/comms/auv_control_board/mod.rs
+++ b/src/comms/auv_control_board/mod.rs
@@ -1,12 +1,17 @@
+//! Low-level implementation of the [AUVControlBoard] communication protocol
+//!
+//! The high-level messaging specification is implemented in the [control_board](crate::comms::control_board) module
+//!
+//! [AUVControlBoard]: https://github.com/ncsurobotics/AUVControlBoard
 use core::fmt::Debug;
 use std::sync::Arc;
 
 use tokio::{io::AsyncWriteExt, sync::Mutex};
 
-use self::util::{crc_itt16_false, AcknowledgeErr};
-
-use super::auv_control_board::util::{END_BYTE, ESCAPE_BYTE, START_BYTE};
-use super::control_board::Result;
+use crate::comms::{
+    auv_control_board::util::{crc_itt16_false, AcknowledgeErr, END_BYTE, ESCAPE_BYTE, START_BYTE},
+    control_board::util::Result,
+};
 
 pub mod response;
 pub mod util;
diff --git a/src/comms/auv_control_board/response.rs b/src/comms/auv_control_board/response.rs
index 85f9d89..02e3596 100644
--- a/src/comms/auv_control_board/response.rs
+++ b/src/comms/auv_control_board/response.rs
@@ -1,3 +1,4 @@
+//! Response parsing
 use bytes::BufMut;
 use tokio::io::AsyncReadExt;
 
diff --git a/src/comms/auv_control_board/util.rs b/src/comms/auv_control_board/util.rs
index 8e713cd..b571c62 100644
--- a/src/comms/auv_control_board/util.rs
+++ b/src/comms/auv_control_board/util.rs
@@ -1,3 +1,5 @@
+//! Helper utilities for communicating with the control board
+//!
 // Implementing <https://mb3hel.github.io/AUVControlBoard/user_guide/comm_protocol/>
 use thiserror::Error;
 
diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs
index 7366833..e5fe4a1 100644
--- a/src/comms/control_board/mod.rs
+++ b/src/comms/control_board/mod.rs
@@ -1,8 +1,11 @@
+//! High-level implementation of the [AUVControlBoard] messaging specification
+//!
+//! The underlying communication protocol is implemented in the [auv_control_board](crate::comms::auv_control_board) module
+//!
+//! [AUVControlBoard]: https://github.com/ncsurobotics/AUVControlBoard
 use core::fmt::Debug;
 use std::{ops::Deref, sync::Arc, time::Duration};
 
-// use anyhow::{anyhow, bail, Result};
-use thiserror::Error;
 use tokio::{
     io::{self, AsyncRead, AsyncWrite, AsyncWriteExt, WriteHalf},
     net::TcpStream,
@@ -13,7 +16,8 @@ use tokio_serial::{DataBits, Parity, SerialStream, StopBits};
 
 use self::{
     response::ResponseMap,
-    util::{Angles, BNO055AxisConfig},
+    util::{Angles, BNO055AxisConfig, ControlBoardError, Result},
+    vehicle_definition::{MotorMatrix, PidAxes, VehicleDefinition},
 };
 
 use super::auv_control_board::{AUVControlBoard, MessageId};
@@ -21,35 +25,22 @@ use crate::logln;
 
 pub mod response;
 pub mod util;
+pub mod vehicle_definition;
 
+/// Status of the control board's sensors
 pub enum SensorStatuses {
+    /// IMU ready
     ImuNr,
+    /// Depth sensor ready
     DepthNr,
+    /// Sensors healthy
     AllGood,
 }
 
-#[derive(Error, Debug)]
-pub enum ControlBoardError {
-    #[error("number of motor matrix rows and number of thruster inversions do not match: {rows:?} rows, {inversions:?} inversions")]
-    ThrusterMismatch { rows: u8, inversions: usize },
-    #[error("thruster index '{0}' is outside of the allowed range 1-8")]
-    ThrusterIndexing(u8),
-    #[error("failed to initialize serial comms with control board")]
-    InitSerial(#[from] tokio_serial::Error),
-    #[error("tcp connect failed")]
-    TcpConnect(#[from] io::Error),
-    #[error("failed to set initial yaw")]
-    InitialYawSet,
-    #[error("{0} is not a valid PID tune, pick from [X, Y, Z, D]")]
-    InvalidPidTuneAxis(char),
-    #[error("acknowledge failed")]
-    AcknowledgeErr(#[from] super::auv_control_board::util::AcknowledgeErr),
-}
-
-pub type Result<T, E = ControlBoardError> = core::result::Result<T, E>;
-
+/// The last yaw reported by the control board
 pub static LAST_YAW: std::sync::Mutex<Option<f32>> = std::sync::Mutex::new(None);
 
+/// Represents a control board
 #[derive(Debug)]
 pub struct ControlBoard<T>
 where
@@ -66,125 +57,6 @@ impl<T: AsyncWriteExt + Unpin> Deref for ControlBoard<T> {
     }
 }
 
-type ThrusterInversions = Vec<bool>;
-type DofSpeeds = [f32; 6];
-
-pub struct VehicleDefinition {
-    pub(crate) motor_matrix: MotorMatrix,
-    pub(crate) thruster_inversions: ThrusterInversions,
-    pub(crate) dof_speeds: DofSpeeds,
-}
-
-impl VehicleDefinition {
-    pub fn new(
-        motor_matrix: MotorMatrix,
-        thruster_inversions: ThrusterInversions,
-        dof_speeds: DofSpeeds,
-    ) -> Result<Self> {
-        use ControlBoardError::*;
-
-        let rows = motor_matrix.0.iter().map(|r| r.is_some() as u8).sum();
-        let inversions = thruster_inversions.len();
-
-        if rows != inversions as u8 {
-            Err(ThrusterMismatch { rows, inversions })
-        } else {
-            Ok(Self {
-                motor_matrix,
-                thruster_inversions,
-                dof_speeds,
-            })
-        }
-    }
-}
-
-/// Contains a *populated* motor matrix.
-/// Can only be created using [`MotorMatrixBuilder`].
-#[derive(Debug)]
-pub struct MotorMatrix(pub(crate) [Option<MotorMatrixRowParams>; 8]);
-
-impl MotorMatrix {
-    /// Creates a new [`MotorMatrixBuilder`]
-    pub fn builder(thrusters_in_use: u8) -> MotorMatrixBuilder {
-        MotorMatrixBuilder::new(thrusters_in_use)
-    }
-}
-
-/// A builder for MotorMatrix. This struct makes it cleaner to define thruster parameters,
-/// as well as validate that the correct amount of thrusters have a definition.
-#[derive(Debug)]
-pub struct MotorMatrixBuilder {
-    pub(crate) params: [Option<MotorMatrixRowParams>; 8],
-    pub(crate) thrusters_in_use: u8,
-}
-
-impl MotorMatrixBuilder {
-    /// Creates an unpopulated builder.
-    pub fn new(thrusters_in_use: u8) -> Self {
-        MotorMatrixBuilder {
-            params: [None, None, None, None, None, None, None, None],
-            thrusters_in_use,
-        }
-    }
-
-    /// Sets the parameters for a specific thruster.
-    pub fn set_row(mut self, thruster: u8, parameters: MotorMatrixRowParams) -> Self {
-        let thruster_index = thruster - 1;
-        self.params[thruster_index as usize] = Some(parameters);
-        self
-    }
-
-    /// Build the populated [`MotorMatrix`]
-    pub fn build(self) -> MotorMatrix {
-        // Make sure that we currently have parameters for `thrusters_in_use` thrusters.
-        let thrusters_defined = self.params.iter().filter(|x| x.is_some()).count();
-
-        assert_eq!(thrusters_defined, self.thrusters_in_use as usize);
-
-        // Make sure that all of the arrays contain `Some()`
-        MotorMatrix(self.params)
-    }
-}
-
-/// Each row corresponds to a line of self.motor_matrix_set(...);
-/// Can either be created manually, or the parameters can be dumped into [`Self::new`]
-/// for shorter code.
-#[derive(Debug)]
-pub struct MotorMatrixRowParams {
-    x: f32,
-    y: f32,
-    z: f32,
-    pitch: f32,
-    roll: f32,
-    yaw: f32,
-}
-
-impl MotorMatrixRowParams {
-    pub fn new(x: f32, y: f32, z: f32, pitch: f32, roll: f32, yaw: f32) -> Self {
-        Self {
-            x,
-            y,
-            z,
-            pitch,
-            roll,
-            yaw,
-        }
-    }
-}
-
-impl From<[f32; 6]> for MotorMatrixRowParams {
-    fn from(vals: [f32; 6]) -> Self {
-        Self {
-            x: vals[0],
-            y: vals[1],
-            z: vals[2],
-            pitch: vals[3],
-            roll: vals[4],
-            yaw: vals[5],
-        }
-    }
-}
-
 impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
     pub async fn new<U>(
         comm_out: T,
@@ -202,18 +74,6 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
             initial_angles: Arc::default(),
         };
 
-        // const THRUSTER_COUNT: u8 = 8;
-        // let motor_matrix = MotorMatrixBuilder::new(THRUSTER_COUNT)
-        //     .set_row(1, MotorMatrixRowParams::new(-1.0, 1.0, 0.0, 0.0, 0.0, -1.0))
-        //     .set_row(2, MotorMatrixRowParams::new(1.0, 1.0, 0.0, 0.0, 0.0, 1.0))
-        //     .set_row(3, MotorMatrixRowParams::new(-1.0, -1.0, 0.0, 0.0, 0.0, 1.0))
-        //     .set_row(4, MotorMatrixRowParams::new(1.0, -1.0, 0.0, 0.0, 0.0, -1.0))
-        //     .set_row(5, MotorMatrixRowParams::new(0.0, 0.0, -1.0, 1.0, -1.0, 0.0))
-        //     .set_row(6, MotorMatrixRowParams::new(0.0, 0.0, -1.0, 1.0, 1.0, 0.0))
-        //     .set_row(7, MotorMatrixRowParams::new(0.0, 0.0, -1.0, -1.0, -1.0, 0.0))
-        //     .set_row(8, MotorMatrixRowParams::new(0.0, 0.0, -1.0, -1.0, 1.0, 0.0))
-        //     .build();
-
         this.init_matrices(&vehicle_defintion.motor_matrix).await?;
         this.thruster_inversion_set(&vehicle_defintion.thruster_inversions)
             .await?;
@@ -270,15 +130,19 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
         self.motor_matrix_update().await
     }
 
-    async fn stab_tune(&self) -> Result<()> {
-        self.stability_assist_pid_tune('X', 0.8, 0.0, 0.0, 0.6, false)
-            .await?;
-        self.stability_assist_pid_tune('Y', 2.0, 0.0, 0.0, 0.1, false)
-            .await?;
-        self.stability_assist_pid_tune('Z', 4.0, 0.0, 0.0, 1.0, false)
+    async fn stab_tune(&self, axes: PidAxes) -> Result<()> {
+        for axis in axes {
+            self.stability_assist_pid_tune(
+                axis.which,
+                axis.kp,
+                axis.ki,
+                axis.kd,
+                axis.limit,
+                axis.invert,
+            )
             .await?;
-        self.stability_assist_pid_tune('D', 1.5, 0.0, 0.0, 1.0, false)
-            .await
+        }
+        Ok(())
     }
 }
 
diff --git a/src/comms/control_board/response.rs b/src/comms/control_board/response.rs
index 6264836..90a5394 100644
--- a/src/comms/control_board/response.rs
+++ b/src/comms/control_board/response.rs
@@ -1,3 +1,4 @@
+//! Response parsing
 use std::{
     collections::HashMap,
     sync::{
diff --git a/src/comms/control_board/util.rs b/src/comms/control_board/util.rs
index 98a13a4..bb5a7d4 100644
--- a/src/comms/control_board/util.rs
+++ b/src/comms/control_board/util.rs
@@ -1,7 +1,45 @@
+//! Utilities for parsing interacting with the control board
 use std::f32::consts::PI;
 
 use color_eyre::eyre::bail;
 use derive_getters::Getters;
+use thiserror::Error;
+
+/// An error occuring trying to communicate with the control board.
+#[derive(Error, Debug)]
+pub enum ControlBoardError {
+    /// Tried passing a malformed [vehicle definition](VehicleDefinition).
+    ///
+    /// The number of thrusters in the [motor matrix](MotorMatrix) and [thruster inversions vector](ThrusterInversions) do not match.
+    #[error("number of motor matrix rows and number of thruster inversions do not match: {rows:?} rows, {inversions:?} inversions")]
+    ThrusterMismatch { rows: u8, inversions: usize },
+
+    /// Tried passing a thruster index outside of the supported range, 1-8 inclusive.
+    #[error("thruster index '{0}' is outside of the allowed range 1-8")]
+    ThrusterIndexing(u8),
+
+    /// An error occured while initializing comms with a control board through the serial backend.
+    #[error("failed to initialize serial comms with control board")]
+    InitSerial(#[from] tokio_serial::Error),
+
+    /// An error occured while initializing comms with a control board through the TCP backend.
+    #[error("tcp connect failed")]
+    TcpConnect(#[from] tokio::io::Error),
+
+    /// An error occured getting the initial yaw.
+    #[error("failed to set initial yaw")]
+    InitialYawSet,
+
+    /// Tried tuning PID parameters for an axis that is not X, Y, Z, or D.
+    #[error("{0} is not a valid PID tune, pick from [X, Y, Z, D]")]
+    InvalidPidTuneAxis(char),
+
+    /// Received an error from the control board itself.
+    #[error("acknowledge failed")]
+    AcknowledgeErr(#[from] crate::comms::auv_control_board::util::AcknowledgeErr),
+}
+
+pub type Result<T, E = ControlBoardError> = core::result::Result<T, E>;
 
 /// See <https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf>,
 /// page 25
diff --git a/src/comms/control_board/vehicle_definition.rs b/src/comms/control_board/vehicle_definition.rs
new file mode 100644
index 0000000..a60930c
--- /dev/null
+++ b/src/comms/control_board/vehicle_definition.rs
@@ -0,0 +1,158 @@
+//! Tools for describing the vehicle to the control board
+
+use crate::comms::control_board::util::{ControlBoardError, Result};
+
+/// Thruster inversions used to invert the positive and negative direction of thrusters.
+///
+/// If `true`, the thruster of the corresponding index will be inverted
+pub type ThrusterInversions = Vec<bool>;
+
+/// Used to set relative speeds of motion in each Degree of Freedom (DoF).
+///
+/// There are two groups: linear (x, y, z) and angular (xrot, yrot, zrot). Expected in the format `[x, y, z, xrot, yrot, zrot]`.
+///
+/// Within each group, use 1.0 for the fastest DoF. Other DoFs in the group are percentages of the fastest speed (from 0.0 to 1.0).
+pub type DofSpeeds = [f32; 6];
+
+/// Contains all parameters needed to describe a vehicle to the control board.
+pub struct VehicleDefinition {
+    pub(crate) motor_matrix: MotorMatrix,
+    pub(crate) thruster_inversions: ThrusterInversions,
+    pub(crate) dof_speeds: DofSpeeds,
+    pub(crate) pid_axes: PidAxes,
+}
+
+impl VehicleDefinition {
+    /// Contructs a new populated `VehicleDefinition`
+    pub fn new(
+        motor_matrix: MotorMatrix,
+        thruster_inversions: ThrusterInversions,
+        dof_speeds: DofSpeeds,
+        pid_axes: PidAxes,
+    ) -> Result<Self> {
+        let rows = motor_matrix.0.iter().map(|r| r.is_some() as u8).sum();
+        let inversions = thruster_inversions.len();
+
+        if rows != inversions as u8 {
+            Err(ControlBoardError::ThrusterMismatch { rows, inversions })
+        } else {
+            Ok(Self {
+                motor_matrix,
+                thruster_inversions,
+                dof_speeds,
+                pid_axes,
+            })
+        }
+    }
+}
+
+/// Contains a *populated* motor matrix.
+/// Can only be created using [`MotorMatrixBuilder`].
+#[derive(Debug)]
+pub struct MotorMatrix(pub(crate) [Option<MotorMatrixRowParams>; 8]);
+
+impl MotorMatrix {
+    /// Creates a new [`MotorMatrixBuilder`]
+    pub fn builder(thrusters_in_use: u8) -> MotorMatrixBuilder {
+        MotorMatrixBuilder::new(thrusters_in_use)
+    }
+}
+
+/// A builder for [`MotorMatrix`]. This struct makes it cleaner to define thruster parameters,
+/// as well as validate that the correct amount of thrusters have a definition.
+#[derive(Debug)]
+pub struct MotorMatrixBuilder {
+    pub(crate) params: [Option<MotorMatrixRowParams>; 8],
+    pub(crate) thrusters_in_use: u8,
+}
+
+impl MotorMatrixBuilder {
+    /// Creates an unpopulated builder.
+    pub fn new(thrusters_in_use: u8) -> Self {
+        MotorMatrixBuilder {
+            params: [None, None, None, None, None, None, None, None],
+            thrusters_in_use,
+        }
+    }
+
+    /// Sets the parameters for a specific thruster.
+    pub fn set_row(mut self, thruster: u8, parameters: MotorMatrixRowParams) -> Self {
+        let thruster_index = thruster - 1;
+        self.params[thruster_index as usize] = Some(parameters);
+        self
+    }
+
+    /// Build the populated [`MotorMatrix`]
+    pub fn build(self) -> MotorMatrix {
+        // Make sure that we currently have parameters for `thrusters_in_use` thrusters.
+        let thrusters_defined = self.params.iter().filter(|x| x.is_some()).count();
+
+        assert_eq!(thrusters_defined, self.thrusters_in_use as usize);
+
+        // Make sure that all of the arrays contain `Some()`
+        MotorMatrix(self.params)
+    }
+}
+
+/// Each row corresponds to a line of self.motor_matrix_set(...);
+/// Can either be created manually, or the parameters can be dumped into [`Self::new`]
+/// for shorter code.
+#[derive(Debug)]
+pub struct MotorMatrixRowParams {
+    pub(crate) x: f32,
+    pub(crate) y: f32,
+    pub(crate) z: f32,
+    pub(crate) pitch: f32,
+    pub(crate) roll: f32,
+    pub(crate) yaw: f32,
+}
+
+impl MotorMatrixRowParams {
+    pub fn new(x: f32, y: f32, z: f32, pitch: f32, roll: f32, yaw: f32) -> Self {
+        Self {
+            x,
+            y,
+            z,
+            pitch,
+            roll,
+            yaw,
+        }
+    }
+}
+
+impl From<[f32; 6]> for MotorMatrixRowParams {
+    fn from(vals: [f32; 6]) -> Self {
+        Self {
+            x: vals[0],
+            y: vals[1],
+            z: vals[2],
+            pitch: vals[3],
+            roll: vals[4],
+            yaw: vals[5],
+        }
+    }
+}
+
+pub type PidAxes = [PidAxis; 4];
+
+pub struct PidAxis {
+    pub(crate) which: char,
+    pub(crate) kp: f32,
+    pub(crate) ki: f32,
+    pub(crate) kd: f32,
+    pub(crate) limit: f32,
+    pub(crate) invert: bool,
+}
+
+impl From<(char, f32, f32, f32, f32, bool)> for PidAxis {
+    fn from(value: (char, f32, f32, f32, f32, bool)) -> Self {
+        Self {
+            which: value.0,
+            kp: value.1,
+            ki: value.2,
+            kd: value.3,
+            limit: value.4,
+            invert: value.5,
+        }
+    }
+}
diff --git a/src/comms/mod.rs b/src/comms/mod.rs
index 5f015ad..696202f 100644
--- a/src/comms/mod.rs
+++ b/src/comms/mod.rs
@@ -1,3 +1,4 @@
+//! Modules for communicating with varius subsystems
 pub mod auv_control_board;
 pub mod control_board;
 #[cfg(feature = "meb")]

From e99b18e6371611d5a9b70256ec51af4c2c7bd5eb Mon Sep 17 00:00:00 2001
From: Cowboylaserkittenjetshark
 <82691052+Cowboylaserkittenjetshark@users.noreply.github.com>
Date: Fri, 10 Apr 2026 20:26:53 -0400
Subject: [PATCH 09/11] feat: complete configurable pid docs wip

---
 src/comms/control_board/mod.rs |  4 ++--
 src/comms/meb/mod.rs           |  4 +++-
 src/main.rs                    | 11 ++++++++++-
 3 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs
index e5fe4a1..8cc7d51 100644
--- a/src/comms/control_board/mod.rs
+++ b/src/comms/control_board/mod.rs
@@ -91,7 +91,7 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
         // Control board needs time to get its life together
         sleep(Duration::from_secs(5)).await;
 
-        this.stab_tune().await?;
+        this.stab_tune(&vehicle_defintion.pid_axes).await?;
 
         let inner_clone = this.inner.clone();
 
@@ -130,7 +130,7 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
         self.motor_matrix_update().await
     }
 
-    async fn stab_tune(&self, axes: PidAxes) -> Result<()> {
+    async fn stab_tune(&self, axes: &PidAxes) -> Result<()> {
         for axis in axes {
             self.stability_assist_pid_tune(
                 axis.which,
diff --git a/src/comms/meb/mod.rs b/src/comms/meb/mod.rs
index 818da6e..e2b8d8d 100644
--- a/src/comms/meb/mod.rs
+++ b/src/comms/meb/mod.rs
@@ -7,6 +7,8 @@ use tokio::{
 };
 use tokio_serial::{DataBits, Parity, SerialStream, StopBits};
 
+use crate::comms::control_board::util::ControlBoardError;
+
 use self::response::Statuses;
 
 use super::auv_control_board::{AUVControlBoard, MessageId};
@@ -18,7 +20,7 @@ pub enum MebError {
     #[error("failed to initialized serial comms with MEB")]
     InitSerial(#[from] tokio_serial::Error),
     #[error("sending message failed")]
-    SendMessage(#[from] super::control_board::ControlBoardError),
+    SendMessage(#[from] ControlBoardError),
 }
 
 pub type Result<T, E = MebError> = core::result::Result<T, E>;
diff --git a/src/main.rs b/src/main.rs
index 4a39554..6e25987 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -6,7 +6,10 @@ use std::process::exit;
 use std::time::Duration;
 use sw9s_lib::{
     comms::{
-        control_board::{ControlBoard, MotorMatrix, SensorStatuses, VehicleDefinition},
+        control_board::{
+            vehicle_definition::{MotorMatrix, VehicleDefinition},
+            ControlBoard, SensorStatuses,
+        },
         meb::MainElectronicsBoard,
         zed_ros2::ZedRos2,
     },
@@ -76,6 +79,12 @@ async fn control_board() -> &'static ControlBoard<WriteHalf<SerialStream>> {
                 motor_matrix,
                 [true, true, false, false, true, false, false, true].into(),
                 [0.7071, 0.7071, 1.0, 0.4413, 1.0, 0.8139],
+                [
+                    ('X', 0.8, 0.0, 0.0, 0.6, false).into(),
+                    ('Y', 2.0, 0.0, 0.0, 0.1, false).into(),
+                    ('Z', 4.0, 0.0, 0.0, 1.0, false).into(),
+                    ('D', 1.5, 0.0, 0.0, 1.0, false).into(),
+                ],
             )
             .inspect_err(|e| logln!("Invalid vehicle definition: {:#?}", e))
             .unwrap();

From 12dfbeed6f6fbaafb5fefb85103cb66afb9540d8 Mon Sep 17 00:00:00 2001
From: Cowboylaserkittenjetshark
 <82691052+Cowboylaserkittenjetshark@users.noreply.github.com>
Date: Fri, 10 Apr 2026 22:19:55 -0400
Subject: [PATCH 10/11] docs: document comms and config modules

---
 src/comms/auv_control_board/mod.rs |  1 +
 src/comms/control_board/mod.rs     |  1 +
 src/comms/control_board/util.rs    |  4 ++--
 src/comms/meb/mod.rs               | 19 +++++++++++++++++++
 src/comms/meb/response.rs          |  3 ++-
 src/comms/mod.rs                   |  3 ++-
 src/comms/zed_ros2.rs              |  4 ++++
 src/config/bin.rs                  |  3 +++
 src/config/coinflip.rs             |  3 +++
 src/config/gate.rs                 |  3 +++
 src/config/mod.rs                  | 30 ++++++++++++++++++++++++++++--
 src/config/octagon.rs              |  3 +++
 src/config/path_align.rs           |  3 +++
 src/config/slalom.rs               |  3 +++
 src/config/sonar.rs                |  3 +++
 src/lib.rs                         |  2 ++
 16 files changed, 82 insertions(+), 6 deletions(-)

diff --git a/src/comms/auv_control_board/mod.rs b/src/comms/auv_control_board/mod.rs
index 92bd26c..9dc5804 100644
--- a/src/comms/auv_control_board/mod.rs
+++ b/src/comms/auv_control_board/mod.rs
@@ -3,6 +3,7 @@
 //! The high-level messaging specification is implemented in the [control_board](crate::comms::control_board) module
 //!
 //! [AUVControlBoard]: https://github.com/ncsurobotics/AUVControlBoard
+
 use core::fmt::Debug;
 use std::sync::Arc;
 
diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs
index 8cc7d51..3b1fdd9 100644
--- a/src/comms/control_board/mod.rs
+++ b/src/comms/control_board/mod.rs
@@ -3,6 +3,7 @@
 //! The underlying communication protocol is implemented in the [auv_control_board](crate::comms::auv_control_board) module
 //!
 //! [AUVControlBoard]: https://github.com/ncsurobotics/AUVControlBoard
+
 use core::fmt::Debug;
 use std::{ops::Deref, sync::Arc, time::Duration};
 
diff --git a/src/comms/control_board/util.rs b/src/comms/control_board/util.rs
index bb5a7d4..b508549 100644
--- a/src/comms/control_board/util.rs
+++ b/src/comms/control_board/util.rs
@@ -8,9 +8,9 @@ use thiserror::Error;
 /// An error occuring trying to communicate with the control board.
 #[derive(Error, Debug)]
 pub enum ControlBoardError {
-    /// Tried passing a malformed [vehicle definition](VehicleDefinition).
+    /// Tried passing a malformed [vehicle definition](super::vehicle_definition::VehicleDefinition).
     ///
-    /// The number of thrusters in the [motor matrix](MotorMatrix) and [thruster inversions vector](ThrusterInversions) do not match.
+    /// The number of thrusters in the [motor matrix](super::vehicle_definition::MotorMatrix) and [thruster inversions vector](super::vehicle_definition::ThrusterInversions) do not match.
     #[error("number of motor matrix rows and number of thruster inversions do not match: {rows:?} rows, {inversions:?} inversions")]
     ThrusterMismatch { rows: u8, inversions: usize },
 
diff --git a/src/comms/meb/mod.rs b/src/comms/meb/mod.rs
index e2b8d8d..d53dc7b 100644
--- a/src/comms/meb/mod.rs
+++ b/src/comms/meb/mod.rs
@@ -1,3 +1,5 @@
+//! Implements communication protocol for the main electronics board (MEB).
+
 use std::sync::Arc;
 
 use thiserror::Error;
@@ -31,6 +33,7 @@ pub struct MainElectronicsBoard<C: AsyncWrite + Unpin> {
 }
 
 impl<C: AsyncWrite + Unpin> MainElectronicsBoard<C> {
+    /// Returns a MEB instance using the given IO backend.
     pub async fn new<T>(read_connection: T, write_connection: C) -> Self
     where
         T: 'static + AsyncReadExt + Unpin + Send,
@@ -44,6 +47,11 @@ impl<C: AsyncWrite + Unpin> MainElectronicsBoard<C> {
         }
     }
 
+    /// Returns a MEB instance using the serial backend.
+    ///
+    /// # Errors
+    ///
+    /// This function will return an error if the serial port cannot be opened.
     pub async fn serial(port_name: &str) -> Result<MainElectronicsBoard<WriteHalf<SerialStream>>> {
         const BAUD_RATE: u32 = 57600;
         const DATA_BITS: DataBits = DataBits::Eight;
@@ -60,26 +68,32 @@ impl<C: AsyncWrite + Unpin> MainElectronicsBoard<C> {
 }
 
 impl<C: AsyncWrite + Unpin> MainElectronicsBoard<C> {
+    /// Returns the temperature sensor value of this [`MainElectronicsBoard<C>`].
     pub async fn temperature(&self) -> Option<f32> {
         (*self.board.responses().temp().read().await).map(f32::from_le_bytes)
     }
 
+    /// Returns the humidity sensor value of this [`MainElectronicsBoard<C>`].
     pub async fn humidity(&self) -> Option<f32> {
         (*self.board.responses().humid().read().await).map(f32::from_le_bytes)
     }
 
+    /// Returns the leak sensor value of this [`MainElectronicsBoard<C>`].
     pub async fn leak(&self) -> Option<bool> {
         *self.board.responses().leak().read().await
     }
 
+    /// Returns the thruster arm state of this [`MainElectronicsBoard<C>`].
     pub async fn thruster_arm(&self) -> Option<bool> {
         *self.board.responses().thruster_arm().read().await
     }
 
+    /// Returns the system voltage of this [`MainElectronicsBoard<C>`].
     pub async fn system_voltage(&self) -> Option<f32> {
         (*self.board.responses().system_voltage().read().await).map(f32::from_le_bytes)
     }
 
+    /// Returns the shutdown cause of this [`MainElectronicsBoard<C>`].
     pub async fn shutdown_cause(&self) -> Option<u8> {
         *self.board.responses().shutdown().read().await
     }
@@ -87,10 +101,15 @@ impl<C: AsyncWrite + Unpin> MainElectronicsBoard<C> {
 
 #[derive(Debug, Copy, Clone)]
 pub enum MebCmd {
+    /// Trigger torpedo 1
     T1Trig = 0x3,
+    /// Trigger torpedo 2
     T2Trig = 0x4,
+    /// Trigger dropper 1
     D1Trig = 0x1,
+    /// Trigger dropper 2
     D2Trig = 0x2,
+    /// Reset relevant peripheral
     Reset = 0x0,
 }
 
diff --git a/src/comms/meb/response.rs b/src/comms/meb/response.rs
index 3c99a48..6f9aa5c 100644
--- a/src/comms/meb/response.rs
+++ b/src/comms/meb/response.rs
@@ -1,3 +1,5 @@
+//! Response parsing and handling
+
 use std::sync::{
     mpsc::{channel, Sender, TryRecvError},
     Arc,
@@ -64,7 +66,6 @@ impl Statuses {
         let shutdown: Lock<_> = Arc::default();
         let ack_map: Arc<Mutex<KeyedAcknowledges>> = Arc::default();
         let (_tx, rx) = channel::<()>(); // Signals struct destruction to thread
-                                         //
         let temp_clone = temp.clone();
         let humid_clone = humid.clone();
         let leak_clone = leak.clone();
diff --git a/src/comms/mod.rs b/src/comms/mod.rs
index 696202f..f4943b6 100644
--- a/src/comms/mod.rs
+++ b/src/comms/mod.rs
@@ -1,4 +1,5 @@
-//! Modules for communicating with varius subsystems
+//! Communications implementations for various subsystems
+
 pub mod auv_control_board;
 pub mod control_board;
 #[cfg(feature = "meb")]
diff --git a/src/comms/zed_ros2.rs b/src/comms/zed_ros2.rs
index bc95d5f..f680ea8 100644
--- a/src/comms/zed_ros2.rs
+++ b/src/comms/zed_ros2.rs
@@ -1,3 +1,7 @@
+//! Implements communication with our external vision stack
+//!
+//! ROS2 and Stereolabs ZED cameras
+
 use color_eyre::eyre::Result;
 use futures::StreamExt;
 use ros2_client::{
diff --git a/src/config/bin.rs b/src/config/bin.rs
index 5be1270..c5c414f 100644
--- a/src/config/bin.rs
+++ b/src/config/bin.rs
@@ -1,5 +1,8 @@
+//! Configuration for the bin mission
+
 use serde::{Deserialize, Serialize};
 
+/// Bin mission config submodule
 #[derive(Debug, Serialize, Deserialize)]
 pub struct Config {
     pub depth: f32,
diff --git a/src/config/coinflip.rs b/src/config/coinflip.rs
index 809ce91..c0148a8 100644
--- a/src/config/coinflip.rs
+++ b/src/config/coinflip.rs
@@ -1,5 +1,8 @@
+//! Configuration for missions that handle the coinflip task
+
 use serde::{Deserialize, Serialize};
 
+/// Coinflip mission config submodule
 #[derive(Debug, Serialize, Deserialize)]
 pub struct Config {
     pub depth: f32,
diff --git a/src/config/gate.rs b/src/config/gate.rs
index 6065edd..074c771 100644
--- a/src/config/gate.rs
+++ b/src/config/gate.rs
@@ -1,6 +1,9 @@
+//! Configuration for the gate family of missions
+
 use super::Side;
 use serde::{Deserialize, Serialize};
 
+/// Gate mission config submodule
 #[derive(Debug, Serialize, Deserialize)]
 pub struct Config {
     pub depth: f32,
diff --git a/src/config/mod.rs b/src/config/mod.rs
index 45b940f..ad95af8 100644
--- a/src/config/mod.rs
+++ b/src/config/mod.rs
@@ -1,3 +1,5 @@
+//! External configuration, parsable from a config file
+
 pub mod bin;
 pub mod coinflip;
 pub mod gate;
@@ -15,6 +17,7 @@ use serde::{Deserialize, Serialize};
 use std::collections::HashMap;
 use std::ops::RangeInclusive;
 
+/// Time to wait on graceful shutdown before hard shutdown
 pub const SHUTDOWN_TIMEOUT: u64 = 5;
 
 // Default values
@@ -29,23 +32,41 @@ const ZED_DEPTH_TOPIC: &str = "depth/depth_registered";
 const ZED_CLOUD_TOPIC: &str = "point_cloud/cloud_registered";
 const ZED_POSE_TOPIC: &str = "pose";
 
+/// Top-level configuration
 #[derive(Debug, Serialize, Deserialize)]
 pub struct Config {
+    /// The path to the control board serial port
     pub control_board_path: String,
+    /// The backup path to the control board serial port
     pub control_board_backup_path: String,
+    /// The path to the main electronics board serial port
     pub meb_path: String,
+    /// The path to the front camera video device
     pub front_cam_path: String,
+    /// The path to the bottom camera video device
     pub bottom_cam_path: String,
-    pub sonar: sonar::Config,
-    pub missions: Missions,
+    /// The active color profile used for vision. Must be in `color_profiles`
     pub color_profile: String,
+    /// The available color profiles usable for vision
     pub color_profiles: HashMap<String, ColorProfile>,
+    /// The [`Side`] the shark prop is on
     pub shark: Side,
+    /// The [`Side`] the saw fish prop is on
     pub saw_fish: Side,
+    /// BlueRobotics Ping360 sonar config submodule
+    pub sonar: sonar::Config,
+    /// Missions config submodule
+    pub missions: Missions,
+    /// ZED ROS2 subsystem config submodule
     pub zed_ros2: ZedRos2Config,
 }
 
 impl Config {
+    /// Generates a populated [`Config`] from a `config.toml` file in the current directory.
+    ///
+    /// # Errors
+    ///
+    /// This function will return an error if `config.toml` does not exist.
     pub fn new() -> Result<Self> {
         let config_string = read_to_string(CONFIG_FILE)?;
         Ok(toml::from_str(&config_string)?)
@@ -53,6 +74,7 @@ impl Config {
 }
 
 impl Config {
+    /// Returns the configured [`ColorProfile`] if it exists in `color_profiles`.
     pub fn get_color_profile(&self) -> Option<&ColorProfile> {
         self.color_profiles.get(&self.color_profile)
     }
@@ -77,6 +99,7 @@ impl Default for Config {
     }
 }
 
+/// ZED ROS2 subsystem config submodule.
 #[derive(Debug, Serialize, Deserialize)]
 pub struct ZedRos2Config {
     pub namespace: String,
@@ -96,6 +119,9 @@ impl Default for ZedRos2Config {
     }
 }
 
+/// Missions config submodule.
+///
+/// Each submodule corresponds to a [mission](crate::missions).
 #[derive(Debug, Default, Serialize, Deserialize)]
 pub struct Missions {
     pub gate: gate::Config,
diff --git a/src/config/octagon.rs b/src/config/octagon.rs
index 5844c1b..ff33dd1 100644
--- a/src/config/octagon.rs
+++ b/src/config/octagon.rs
@@ -1,5 +1,8 @@
+//! Configuration for the octagon family of missions
+
 use serde::{Deserialize, Serialize};
 
+/// Octagon mission config submodule
 #[derive(Debug, Serialize, Deserialize)]
 pub struct Config {}
 
diff --git a/src/config/path_align.rs b/src/config/path_align.rs
index 72bab3c..4832a30 100644
--- a/src/config/path_align.rs
+++ b/src/config/path_align.rs
@@ -1,5 +1,8 @@
+//! Configuration for the path align mission
+
 use serde::{Deserialize, Serialize};
 
+/// Path align mission config submodule
 #[derive(Debug, Serialize, Deserialize)]
 pub struct Config {
     pub depth: f32,
diff --git a/src/config/slalom.rs b/src/config/slalom.rs
index 97404e4..99301c5 100644
--- a/src/config/slalom.rs
+++ b/src/config/slalom.rs
@@ -1,8 +1,11 @@
+//! Configuration for the slalom family of missions
+
 use std::ops::RangeInclusive;
 
 use super::Side;
 use serde::{Deserialize, Serialize};
 
+/// Slalom mission config submodule
 #[derive(Debug, Serialize, Deserialize)]
 pub struct Config {
     pub depth: f32,
diff --git a/src/config/sonar.rs b/src/config/sonar.rs
index 66e39cb..fea839f 100644
--- a/src/config/sonar.rs
+++ b/src/config/sonar.rs
@@ -1,6 +1,9 @@
+//! Configuration for the BlueRobotics Ping360 sonar module
+
 use serde::{Deserialize, Serialize};
 use std::path::PathBuf;
 
+/// Sonar config submodule
 #[derive(Debug, Serialize, Deserialize)]
 pub struct Config {
     pub serial_port: PathBuf,
diff --git a/src/lib.rs b/src/lib.rs
index d802d38..640d7c8 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,3 +1,5 @@
+//! Supporting library for `sw9s`, the high-level autonamous driver program for [SeaWolf IX](https://aquapackrobotics.org/seawolves/seawolfIX/), [AquaPack Robotics'](https://aquapackrobotics.org/) 9th gen autonamous underwater vehicle (AUV).
+
 use std::{
     fs::{create_dir, File},
     sync::{LazyLock, Mutex},

From 486cab8818f351825c038e2131625f69b39a9dd5 Mon Sep 17 00:00:00 2001
From: Cowboylaserkittenjetshark
 <82691052+Cowboylaserkittenjetshark@users.noreply.github.com>
Date: Wed, 15 Apr 2026 00:04:56 -0400
Subject: [PATCH 11/11] resolve merge conflict for real (copilot L)

---
 Cargo.lock | 454 +++++++----------------------------------------------
 Cargo.toml |  18 ---
 2 files changed, 54 insertions(+), 418 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 614276d..5f2187c 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -17,17 +17,6 @@ version = "2.0.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "320119579fcad9c21884f5c4861d16174d0e06250625266f50fe6898340abefa"
 
-[[package]]
-name = "aes"
-version = "0.8.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b169f7a6d4742236a0a00c541b845991d0ac43e546831af1249753ab4c3aa3a0"
-dependencies = [
- "cfg-if 1.0.4",
- "cipher",
- "cpufeatures 0.2.17",
-]
-
 [[package]]
 name = "ahash"
 version = "0.8.12"
@@ -66,12 +55,6 @@ dependencies = [
  "libc",
 ]
 
-[[package]]
-name = "anes"
-version = "0.1.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4b46cbb362ab8752921c97e041f5e366ee6297bd428a31275b9fcf1e380f7299"
-
 [[package]]
 name = "anstream"
 version = "1.0.0"
@@ -377,12 +360,6 @@ version = "1.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "d92bec98840b8f03a5ff5413de5293bfcd8bf96467cf5452609f939ec6f5de16"
 
-[[package]]
-name = "assert_approx_eq"
-version = "1.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3c07dab4369547dbe5114677b33fbbf724971019f3818172d59a97a61c774ffd"
-
 [[package]]
 name = "async-channel"
 version = "2.5.0"
@@ -519,12 +496,6 @@ version = "0.22.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "72b3254f16251a8381aa12e40e3c4d2f0199f8c6508fbecb9d91f575e0fbb8c6"
 
-[[package]]
-name = "base64ct"
-version = "1.8.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2af50177e190e07a26ab74f8b1efbfe2ef87da2116221318cb1c2e82baf7de06"
-
 [[package]]
 name = "bigdecimal"
 version = "0.4.10"
@@ -581,7 +552,7 @@ dependencies = [
  "arrayvec",
  "cc",
  "cfg-if 1.0.4",
- "constant_time_eq 0.4.2",
+ "constant_time_eq",
  "cpufeatures 0.3.0",
 ]
 
@@ -677,26 +648,6 @@ dependencies = [
  "bytes",
 ]
 
-[[package]]
-name = "bzip2"
-version = "0.4.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bdb116a6ef3f6c3698828873ad02c3014b3c85cadb88496095628e3ef1e347f8"
-dependencies = [
- "bzip2-sys",
- "libc",
-]
-
-[[package]]
-name = "bzip2-sys"
-version = "0.1.13+1.0.8"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "225bff33b2141874fe80d71e07d6eec4f85c5c216453dd96388240f96e1acc14"
-dependencies = [
- "cc",
- "pkg-config",
-]
-
 [[package]]
 name = "camino"
 version = "1.2.2"
@@ -708,9 +659,9 @@ dependencies = [
 
 [[package]]
 name = "cargo-platform"
-version = "0.3.2"
+version = "0.3.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "87a0c0e6148f11f01f32650a2ea02d532b2ad4e81d8bd41e6e565b5adc5e6082"
+checksum = "dd0061da739915fae12ea00e16397555ed4371a6bb285431aab930f61b0aa4ba"
 dependencies = [
  "serde",
  "serde_core",
@@ -730,12 +681,6 @@ dependencies = [
  "thiserror 2.0.18",
 ]
 
-[[package]]
-name = "cast"
-version = "0.3.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5"
-
 [[package]]
 name = "cc"
 version = "1.2.60"
@@ -853,43 +798,6 @@ version = "1.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6e4de3bc4ea267985becf712dc6d9eed8b04c953b3fcfb339ebc87acd9804901"
 
-[[package]]
-name = "ciborium"
-version = "0.2.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "42e69ffd6f0917f5c029256a24d0161db17cea3997d185db0d35926308770f0e"
-dependencies = [
- "ciborium-io",
- "ciborium-ll",
- "serde",
-]
-
-[[package]]
-name = "ciborium-io"
-version = "0.2.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "05afea1e0a06c9be33d539b876f1ce3692f4afea2cb41f740e7743225ed1c757"
-
-[[package]]
-name = "ciborium-ll"
-version = "0.2.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "57663b653d948a338bfb3eeba9bb2fd5fcfaecb9e199e87e1eda4d9e8b240fd9"
-dependencies = [
- "ciborium-io",
- "half",
-]
-
-[[package]]
-name = "cipher"
-version = "0.4.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "773f3b9af64447d2ce9850330c473515014aa235e6a783b02db81ff39e4a3dad"
-dependencies = [
- "crypto-common",
- "inout",
-]
-
 [[package]]
 name = "clang"
 version = "2.0.0"
@@ -912,9 +820,9 @@ dependencies = [
 
 [[package]]
 name = "clap"
-version = "4.6.0"
+version = "4.6.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b193af5b67834b676abd72466a96c1024e6a6ad978a1f484bd90b85c94041351"
+checksum = "1ddb117e43bbf7dacf0a4190fef4d345b9bad68dfc649cb349e7d17d28428e51"
 dependencies = [
  "clap_builder",
  "clap_derive",
@@ -934,9 +842,9 @@ dependencies = [
 
 [[package]]
 name = "clap_derive"
-version = "4.6.0"
+version = "4.6.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1110bd8a634a1ab8cb04345d8d878267d57c3cf1b38d91b71af6686408bbca6a"
+checksum = "f2ce8604710f6733aa641a2b3731eaa1e8b3d9973d5e3565da11800813f997a9"
 dependencies = [
  "heck",
  "proc-macro2",
@@ -1041,11 +949,12 @@ dependencies = [
 
 [[package]]
 name = "const_format"
-version = "0.2.35"
+version = "0.2.36"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7faa7469a93a566e9ccc1c73fe783b4a65c274c5ace346038dca9c39fe0030ad"
+checksum = "4481a617ad9a412be3b97c5d403fef8ed023103368908b9c50af598ff467cc1e"
 dependencies = [
  "const_format_proc_macros",
+ "konst",
 ]
 
 [[package]]
@@ -1059,12 +968,6 @@ dependencies = [
  "unicode-xid",
 ]
 
-[[package]]
-name = "constant_time_eq"
-version = "0.1.5"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "245097e9a4535ee1e3e3931fcfcd55a796a44c643e8596ff6566d68f09b87bbc"
-
 [[package]]
 name = "constant_time_eq"
 version = "0.4.2"
@@ -1132,44 +1035,6 @@ dependencies = [
  "cfg-if 1.0.4",
 ]
 
-[[package]]
-name = "criterion"
-version = "0.5.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f2b12d017a929603d80db1831cd3a24082f8137ce19c69e6447f54f5fc8d692f"
-dependencies = [
- "anes",
- "cast",
- "ciborium",
- "clap",
- "criterion-plot",
- "futures",
- "is-terminal",
- "itertools 0.10.5",
- "num-traits",
- "once_cell",
- "oorandom",
- "plotters",
- "rayon",
- "regex",
- "serde",
- "serde_derive",
- "serde_json",
- "tinytemplate",
- "tokio",
- "walkdir",
-]
-
-[[package]]
-name = "criterion-plot"
-version = "0.5.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6b50826342786a51a89e2da3a28f1c32b06e387201bc2d19791f622c673706b1"
-dependencies = [
- "cast",
- "itertools 0.10.5",
-]
-
 [[package]]
 name = "cros-codecs"
 version = "0.0.6"
@@ -2250,17 +2115,6 @@ version = "2.4.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "9f1f227452a390804cdb637b74a86990f2a7d7ba4b7d5693aac9b4dd6defd8d6"
 
-[[package]]
-name = "filetime"
-version = "0.2.27"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f98844151eee8917efc50bd9e8318cb963ae8b297431495d3f758616ea5c57db"
-dependencies = [
- "cfg-if 1.0.4",
- "libc",
- "libredox",
-]
-
 [[package]]
 name = "find-msvc-tools"
 version = "0.1.9"
@@ -2598,12 +2452,6 @@ version = "0.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea"
 
-[[package]]
-name = "hermit-abi"
-version = "0.5.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "fc0fef456e4baa96da950455cd02c081ca953b141298e41db3fc7e36b1da849c"
-
 [[package]]
 name = "hex"
 version = "0.4.3"
@@ -2616,15 +2464,6 @@ version = "0.1.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "5491a308e0214554f07a81d8944abe45f552871c12e3c3c6e7e5d354039a6c4c"
 
-[[package]]
-name = "hmac"
-version = "0.12.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6c49c37c09c17a53d937dfbb742eb3a961d65a994e6bcdcf37e7399d0cc8ab5e"
-dependencies = [
- "digest",
-]
-
 [[package]]
 name = "http"
 version = "1.4.0"
@@ -2937,15 +2776,6 @@ dependencies = [
  "cfb",
 ]
 
-[[package]]
-name = "inout"
-version = "0.1.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "879f10e63c20629ecabbb64a8010319738c66a5cd0c29b02d63d272b03751d01"
-dependencies = [
- "generic-array",
-]
-
 [[package]]
 name = "io-extras"
 version = "0.18.4"
@@ -2990,17 +2820,6 @@ dependencies = [
  "serde",
 ]
 
-[[package]]
-name = "is-terminal"
-version = "0.4.17"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3640c1c38b8e4e43584d8df18be5fc6b0aa314ce6ebf51b53313d4306cca8e46"
-dependencies = [
- "hermit-abi",
- "libc",
- "windows-sys 0.61.2",
-]
-
 [[package]]
 name = "is_terminal_polyfill"
 version = "1.70.2"
@@ -3128,6 +2947,21 @@ dependencies = [
  "winapi-build",
 ]
 
+[[package]]
+name = "konst"
+version = "0.2.20"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "128133ed7824fcd73d6e7b17957c5eb7bacb885649bd8c69708b2331a10bcefb"
+dependencies = [
+ "konst_macro_rules",
+]
+
+[[package]]
+name = "konst_macro_rules"
+version = "0.2.19"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a4933f3f57a8e9d9da04db23fb153356ecaf00cbd14aee46279c33dc80925c37"
+
 [[package]]
 name = "lazy_static"
 version = "1.5.0"
@@ -3221,10 +3055,7 @@ version = "0.1.16"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "e02f3bb43d335493c96bf3fd3a321600bf6bd07ed34bc64118e9293bdffea46c"
 dependencies = [
- "bitflags 2.11.1",
  "libc",
- "plain",
- "redox_syscall 0.7.4",
 ]
 
 [[package]]
@@ -3247,9 +3078,9 @@ checksum = "11d3d7f243d5c5a8b9bb5d6dd2b1602c0cb0b9db1621bafc7ed66e35ff9fe092"
 
 [[package]]
 name = "local-ip-address"
-version = "0.6.11"
+version = "0.6.12"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d4a59a0cb1c7f84471ad5cd38d768c2a29390d17f1ff2827cdf49bc53e8ac70b"
+checksum = "d7b0187df4e614e42405b49511b82ff7a1774fbd9a816060ee465067847cac22"
 dependencies = [
  "libc",
  "neli",
@@ -3739,12 +3570,6 @@ version = "1.70.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "384b8ab6d37215f3c5301a95a4accb5d64aa607f1fcb26a11b5303878451b4fe"
 
-[[package]]
-name = "oorandom"
-version = "11.1.5"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d6790f58c7ff633d8771f42965289203411a5e5c68388703c06e14f24770b41e"
-
 [[package]]
 name = "opencv"
 version = "0.94.4"
@@ -3822,40 +3647,17 @@ checksum = "2621685985a2ebf1c516881c026032ac7deafcda1a2c9b7850dc81e3dfcb64c1"
 dependencies = [
  "cfg-if 1.0.4",
  "libc",
- "redox_syscall 0.5.18",
+ "redox_syscall",
  "smallvec",
  "windows-link 0.2.1",
 ]
 
-[[package]]
-name = "password-hash"
-version = "0.4.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7676374caaee8a325c9e7a2ae557f216c5563a171d6997b0ef8a65af35147700"
-dependencies = [
- "base64ct",
- "rand_core 0.6.4",
- "subtle",
-]
-
 [[package]]
 name = "paste"
 version = "1.0.15"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a"
 
-[[package]]
-name = "pbkdf2"
-version = "0.11.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "83a0692ec44e4cf1ef28ca317f14f8f07da2d95ec3fa01f86e4467b725e60917"
-dependencies = [
- "digest",
- "hmac",
- "password-hash",
- "sha2",
-]
-
 [[package]]
 name = "peg"
 version = "0.8.5"
@@ -3927,7 +3729,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "3c80231409c20246a13fddb31776fb942c38553c51e871f8cbd687a4cfb5843d"
 dependencies = [
  "phf_shared 0.11.3",
- "rand 0.8.5",
+ "rand 0.8.6",
 ]
 
 [[package]]
@@ -4001,40 +3803,6 @@ version = "0.3.33"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "19f132c84eca552bf34cab8ec81f1c1dcc229b811638f9d283dceabe58c5569e"
 
-[[package]]
-name = "plain"
-version = "0.2.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b4596b6d070b27117e987119b4dac604f3c58cfb0b191112e24771b2faeac1a6"
-
-[[package]]
-name = "plotters"
-version = "0.3.7"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5aeb6f403d7a4911efb1e33402027fc44f29b5bf6def3effcc22d7bb75f2b747"
-dependencies = [
- "num-traits",
- "plotters-backend",
- "plotters-svg",
- "wasm-bindgen",
- "web-sys",
-]
-
-[[package]]
-name = "plotters-backend"
-version = "0.3.7"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "df42e13c12958a16b3f7f4386b9ab1f3e7933914ecea48da7139435263a4172a"
-
-[[package]]
-name = "plotters-svg"
-version = "0.3.7"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "51bae2ac328883f7acdfea3d66a7c35751187f870bc81f94563733a154d7a670"
-dependencies = [
- "plotters-backend",
-]
-
 [[package]]
 name = "ply-rs-bw"
 version = "3.0.0"
@@ -4155,9 +3923,9 @@ checksum = "c33a9471896f1c69cecef8d20cbe2f7accd12527ce60845ff44c153bb2a21b49"
 
 [[package]]
 name = "portable-atomic-util"
-version = "0.2.6"
+version = "0.2.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "091397be61a01d4be58e7841595bd4bfedb15f1cd54977d79b8271e94ed799a3"
+checksum = "c2a106d1259c23fac8e543272398ae0e3c0b8d33c88ed73d0cc71b0f1d902618"
 dependencies = [
  "portable-atomic",
 ]
@@ -4296,9 +4064,9 @@ checksum = "f8dcc9c7d52a811697d2151c701e0d08956f92b0e24136cf4cf27b57a6a0d9bf"
 
 [[package]]
 name = "rand"
-version = "0.8.5"
+version = "0.8.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404"
+checksum = "5ca0ecfa931c29007047d1bc58e623ab12e5590e8c7cc53200d5202b69266d8a"
 dependencies = [
  "rand_core 0.6.4",
 ]
@@ -5206,15 +4974,6 @@ dependencies = [
  "bitflags 2.11.1",
 ]
 
-[[package]]
-name = "redox_syscall"
-version = "0.7.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f450ad9c3b1da563fb6948a8e0fb0fb9269711c9c73d9ea1de5058c79c8d643a"
-dependencies = [
- "bitflags 2.11.1",
-]
-
 [[package]]
 name = "redox_users"
 version = "0.5.2"
@@ -5487,9 +5246,9 @@ dependencies = [
 
 [[package]]
 name = "rustls-webpki"
-version = "0.103.12"
+version = "0.103.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8279bb85272c9f10811ae6a6c547ff594d6a7f3c6c6b02ee9726d1d0dcfcdd06"
+checksum = "61c429a8649f110dddef65e2a5ad240f747e85f7758a6bccc7e5777bd33f756e"
 dependencies = [
  "ring",
  "rustls-pki-types",
@@ -5753,17 +5512,6 @@ dependencies = [
  "windows-sys 0.52.0",
 ]
 
-[[package]]
-name = "sha1"
-version = "0.10.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e3bf829a2d51ab4a5ddf1352d8470c140cadc8301b2ae1789db023f01cedd6ba"
-dependencies = [
- "cfg-if 1.0.4",
- "cpufeatures 0.2.17",
- "digest",
-]
-
 [[package]]
 name = "sha2"
 version = "0.10.9"
@@ -5956,36 +5704,26 @@ checksum = "13c2bddecc57b384dee18652358fb23172facb8a2c51ccc10d74c157bdea3292"
 name = "sw9s"
 version = "0.1.0"
 dependencies = [
- "assert_approx_eq",
  "bluerobotics-ping",
  "bytes",
- "cc",
  "chrono",
  "color-eyre",
- "criterion",
  "derive-getters",
- "flate2",
  "futures",
  "itertools 0.13.0",
  "num-traits",
  "opencv",
- "proc-macro2",
- "quote",
- "rayon",
  "rerun",
  "ros2-client",
  "ros2-interfaces-jazzy-serde",
  "serde",
  "serde_json",
- "syn 2.0.117",
- "tar",
  "thiserror 2.0.18",
  "tokio",
  "tokio-serial",
  "tokio-util",
  "toml",
  "uuid",
- "zip-extract",
 ]
 
 [[package]]
@@ -6047,17 +5785,6 @@ version = "1.0.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "55937e1799185b12863d447f42597ed69d9928686b8d88a1df17376a097d8369"
 
-[[package]]
-name = "tar"
-version = "0.4.45"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "22692a6476a21fa75fdfc11d452fda482af402c008cdbaf3476414e122040973"
-dependencies = [
- "filetime",
- "libc",
- "xattr",
-]
-
 [[package]]
 name = "tempfile"
 version = "3.27.0"
@@ -6173,21 +5900,11 @@ dependencies = [
  "zerovec",
 ]
 
-[[package]]
-name = "tinytemplate"
-version = "1.2.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "be4d6b5f19ff7664e8c98d03e2139cb510db9b0a60b55f8e8709b689d939b6bc"
-dependencies = [
- "serde",
- "serde_json",
-]
-
 [[package]]
 name = "tokio"
-version = "1.52.0"
+version = "1.52.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a91135f59b1cbf38c91e73cf3386fca9bb77915c45ce2771460c9d92f0f3d776"
+checksum = "b67dee974fe86fd92cc45b7a95fdd2f99a36a6d7b0d431a231178d3d670bbcc6"
 dependencies = [
  "bytes",
  "libc",
@@ -6477,9 +6194,9 @@ checksum = "9ea3136b675547379c4bd395ca6b938e5ad3c3d20fad76e7fe85f9e0d011419c"
 
 [[package]]
 name = "typenum"
-version = "1.19.0"
+version = "1.20.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "562d481066bde0658276a35467c4af00bdc6ee726305698a55b86e61d7ad82bb"
+checksum = "40ce102ab67701b8526c123c1bab5cbe42d7040ccfd0f64af1a385808d2f43de"
 
 [[package]]
 name = "unescaper"
@@ -6581,9 +6298,9 @@ checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821"
 
 [[package]]
 name = "uuid"
-version = "1.23.0"
+version = "1.23.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5ac8b6f42ead25368cf5b098aeb3dc8a1a2c05a3eee8a9a1a68c640edbfc79d9"
+checksum = "ddd74a9687298c6858e9b88ec8935ec45d22e8fd5e6394fa1bd4e99a87789c76"
 dependencies = [
  "getrandom 0.4.2",
  "js-sys",
@@ -6641,11 +6358,11 @@ checksum = "ccf3ec651a847eb01de73ccad15eb7d99f80485de043efb2f370cd654f4ea44b"
 
 [[package]]
 name = "wasip2"
-version = "1.0.2+wasi-0.2.9"
+version = "1.0.3+wasi-0.2.9"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9517f9239f02c069db75e65f174b3da828fe5f5b945c4dd26bd25d89c03ebcf5"
+checksum = "20064672db26d7cdc89c7798c48a0fdfac8213434a1186e5ef29fd560ae223d6"
 dependencies = [
- "wit-bindgen",
+ "wit-bindgen 0.57.1",
 ]
 
 [[package]]
@@ -6654,7 +6371,7 @@ version = "0.4.0+wasi-0.3.0-rc-2026-01-06"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "5428f8bf88ea5ddc08faddef2ac4a67e390b88186c703ce6dbd955e1c145aca5"
 dependencies = [
- "wit-bindgen",
+ "wit-bindgen 0.51.0",
 ]
 
 [[package]]
@@ -6785,14 +6502,14 @@ version = "0.26.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "521bc38abb08001b01866da9f51eb7c5d647a19260e00054a8c7fd5f9e57f7a9"
 dependencies = [
- "webpki-roots 1.0.6",
+ "webpki-roots 1.0.7",
 ]
 
 [[package]]
 name = "webpki-roots"
-version = "1.0.6"
+version = "1.0.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "22cfaf3c063993ff62e73cb4311efde4db1efb31ab78a3e5c457939ad5cc0bed"
+checksum = "52f5ee44c96cf55f1b349600768e3ece3a8f26010c05265ab73f945bb1a2eb9d"
 dependencies = [
  "rustls-pki-types",
 ]
@@ -7222,6 +6939,12 @@ dependencies = [
  "wit-bindgen-rust-macro",
 ]
 
+[[package]]
+name = "wit-bindgen"
+version = "0.57.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1ebf944e87a7c253233ad6766e082e3cd714b5d03812acc24c318f549614536e"
+
 [[package]]
 name = "wit-bindgen-core"
 version = "0.51.0"
@@ -7317,16 +7040,6 @@ dependencies = [
  "winapi-build",
 ]
 
-[[package]]
-name = "xattr"
-version = "1.6.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "32e45ad4206f6d2479085147f02bc2ef834ac85886624a23575ae137c8aa8156"
-dependencies = [
- "libc",
- "rustix",
-]
-
 [[package]]
 name = "xxhash-rust"
 version = "0.8.15"
@@ -7436,67 +7149,8 @@ dependencies = [
  "syn 2.0.117",
 ]
 
-[[package]]
-name = "zip"
-version = "0.6.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "760394e246e4c28189f19d488c058bf16f564016aefac5d32bb1f3b51d5e9261"
-dependencies = [
- "aes",
- "byteorder",
- "bzip2",
- "constant_time_eq 0.1.5",
- "crc32fast",
- "crossbeam-utils",
- "flate2",
- "hmac",
- "pbkdf2",
- "sha1",
- "zstd",
-]
-
-[[package]]
-name = "zip-extract"
-version = "0.1.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e109e5a291403b4c1e514d39f8a22d3f98d257e691a52bb1f16051bb1ffed63e"
-dependencies = [
- "log",
- "thiserror 1.0.69",
- "zip",
-]
-
 [[package]]
 name = "zmij"
 version = "1.0.21"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b8848ee67ecc8aedbaf3e4122217aff892639231befc6a1b58d29fff4c2cabaa"
-
-[[package]]
-name = "zstd"
-version = "0.11.2+zstd.1.5.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "20cc960326ece64f010d2d2107537f26dc589a6573a316bd5b1dba685fa5fde4"
-dependencies = [
- "zstd-safe",
-]
-
-[[package]]
-name = "zstd-safe"
-version = "5.0.2+zstd.1.5.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1d2a5585e04f9eea4b2a3d1eca508c4dee9592a89ef6f450c11719da0726f4db"
-dependencies = [
- "libc",
- "zstd-sys",
-]
-
-[[package]]
-name = "zstd-sys"
-version = "2.0.16+zstd.1.5.7"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "91e19ebc2adc8f83e43039e79776e3fda8ca919132d68a1fed6a5faca2683748"
-dependencies = [
- "cc",
- "pkg-config",
-]
diff --git a/Cargo.toml b/Cargo.toml
index 137a2f6..763513b 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -48,21 +48,3 @@ ros2-interfaces-jazzy-serde = { version = "0.0.4", features = ["sensor_msgs"] }
 thiserror = "2.0.18"
 color-eyre = { version = "0.6.5", default-features = false }
 rerun = { version = "0.29.2", default-features = false, features = ["sdk", "server", "ecolor"] }
-
-[build-dependencies]
-quote = { version = "1.0.36", optional = true }
-syn = { version = "2.0.68", features = ["full", "fold"], optional = true }
-proc-macro2 = { version = "1.0.86", optional = true }
-cc = { version = "1.0.99", optional = true }
-
-[dev-dependencies]
-assert_approx_eq = "1.1.0" # Floating point eq
-criterion = { version = "0.5.1", features = ["async_tokio"] } # Benchmarking
-rayon = "1.10.0"
-
-[target.'cfg(target_os = "linux")'.dev-dependencies]
-flate2 = "1.0.30" # Decompressing gz
-tar = "0.4.40" # Unpacking tar
-
-[target.'cfg(not(target_os = "linux"))'.dev-dependencies]
-zip-extract = "0.1.3" # Decompressing zip