Configurable vehicle parameters

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
@@ -46,22 +45,6 @@ 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"
+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

src/comms/auv_control_board/mod.rs

@@ -1,12 +1,18 @@
+//! 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 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 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;

src/comms/auv_control_board/response.rs

@@ -1,3 +1,4 @@
+//! Response parsing
 use bytes::BufMut;
 use tokio::io::AsyncReadExt;
 
@@ -194,7 +195,7 @@ mod tests {
             )
             .collect::<Vec<Vec<u8>>>()
             .await,
-            vec![vec![]]
+            Vec::<Vec<u8>>::new()
         );
 
         assert_eq!(

src/comms/auv_control_board/util.rs

@@ -1,5 +1,7 @@
+//! Helper utilities for communicating with the control board
+//!
 // 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 +40,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 {

src/comms/control_board/mod.rs

@@ -1,7 +1,12 @@
+//! 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 tokio::{
     io::{self, AsyncRead, AsyncWrite, AsyncWriteExt, WriteHalf},
     net::TcpStream,
@@ -12,23 +17,31 @@ 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};
 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,
 }
 
+/// 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
@@ -46,24 +59,27 @@ 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>,
+        vehicle_defintion: &VehicleDefinition,
+    ) -> 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 {
             inner: AUVControlBoard::new(Mutex::from(comm_out).into(), responses, msg_id).into(),
             initial_angles: Arc::default(),
         };
 
-        this.init_matrices().await?;
-        this.thruster_inversion_set(&THRUSTER_INVS).await?;
-        this.relative_dof_speed_set_batch(&DOF_SPEEDS).await?;
+        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 {
@@ -76,7 +92,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();
 
@@ -103,41 +119,36 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
         Ok(this)
     }
 
-    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, 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)
+                    .await?;
+            }
+        }
 
         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)
+    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('Z', 4.0, 0.0, 0.0, 1.0, false)
-            .await?;
-        self.stability_assist_pid_tune('D', 1.5, 0.0, 0.0, 1.0, false)
-            .await
+        }
+        Ok(())
     }
 }
 
 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;
@@ -148,14 +159,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 {
@@ -170,7 +186,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
     }
 }
 
@@ -199,7 +215,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());
@@ -219,7 +235,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);
 
@@ -351,7 +367,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()
             }
         };
@@ -419,7 +435,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);
 

src/comms/control_board/response.rs

@@ -1,3 +1,4 @@
+//! Response parsing
 use std::{
     collections::HashMap,
     sync::{

src/comms/control_board/util.rs

@@ -1,7 +1,45 @@
+//! Utilities for parsing interacting with the control board
 use std::f32::consts::PI;
 
-use anyhow::bail;
+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](super::vehicle_definition::VehicleDefinition).
+    ///
+    /// 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 },
+
+    /// 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
@@ -18,7 +56,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 {