adds initial motor bravo implementation

flight/Cargo.toml

@@ -15,6 +15,9 @@ nautilus_common = { workspace = true }
 postcard = { workspace = true }
 rpi-pal = { workspace = true, features = ["hal"], optional = true }
 good_lp = { workspace = true, features = ["microlp"] }
+strum = {workspace = true}
+strum_macros = {workspace = true}
+thiserror = {workspace = true}
 
 [dev-dependencies]
 embedded-hal-mock = { workspace = true }

flight/src/commanded_state.rs

@@ -30,7 +30,7 @@ where
         Self {
             entries: [const { None }; N],
             default,
-            changed: false,
+            changed: true,
         }
     }
 

flight/src/comms/mod.rs

@@ -27,7 +27,7 @@ impl TelemetrySender {
     }
 }
 
-type CommandCallback<'a> = Box<dyn Fn(&[u8]) -> Result<()> + Send + 'a>;
+type CommandCallback<'a> = Box<dyn FnMut(&[u8]) -> Result<()> + Send + 'a>;
 
 pub struct CommsTask<'a, A>
 where
@@ -82,7 +82,7 @@ where
     pub fn add_command_handler<T: Command>(
         &mut self,
         command: impl Into<String>,
-        handler: impl Fn(T) + Send + 'a,
+        mut handler: impl FnMut(T) + Send + 'a,
     ) -> Result<()> {
         let command = command.into();
         ensure!(
@@ -137,7 +137,7 @@ where
                         .rx_bytes
                         .wrapping_add(u32::try_from(size % (u32::MAX as usize)).unwrap_or(0));
                 });
-                match self.command_callbacks.get(cmd.name) {
+                match self.command_callbacks.get_mut(cmd.name) {
                     Some(handler) => {
                         if let Err(e) = handler(cmd.data) {
                             error!("Command Error: {e}");

flight/src/hardware/a4963/driver.rs

@@ -0,0 +1,145 @@
+use std::any::type_name;
+use std::fmt::{Debug, Formatter};
+use std::thread::sleep;
+use std::time::Duration;
+use crate::hardware::a4963::A4963;
+use anyhow::{ensure, Result};
+use embedded_hal::spi::SpiDevice;
+use log::{trace, warn};
+use crate::hardware::a4963::register::{A4963Register, Configuration0Register, Configuration1Register, Configuration2Register, Configuration3Register, Configuration4Register, Configuration5Register, DiagnosticRegister, Direction, MaskRegister, MotorControlMode, RunRegister, SpeedOutputSelection, WRITE_BIT};
+use crate::hardware::error::WrappingError;
+
+pub struct A4963Driver<SPI> {
+    spi: SPI,
+    mask_register: MaskRegister
+}
+
+impl<SPI> A4963Driver<SPI>
+where
+    SPI: SpiDevice<u8>,
+    SPI::Error: Send,
+    SPI::Error: Sync,
+    SPI::Error: 'static, {
+    pub fn new(spi: SPI) -> Self {
+        trace!(
+            "A4963Driver<SPI={}>::new(spi)",
+            type_name::<SPI>()
+        );
+        Self {
+            spi,
+            mask_register: MaskRegister {
+                ..Default::default()
+            },
+        }
+    }
+
+    fn update_mask_register(&mut self, mask_register: MaskRegister) -> Result<DiagnosticRegister> {
+        self.mask_register = mask_register;
+        self.write_verify(self.mask_register.clone())
+    }
+
+    fn read_diagnostic_register(&mut self) -> Result<DiagnosticRegister> {
+        self.write(self.mask_register.clone())
+    }
+
+    fn write<Register: A4963Register>(&mut self, register: Register) -> Result<DiagnosticRegister> {
+        trace!("A4963Driver::write(self: {self:?}, register: {register:?})");
+        let mut rx_buffer = [0u8; 2];
+        let tx_data = register.encode() | Register::ADDRESS | WRITE_BIT;
+        trace!("A4963Driver::write - tx {} = {tx_data:016b}", type_name::<Register>());
+        let tx_buffer = tx_data.to_be_bytes();
+        self.spi.transfer(&mut rx_buffer, &tx_buffer).map_err(WrappingError)?;
+        let rx_data = u16::from_be_bytes(rx_buffer);
+        trace!("A4963Driver::write - rx = {rx_data:016b}");
+        Ok(DiagnosticRegister::decode(rx_data))
+    }
+
+    fn read<Register: A4963Register>(&mut self) -> Result<Register> {
+        trace!("A4963Driver::read<{}>(self: {self:?})", type_name::<Register>());
+        let mut rx_buffer = [0u8; 2];
+        let tx_buffer = Register::ADDRESS.to_be_bytes();
+        trace!("A4963Driver::read - tx = {:016b}", Register::ADDRESS);
+        self.spi.transfer(&mut rx_buffer, &tx_buffer).map_err(WrappingError)?;
+        let rx_data = u16::from_be_bytes(rx_buffer);
+        trace!("A4963Driver::read - rx {} = {rx_data:016b}", type_name::<Register>());
+        DiagnosticRegister::decode(rx_data & 0b1110_0000_0000_0000)
+            .assert_healthy()?;
+        Ok(Register::decode(rx_data))
+    }
+
+    fn write_verify<Register: A4963Register>(&mut self, register: Register) -> Result<DiagnosticRegister> {
+        let result = self.write(register.clone())?;
+        let readback = self.read()?;
+
+        ensure!(register == readback, "Register did not match readback.\nregister = {register:?},\nreadback = {readback:?}");
+
+        Ok(result)
+    }
+}
+
+impl<SPI> A4963 for A4963Driver<SPI>
+where
+    SPI: SpiDevice<u8>,
+    SPI::Error: Send,
+    SPI::Error: Sync,
+    SPI::Error: 'static, {
+
+    fn init(&mut self) -> Result<()> {
+        trace!("A4963Driver::init(self: {self:?})");
+
+        // Write the fault mask first
+        self.update_mask_register(MaskRegister {
+            enable_loss_of_synchronization: false,
+            ..Default::default()
+        })?.assert_healthy()?;
+        // Write our configurations
+        self.write_verify(Configuration0Register {
+            ..Default::default()
+        })?.assert_healthy()?;
+        self.write_verify(Configuration1Register {
+            // I_lim = (n + 1) * 0.0125 / 0.007 = 8.9285714286A
+            current_sense_threshold: 4,
+            ..Default::default()
+        })?.assert_healthy()?;
+        self.write_verify(Configuration2Register {
+            ..Default::default()
+        })?.assert_healthy()?;
+        self.write_verify(Configuration3Register {
+            ..Default::default()
+        })?.assert_healthy()?;
+        self.write_verify(Configuration4Register {
+            forced_startup_torque_duty_cycle: 0b0111,
+            start_speed: 0b1111,
+            ..Default::default()
+        })?.assert_healthy()?;
+        self.write_verify(Configuration5Register {
+            speed_output_selection: SpeedOutputSelection::CommutationFrequency,
+            // 819.1Hz = 49146 rpm max speed (divide by number of motor pole pairs)
+            // Measured maximum of the motor is ~840Hz
+            maximum_electrical_cycle_frequency: 0b101,
+            ..Default::default()
+        })?.assert_healthy()?;
+        // Set up the run mode for the motor (this enables the motor)
+        self.write_verify(RunRegister {
+            motor_control_mode: MotorControlMode::ClosedLoopSpeed,
+            direction: Direction::Forward, // A = Black, B = Yellow, C = Red
+            ..Default::default()
+        })?.assert_healthy()?;
+
+        Ok(())
+    }
+
+    fn check_health(&mut self) -> Result<()> {
+        self.read_diagnostic_register()?.assert_healthy()
+    }
+}
+
+impl<SPI> Debug for A4963Driver<SPI> {
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        write!(
+            f,
+            "A4963Driver<SPI={}> {{ }}",
+            type_name::<SPI>(),
+        )
+    }
+}

flight/src/hardware/a4963/mod.rs

@@ -0,0 +1,12 @@
+mod driver;
+mod register;
+
+use anyhow::Result;
+
+pub trait A4963 {
+    fn init(&mut self) -> Result<()>;
+
+    fn check_health(&mut self) -> Result<()>;
+}
+
+pub use driver::A4963Driver;

flight/src/hardware/a4963/register.rs

@@ -0,0 +1,587 @@
+use std::fmt::Debug;
+use std::hint::unreachable_unchecked;
+use strum_macros::FromRepr;
+use anyhow::Result;
+use thiserror::Error;
+
+pub(super) const WRITE_BIT: u16 = 0b0001_0000_0000_0000;
+
+pub(super) trait A4963Register: Clone + PartialEq + Debug {
+    const ADDRESS: u16;
+
+    fn encode(self) -> u16;
+    fn decode(value: u16) -> Self;
+}
+
+#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
+#[repr(u8)]
+pub(super) enum RecirculationMode {
+    Auto = 0b00,
+    High = 0b01,
+    Low = 0b10,
+    Off = 0b11,
+}
+
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub(super) struct Configuration0Register {
+    pub recirculation_mode: RecirculationMode,
+    /// 4 bits long. Blank time is this * 400ns
+    pub blank_time: u8,
+    /// 6 bits long. Dead time is this * 50ns
+    pub dead_time: u8,
+}
+
+impl Default for Configuration0Register {
+    fn default() -> Self {
+        Self {
+            recirculation_mode: RecirculationMode::Auto,
+            blank_time: 0b1000,
+            dead_time: 0b01_0100,
+        }
+    }
+}
+
+impl A4963Register for Configuration0Register {
+    const ADDRESS: u16 = 0b0000_0000_0000_0000;
+
+    fn encode(self) -> u16 {
+        assert!(self.blank_time <= 0b1111);
+        assert!(self.dead_time <= 0b11_1111);
+
+        let mut result = 0;
+
+        result |= (self.recirculation_mode as u16) << 10;
+        result |= (self.blank_time as u16) << 6;
+        result |= self.dead_time as u16;
+
+        result
+    }
+
+    fn decode(value: u16) -> Self {
+        Self {
+            recirculation_mode: RecirculationMode::from_repr(((value >> 10) & 0b11) as u8)
+                // Safety: the enum covers all permutations
+                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
+            blank_time: ((value >> 6) & 0b1111) as u8,
+            dead_time: (value & 0b11_1111) as u8,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
+#[repr(u8)]
+pub(super) enum BemfTimeQualifier {
+    Debounce = 0b0,
+    Window = 0b1,
+}
+
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub(super) struct Configuration1Register {
+    /// 12.5% if false, 25% if true
+    pub enable_fast_decay: bool,
+    pub invert_pwm: bool,
+    /// 4 bit value. V_ilm = (n + 1) * 12.5mV
+    pub current_sense_threshold: u8,
+    pub bemf_time_qualifier: BemfTimeQualifier,
+    /// 5 bit value. V_dst = n * 50mV
+    pub short_detection_threshold: u8,
+}
+
+impl Default for Configuration1Register {
+    fn default() -> Self {
+        Self {
+            enable_fast_decay: false,
+            invert_pwm: false,
+            current_sense_threshold: 0b1111,
+            bemf_time_qualifier: BemfTimeQualifier::Debounce,
+            short_detection_threshold: 0b1_1111,
+        }
+    }
+}
+
+impl A4963Register for Configuration1Register {
+    const ADDRESS: u16 = 0b0010_0000_0000_0000;
+
+    fn encode(self) -> u16 {
+        assert!(self.current_sense_threshold <= 0b1111);
+        assert!(self.short_detection_threshold <= 0b1_1111);
+
+        let mut result = 0;
+
+        result |= if self.enable_fast_decay { 0b0000_1000_0000_0000 } else {0};
+        result |= if self.invert_pwm { 0b0000_0100_0000_0000 } else {0};
+        result |= (self.current_sense_threshold as u16) << 6;
+        result |= (self.bemf_time_qualifier as u16) << 5;
+        result |= self.short_detection_threshold as u16;
+
+        result
+    }
+
+    fn decode(value: u16) -> Self {
+        Self {
+            enable_fast_decay: (value & 0b0000_1000_0000_0000) != 0,
+            invert_pwm: (value & 0b0000_0100_0000_0000) != 0,
+            current_sense_threshold: ((value >> 6) & 0b1111) as u8,
+            bemf_time_qualifier: BemfTimeQualifier::from_repr(((value >> 5) & 0b1) as u8)
+                // Safety: the enum covers all permutations
+                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
+            short_detection_threshold: (value & 0b1_1111) as u8,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
+#[repr(u8)]
+pub(super) enum OverspeedLimit {
+    Percent100 = 0b00,
+    Percent125 = 0b01,
+    Percent150 = 0b10,
+    Percent200 = 0b11,
+}
+
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub(super) struct Configuration2Register {
+    /// K_CP = 2^(n-7)
+    pub position_proportional_gain: u8,
+    pub overspeed_limit: OverspeedLimit,
+    pub degauss_compensation: bool,
+    /// t_PW = 20us + (n * 1.6us)
+    pub fixed_period: u8,
+}
+
+impl Default for Configuration2Register {
+    fn default() -> Self {
+        Self {
+            position_proportional_gain: 0b1000,
+            overspeed_limit: OverspeedLimit::Percent150,
+            degauss_compensation: false,
+            fixed_period: 0b1_0011,
+        }
+    }
+}
+
+impl A4963Register for Configuration2Register {
+    const ADDRESS: u16 = 0b0100_0000_0000_0000;
+
+    fn encode(self) -> u16 {
+        assert!(self.position_proportional_gain <= 0b1111);
+        assert!(self.fixed_period <= 0b1_1111);
+
+        let mut result = 0;
+
+        result |= (self.position_proportional_gain as u16) << 8;
+        result |= (self.overspeed_limit as u16) << 6;
+        result |= if self.degauss_compensation {0b0000_0000_0010_0000} else {0};
+        result |= self.fixed_period as u16;
+
+        result
+    }
+
+    fn decode(value: u16) -> Self {
+        Self {
+            position_proportional_gain: ((value >> 8) & 0b1111) as u8,
+            overspeed_limit: OverspeedLimit::from_repr(((value >> 6) & 0b11) as u8)
+                // Safety: the enum covers all permutations
+                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
+            degauss_compensation: (value & 0b0000_0000_0010_0000) != 0,
+            fixed_period: (value & 0b1_1111) as u8,
+        }
+    }
+}
+
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub(super) struct Configuration3Register {
+    /// K_CI = 2^(n-7)
+    pub position_integral_gain: u8,
+    /// D_H = (n + 1) * 6.25%
+    pub hold_torque_duty_cycle: u8,
+    /// t_HOLD = n * 8ms
+    pub hold_time: u8,
+}
+
+impl Default for Configuration3Register {
+    fn default() -> Self {
+        Self {
+            position_integral_gain: 0b1000,
+            hold_torque_duty_cycle: 0b0101,
+            hold_time: 0b0010,
+        }
+    }
+}
+
+impl A4963Register for Configuration3Register {
+    const ADDRESS: u16 = 0b0110_0000_0000_0000;
+
+    fn encode(self) -> u16 {
+        assert!(self.position_integral_gain <= 0b1111);
+        assert!(self.hold_torque_duty_cycle <= 0b1111);
+        assert!(self.hold_time <= 0b1111);
+
+        let mut result = 0;
+
+        result |= (self.position_integral_gain as u16) << 8;
+        result |= (self.hold_torque_duty_cycle as u16) << 4;
+        result |= self.hold_time as u16;
+
+        result
+    }
+
+    fn decode(value: u16) -> Self {
+        Self {
+            position_integral_gain: ((value >> 8) & 0b1111) as u8,
+            hold_torque_duty_cycle: ((value >> 4) & 0b1111) as u8,
+            hold_time: (value & 0b1111) as u8,
+        }
+    }
+}
+
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub(super) struct Configuration4Register {
+    /// K_SP = 2^(n-7) * K_NSP
+    pub speed_proportional_gain: u8,
+    /// D_S = (n + 1) * 6.25%
+    pub forced_startup_torque_duty_cycle: u8,
+    /// f_ST = (n + 1) * 2Hz
+    pub start_speed: u8,
+}
+
+impl Default for Configuration4Register {
+    fn default() -> Self {
+        Self {
+            speed_proportional_gain: 0b1000,
+            forced_startup_torque_duty_cycle: 0b0111,
+            start_speed: 0b0011,
+        }
+    }
+}
+
+impl A4963Register for Configuration4Register {
+    const ADDRESS: u16 = 0b1000_0000_0000_0000;
+
+    fn encode(self) -> u16 {
+        assert!(self.speed_proportional_gain <= 0b1111);
+        assert!(self.forced_startup_torque_duty_cycle <= 0b1111);
+        assert!(self.start_speed <= 0b1111);
+
+        let mut result = 0;
+
+        result |= (self.speed_proportional_gain as u16) << 8;
+        result |= (self.forced_startup_torque_duty_cycle as u16) << 4;
+        result |= self.start_speed as u16;
+
+        result
+    }
+
+    fn decode(value: u16) -> Self {
+        Self {
+            speed_proportional_gain: ((value >> 8) & 0b1111) as u8,
+            forced_startup_torque_duty_cycle: ((value >> 4) & 0b1111) as u8,
+            start_speed: (value & 0b1111) as u8,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
+#[repr(u8)]
+pub(super) enum SpeedOutputSelection {
+    ElectricalFrequency = 0b0,
+    CommutationFrequency = 0b1,
+}
+
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub(super) struct Configuration5Register {
+    /// K_SI = 2^(n-7) * K_NSI
+    pub speed_integral_gain: u8,
+    pub speed_output_selection: SpeedOutputSelection,
+    /// f_MS = (2^(8 + n) - 1) * 0.1Hz
+    pub maximum_electrical_cycle_frequency: u8,
+    /// theta_ADV = n * 1.875˚ (electrical)
+    pub phase_advance: u8,
+}
+
+impl Default for Configuration5Register {
+    fn default() -> Self {
+        Self {
+            speed_integral_gain: 0b1000,
+            speed_output_selection: SpeedOutputSelection::ElectricalFrequency,
+            maximum_electrical_cycle_frequency: 0b101,
+            phase_advance: 0b1000,
+        }
+    }
+}
+
+impl A4963Register for Configuration5Register {
+    const ADDRESS: u16 = 0b1010_0000_0000_0000;
+
+    fn encode(self) -> u16 {
+        assert!(self.speed_integral_gain <= 0b1111);
+        assert!(self.maximum_electrical_cycle_frequency <= 0b111);
+        assert!(self.phase_advance <= 0b1111);
+
+        let mut result = 0;
+
+        result |= (self.speed_integral_gain as u16) << 8;
+        result |= (self.speed_output_selection as u16) << 7;
+        result |= (self.maximum_electrical_cycle_frequency as u16) << 4;
+        result |= self.phase_advance as u16;
+
+        result
+    }
+
+    fn decode(value: u16) -> Self {
+        Self {
+            speed_integral_gain: ((value >> 8) & 0b1111) as u8,
+            speed_output_selection: SpeedOutputSelection::from_repr(((value >> 7) & 0b1) as u8)
+                // Safety: the enum covers all permutations
+                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
+            maximum_electrical_cycle_frequency: ((value >> 4) & 0b111) as u8,
+            phase_advance: (value & 0b1111) as u8,
+        }
+    }
+}
+
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub(super) struct MaskRegister {
+    pub enable_temperature_warning: bool,
+    pub enable_overtemperature: bool,
+    pub enable_loss_of_synchronization: bool,
+    pub enable_undervoltage: bool,
+    pub enable_phase_a_high_side: bool,
+    pub enable_phase_a_low_side: bool,
+    pub enable_phase_b_high_side: bool,
+    pub enable_phase_b_low_side: bool,
+    pub enable_phase_c_high_side: bool,
+    pub enable_phase_c_low_side: bool,
+}
+
+impl Default for MaskRegister {
+    fn default() -> Self {
+        Self {
+            enable_temperature_warning: true,
+            enable_overtemperature: true,
+            enable_loss_of_synchronization: true,
+            enable_undervoltage: true,
+            enable_phase_a_high_side: true,
+            enable_phase_a_low_side: true,
+            enable_phase_b_high_side: true,
+            enable_phase_b_low_side: true,
+            enable_phase_c_high_side: true,
+            enable_phase_c_low_side: true,
+        }
+    }
+}
+
+impl A4963Register for MaskRegister {
+    const ADDRESS: u16 = 0b1100_0000_0000_0000;
+
+    fn encode(self) -> u16 {
+        let mut result = 0;
+
+        result |= if !self.enable_temperature_warning { 0b0000_1000_0000_0000 } else { 0 };
+        result |= if !self.enable_overtemperature { 0b0000_0100_0000_0000 } else { 0 };
+        result |= if !self.enable_loss_of_synchronization { 0b0000_0010_0000_0000 } else { 0 };
+        result |= if !self.enable_undervoltage { 0b0000_0000_1000_0000 } else { 0 };
+        result |= if !self.enable_phase_a_high_side { 0b0000_0000_0010_0000 } else { 0 };
+        result |= if !self.enable_phase_a_low_side { 0b0000_0000_0001_0000 } else { 0 };
+        result |= if !self.enable_phase_b_high_side { 0b0000_0000_0000_1000 } else { 0 };
+        result |= if !self.enable_phase_b_low_side { 0b0000_0000_0000_0100 } else { 0 };
+        result |= if !self.enable_phase_c_high_side { 0b0000_0000_0000_0010 } else { 0 };
+        result |= if !self.enable_phase_c_low_side { 0b0000_0000_0000_0001 } else { 0 };
+
+        result
+    }
+
+    fn decode(value: u16) -> Self {
+        Self {
+            enable_temperature_warning: (value & 0b0000_1000_0000_0000) == 0,
+            enable_overtemperature: (value & 0b0000_0100_0000_0000) == 0,
+            enable_loss_of_synchronization: (value & 0b0000_0010_0000_0000) == 0,
+            enable_undervoltage: (value & 0b0000_0000_1000_0000) == 0,
+            enable_phase_a_high_side: (value & 0b0000_0000_0010_0000) == 0,
+            enable_phase_a_low_side: (value & 0b0000_0000_0001_0000) == 0,
+            enable_phase_b_high_side: (value & 0b0000_0000_0000_1000) == 0,
+            enable_phase_b_low_side: (value & 0b0000_0000_0000_0100) == 0,
+            enable_phase_c_high_side: (value & 0b0000_0000_0000_0010) == 0,
+            enable_phase_c_low_side: (value & 0b0000_0000_0000_0001) == 0,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
+#[repr(u8)]
+pub(super) enum MotorControlMode {
+    IndirectDutyCycle = 0b00,
+    DirectDutyCycle = 0b01,
+    ClosedLoopCurrent = 0b10,
+    ClosedLoopSpeed = 0b11,
+}
+
+#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
+#[repr(u8)]
+pub(super) enum Direction {
+    Forward = 0b0,
+    Reverse = 0b1,
+}
+
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub(super) struct RunRegister {
+    pub motor_control_mode: MotorControlMode,
+    pub stop_on_fail: bool,
+    /// 0 enables PWM input; otherwise duty cycle D_C = 7 + (n * 3)%
+    pub duty_cycle_control: u8,
+    pub restart_after_loss_of_sync: bool,
+    pub brake_when_inactive: bool,
+    pub direction: Direction,
+    pub enable: bool,
+}
+
+impl Default for RunRegister {
+    fn default() -> Self {
+        Self {
+            motor_control_mode: MotorControlMode::IndirectDutyCycle,
+            stop_on_fail: false,
+            duty_cycle_control: 0,
+            restart_after_loss_of_sync: true,
+            brake_when_inactive: false,
+            direction: Direction::Forward,
+            enable: true,
+        }
+    }
+}
+
+impl A4963Register for RunRegister {
+    const ADDRESS: u16 = 0b1110_0000_0000_0000;
+
+    fn encode(self) -> u16 {
+        assert!(self.duty_cycle_control <= 0b11111);
+
+        let mut result = 0;
+
+        result |= (self.motor_control_mode as u16) << 10;
+        result |= if self.stop_on_fail { 0b0000_0010_0000_0000 } else { 0 };
+        result |= (self.duty_cycle_control as u16) << 4;
+        result |= if self.restart_after_loss_of_sync { 0b0000_0000_0000_1000 } else { 0 };
+        result |= if self.brake_when_inactive { 0b0000_0000_0000_0100 } else { 0 };
+        result |= (self.direction as u16) << 1;
+        result |= if self.enable { 0b0000_0000_0000_0001 } else { 0 };
+
+        result
+    }
+
+    fn decode(value: u16) -> Self {
+        Self {
+            motor_control_mode: MotorControlMode::from_repr(((value >> 10) & 0b11) as u8)
+                // Safety: the enum covers all permutations
+                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
+            stop_on_fail: (value & 0b0000_0010_0000_0000) != 0,
+            duty_cycle_control: ((value >> 4) & 0b11111) as u8,
+            restart_after_loss_of_sync: (value & 0b0000_0000_0000_1000) != 0,
+            brake_when_inactive: (value & 0b0000_0000_0000_0100) != 0,
+            direction: Direction::from_repr(((value >> 1) & 0b1) as u8)
+                // Safety: the enum covers all permutations
+                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
+            enable: (value & 0b0000_0000_0000_0001) != 0,
+        }
+    }
+}
+
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub(super) struct DiagnosticRegister {
+    pub fault_flag: bool,
+    pub power_on_reset: bool,
+    pub serial_transfer_error: bool,
+    pub high_temperature_warning: bool,
+    pub overtemperature_shutdown: bool,
+    pub loss_of_synchronization: bool,
+    pub undervoltage: bool,
+    pub phase_a_high_side_fault: bool,
+    pub phase_a_low_side_fault: bool,
+    pub phase_b_high_side_fault: bool,
+    pub phase_b_low_side_fault: bool,
+    pub phase_c_high_side_fault: bool,
+    pub phase_c_low_side_fault: bool,
+}
+
+#[derive(Clone, Debug, Error, PartialEq, Eq)]
+enum DiagnosticRegisterError {
+    #[error("Unknown Fault")]
+    UnknownFault,
+    #[error("Power On Reset")]
+    PowerOnReset,
+    #[error("Serial Transfer Error")]
+    SerialTransferError,
+    #[error("High Temperature Warning")]
+    HighTemperatureWarning,
+    #[error("Overtemperature Fault")]
+    Overtemperature,
+    #[error("Loss of Synchronization Fault")]
+    LossOfSynchronization,
+    #[error("Undervoltage Fault")]
+    Undervoltage,
+    #[error("Phase A High Side Fault")]
+    PhaseAHighSide,
+    #[error("Phase A Low Side Fault")]
+    PhaseALowSide,
+    #[error("Phase B High Side Fault")]
+    PhaseBHighSide,
+    #[error("Phase B Low Side Fault")]
+    PhaseBLowSide,
+    #[error("Phase C High Side Fault")]
+    PhaseCHighSide,
+    #[error("Phase C Low Side Fault")]
+    PhaseCLowSide,
+}
+
+impl DiagnosticRegister {
+    pub(super) const fn decode(value: u16) -> Self {
+        Self {
+            fault_flag: (value & 0b1000_0000_0000_0000) != 0,
+            power_on_reset: (value & 0b0100_0000_0000_0000) != 0,
+            serial_transfer_error: (value & 0b0010_0000_0000_0000) != 0,
+
+            high_temperature_warning: (value & 0b0000_1000_0000_0000) != 0,
+            overtemperature_shutdown: (value & 0b0000_0100_0000_0000) != 0,
+            loss_of_synchronization: (value & 0b0000_0010_0000_0000) != 0,
+            undervoltage: (value & 0b0000_0000_1000_0000) != 0,
+            phase_a_high_side_fault: (value & 0b0000_0000_0010_0000) != 0,
+            phase_a_low_side_fault: (value & 0b0000_0000_0001_0000) != 0,
+            phase_b_high_side_fault: (value & 0b0000_0000_0000_1000) != 0,
+            phase_b_low_side_fault: (value & 0b0000_0000_0000_0100) != 0,
+            phase_c_high_side_fault: (value & 0b0000_0000_0000_0010) != 0,
+            phase_c_low_side_fault: (value & 0b0000_0000_0000_0001) != 0,
+        }
+    }
+
+    pub fn assert_healthy(self) -> Result<()> {
+        if self.power_on_reset {
+            Err(DiagnosticRegisterError::PowerOnReset.into())
+        } else if self.serial_transfer_error {
+            Err(DiagnosticRegisterError::SerialTransferError.into())
+        } else if self.high_temperature_warning {
+            Err(DiagnosticRegisterError::HighTemperatureWarning.into())
+        } else if self.overtemperature_shutdown {
+            Err(DiagnosticRegisterError::Overtemperature.into())
+        } else if self.loss_of_synchronization {
+            Err(DiagnosticRegisterError::LossOfSynchronization.into())
+        } else if self.undervoltage {
+            Err(DiagnosticRegisterError::Undervoltage.into())
+        } else if self.phase_a_high_side_fault {
+            Err(DiagnosticRegisterError::PhaseAHighSide.into())
+        } else if self.phase_a_low_side_fault {
+            Err(DiagnosticRegisterError::PhaseALowSide.into())
+        } else if self.phase_b_high_side_fault {
+            Err(DiagnosticRegisterError::PhaseBHighSide.into())
+        } else if self.phase_b_low_side_fault {
+            Err(DiagnosticRegisterError::PhaseBLowSide.into())
+        } else if self.phase_c_high_side_fault {
+            Err(DiagnosticRegisterError::PhaseCHighSide.into())
+        } else if self.phase_c_low_side_fault {
+            Err(DiagnosticRegisterError::PhaseCLowSide.into())
+        } else if self.fault_flag {
+            // This shouldn't be possible
+            Err(DiagnosticRegisterError::UnknownFault.into())
+        } else {
+            Ok(())
+        }
+    }
+}

flight/src/hardware/channelization.rs

@@ -7,38 +7,38 @@ use std::any::type_name;
 use std::fmt::Debug;
 use std::time::Instant;
 
-pub const RCS5: PinoutChannel = PinoutChannel::ExtA(0);
-pub const RCS6: PinoutChannel = PinoutChannel::ExtA(1);
-pub const RCS7: PinoutChannel = PinoutChannel::ExtA(2);
-pub const RCS8: PinoutChannel = PinoutChannel::ExtA(3);
-pub const RCS9: PinoutChannel = PinoutChannel::ExtA(4);
-pub const MOTOR0_A: PinoutChannel = PinoutChannel::ExtA(5);
-pub const MOTOR0_B: PinoutChannel = PinoutChannel::ExtA(6);
-pub const LED_A: PinoutChannel = PinoutChannel::ExtA(7);
-pub const RCS0: PinoutChannel = PinoutChannel::ExtA(8);
-pub const RCS1: PinoutChannel = PinoutChannel::ExtA(9);
-pub const DRIVE0_BRAKE: PinoutChannel = PinoutChannel::ExtA(10);
-pub const DRIVE0_DIR: PinoutChannel = PinoutChannel::ExtA(11);
-pub const DRIVE0_DRIVEOFF: PinoutChannel = PinoutChannel::ExtA(12);
-pub const RCS2: PinoutChannel = PinoutChannel::ExtA(13);
-pub const RCS3: PinoutChannel = PinoutChannel::ExtA(14);
-pub const RCS4: PinoutChannel = PinoutChannel::ExtA(15);
-pub const MOTOR1_A: PinoutChannel = PinoutChannel::ExtB(8);
-pub const MOTOR1_B: PinoutChannel = PinoutChannel::ExtB(9);
-pub const MOTOR2_A: PinoutChannel = PinoutChannel::ExtB(10);
-pub const MOTOR2_B: PinoutChannel = PinoutChannel::ExtB(11);
-pub const MOTOR3_A: PinoutChannel = PinoutChannel::ExtB(12);
-pub const MOTOR3_B: PinoutChannel = PinoutChannel::ExtB(13);
-pub const OUT1: PinoutChannel = PinoutChannel::ExtB(14);
-pub const OUT2: PinoutChannel = PinoutChannel::ExtB(15);
-pub const OUT3: PinoutChannel = PinoutChannel::ExtB(0);
-pub const OUT4: PinoutChannel = PinoutChannel::ExtB(1);
-pub const OUT5: PinoutChannel = PinoutChannel::ExtB(2);
-pub const OUT6: PinoutChannel = PinoutChannel::ExtB(3);
-pub const OUT7: PinoutChannel = PinoutChannel::ExtB(4);
-pub const OUT8: PinoutChannel = PinoutChannel::ExtB(5);
-pub const OUT9: PinoutChannel = PinoutChannel::ExtB(6);
-pub const LED_B: PinoutChannel = PinoutChannel::ExtB(7);
+pub const OUT1: PinoutChannel = PinoutChannel::ExtA(0);
+pub const OUT2: PinoutChannel = PinoutChannel::ExtA(1);
+pub const OUT3: PinoutChannel = PinoutChannel::ExtA(2);
+pub const OUT4: PinoutChannel = PinoutChannel::ExtA(3);
+pub const OUT5: PinoutChannel = PinoutChannel::ExtA(4);
+pub const OUT6: PinoutChannel = PinoutChannel::ExtA(5);
+pub const OUT7: PinoutChannel = PinoutChannel::ExtA(6);
+pub const OUT8: PinoutChannel = PinoutChannel::ExtA(7);
+pub const LED_A: PinoutChannel = PinoutChannel::ExtA(8);
+pub const MOTOR1_B: PinoutChannel = PinoutChannel::ExtA(9);
+pub const MOTOR1_A: PinoutChannel = PinoutChannel::ExtA(10);
+pub const MOTOR0_B: PinoutChannel = PinoutChannel::ExtA(11);
+pub const MOTOR0_A: PinoutChannel = PinoutChannel::ExtA(12);
+pub const DRIVE0_BRAKE: PinoutChannel = PinoutChannel::ExtA(13);
+pub const DRIVE0_DIR: PinoutChannel = PinoutChannel::ExtA(14);
+pub const DRIVE0_DRIVEOFF: PinoutChannel = PinoutChannel::ExtA(15);
+pub const RCS1: PinoutChannel = PinoutChannel::ExtB(0);
+pub const RCS0: PinoutChannel = PinoutChannel::ExtB(1);
+pub const OUT9: PinoutChannel = PinoutChannel::ExtB(2);
+pub const MOTOR3_B: PinoutChannel = PinoutChannel::ExtB(3);
+pub const MOTOR3_A: PinoutChannel = PinoutChannel::ExtB(4);
+pub const MOTOR2_B: PinoutChannel = PinoutChannel::ExtB(5);
+pub const MOTOR2_A: PinoutChannel = PinoutChannel::ExtB(6);
+pub const RCS9: PinoutChannel = PinoutChannel::ExtB(7);
+pub const LED_B: PinoutChannel = PinoutChannel::ExtB(8);
+pub const RCS8: PinoutChannel = PinoutChannel::ExtB(9);
+pub const RCS6: PinoutChannel = PinoutChannel::ExtB(10);
+pub const RCS7: PinoutChannel = PinoutChannel::ExtB(11);
+pub const RCS4: PinoutChannel = PinoutChannel::ExtB(12);
+pub const RCS5: PinoutChannel = PinoutChannel::ExtB(13);
+pub const RCS2: PinoutChannel = PinoutChannel::ExtB(14);
+pub const RCS3: PinoutChannel = PinoutChannel::ExtB(15);
 
 #[derive(Copy, Clone, Debug)]
 pub enum PinoutChannel {
@@ -93,3 +93,15 @@ impl PinoutChannel {
         }
     }
 }
+
+#[derive(Copy, Clone, Debug)]
+pub enum PininChannel {
+    In1 = 0,
+    In2 = 1,
+    In3 = 2,
+    In4 = 3,
+    In5 = 4,
+    BatterySense = 5,
+    In6 = 6,
+    In7 = 7,
+}

flight/src/hardware/mcp23017/driver.rs

@@ -18,7 +18,7 @@ where
     i2c: Mutex<I2C>,
     address: u8,
     bank: u16,
-    last_flushed: u16,
+    last_flushed: Option<u16>,
 }
 
 impl<I2C> Debug for Mcp23017Driver<I2C>
@@ -54,7 +54,7 @@ where
             i2c: i2c.into(),
             address,
             bank: 0,
-            last_flushed: 0,
+            last_flushed: None,
         }
     }
 }
@@ -112,14 +112,14 @@ where
 
     fn flush(&mut self) -> anyhow::Result<()> {
         trace!("Mcp23017Driver::flush(self: {self:?})");
-        if self.bank != self.last_flushed {
+        if Some(self.bank) != self.last_flushed {
             let bytes = self.bank.to_le_bytes();
             let data: [u8; _] = [0x12, bytes[0], bytes[1]];
             // This blocks while writing
             if let Ok(mut lock) = self.i2c.lock() {
                 lock.write(self.address, &data).map_err(WrappingError)?;
                 self.i2c.clear_poison();
-                self.last_flushed = self.bank;
+                self.last_flushed = Some(self.bank);
             }
         }
         Ok(())

flight/src/hardware/mcp23017/task.rs

@@ -69,10 +69,10 @@ struct AllPins {
 }
 
 impl AllPins {
-    fn new() -> Self {
+    fn new(default_states: [PinState; 16]) -> Self {
         trace!("AllPins::new()");
         Self {
-            pins: array::repeat(PinData::new(PinState::Low)),
+            pins: array::from_fn(|i| PinData::new(default_states[i])),
         }
     }
 }
@@ -80,11 +80,11 @@ impl AllPins {
 type PinData = CommandedState<PinState>;
 
 impl<'a, M: Mcp23017 + Debug> Mcp23017Task<'a, M> {
-    pub fn new(mcp23017: M, state_vector: &'a StateVector) -> Self {
+    pub fn new(mcp23017: M, state_vector: &'a StateVector, _is_a: bool) -> Self {
         trace!("Mcp23017Task::new(mcp23017: {mcp23017:?})");
         Self {
             mcp23017,
-            pins: AllPins::new(),
+            pins: AllPins::new(array::repeat(PinState::Low)),
             state: state_vector.create_section(Mcp23017State::default()),
         }
     }

flight/src/hardware/mct8316a/driver.rs

@@ -57,8 +57,9 @@ use log::trace;
 use std::any::type_name;
 use std::fmt::{Debug, Display, Formatter};
 use std::sync::Mutex;
+use crate::CRC_8_CCITT;
 
-const CRC: crc::Crc<u8> = crc::Crc::<u8>::new(&crc::CRC_8_SMBUS);
+const CRC: crc::Crc<u8> = crc::Crc::<u8>::new(&CRC_8_CCITT);
 
 #[allow(dead_code)]
 #[derive(Copy, Clone, Eq, PartialEq, Debug)]
@@ -171,15 +172,18 @@ where
         write_data[1..4].copy_from_slice(&control_word(OperationRW::Write, true, data, address));
         let data_length = match data {
             Mct8316AVData::Two(val) => {
-                write_data[4..6].copy_from_slice(&val.to_be_bytes());
+                write_data[4..6].copy_from_slice(&val.to_le_bytes());
                 2
             }
             Mct8316AVData::Four(val) => {
-                write_data[4..8].copy_from_slice(&val.to_be_bytes());
+                write_data[4..8].copy_from_slice(&val.to_le_bytes());
                 4
             }
             Mct8316AVData::Eight(val) => {
-                write_data[4..12].copy_from_slice(&val.to_be_bytes());
+                let first_half = ((val >> 32) & u32::MAX as u64) as u32;
+                let second_half = (val & u32::MAX as u64) as u32;
+                write_data[4..8].copy_from_slice(&first_half.to_le_bytes());
+                write_data[8..12].copy_from_slice(&second_half.to_le_bytes());
                 8
             }
         };
@@ -239,21 +243,21 @@ where
 
         match data {
             Mct8316AVData::Two(val) => {
-                *val = u16::from_be_bytes([read_data[5], read_data[6]]);
+                *val = u16::from_le_bytes([read_data[5], read_data[6]]);
             }
             Mct8316AVData::Four(val) => {
-                *val = u32::from_be_bytes([read_data[5], read_data[6], read_data[7], read_data[8]]);
+                *val = u32::from_le_bytes([read_data[5], read_data[6], read_data[7], read_data[8]]);
             }
             Mct8316AVData::Eight(val) => {
-                *val = u64::from_be_bytes([
-                    read_data[5],
-                    read_data[6],
-                    read_data[7],
-                    read_data[8],
+                *val = u64::from_le_bytes([
                     read_data[9],
                     read_data[10],
                     read_data[11],
                     read_data[12],
+                    read_data[5],
+                    read_data[6],
+                    read_data[7],
+                    read_data[8],
                 ]);
             }
         }

flight/src/hardware/mod.rs

@@ -1,24 +1,29 @@
 use crate::hardware::mcp23017::Mcp23017;
-use crate::hardware::mct8316a::Mct8316a;
 use anyhow::Result;
 use embedded_hal::pwm::SetDutyCycle;
 use log::trace;
 use std::fmt::Debug;
+use crate::hardware::a4963::A4963;
 
 pub trait Hardware {
     type Mcp23017<'a>: Mcp23017 + Send + Debug
     where
         Self: 'a;
-    type Pwm: SetDutyCycle<Error: std::error::Error + Sync + Send> + Sync;
+    type Pwm: SetDutyCycle<Error: std::error::Error + Sync + Send> + Sync + Send;
 
     fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>>;
     fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>>;
 
     fn new_pwm0(&self) -> Result<Self::Pwm>;
 
-    fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync>;
+    // fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync>;
+    fn new_a4963<'a, 'b>(&'a mut self) -> Result<impl A4963 + Send + 'b> ;
 
     fn get_battery_voltage(&self) -> Result<f64>;
+
+    // fn get_fault(&self) -> Result<bool>;
+
+    // fn get_fg(&self) -> Result<bool>;
 }
 
 #[cfg(feature = "raspi")]
@@ -43,6 +48,7 @@ pub mod channelization;
 pub mod mcp23017;
 #[cfg(feature = "raspi")]
 mod mcp3208;
-pub mod mct8316a;
+// pub mod mct8316a;
 pub mod pin;
 mod sim;
+pub mod a4963;

flight/src/hardware/raspi/mod.rs

@@ -3,28 +3,32 @@ mod pwm;
 use crate::hardware::Hardware;
 use crate::hardware::mcp3208::Mcp3208;
 use crate::hardware::mcp23017::Mcp23017Driver;
-use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a};
 use crate::hardware::raspi::pwm::PwmWrapper;
-use crate::hardware::sim::mct8316a::SimMct8316a;
-use anyhow::Result;
+use anyhow::{anyhow, Result};
 use embedded_hal_bus::i2c::MutexDevice;
 use log::{debug, info, trace};
-use rpi_pal::gpio::Gpio;
 use rpi_pal::i2c::I2c;
 use rpi_pal::pwm::Pwm;
 use rpi_pal::spi::SimpleHalSpiDevice;
 use rpi_pal::spi::{Bus, Mode, SlaveSelect, Spi};
 use std::cell::RefCell;
 use std::sync::Mutex;
+use rpi_pal::gpio::{Gpio, InputPin};
+use crate::hardware::a4963::{A4963Driver, A4963};
+use crate::hardware::channelization::PininChannel;
 
 const CLOCK_1MHZ: u32 = 1_000_000;
 
 #[derive(Debug)]
 pub struct RaspiHardware {
-    _gpio: Gpio,
+    // gpio: Gpio,
+    // fault: InputPin,
+    // fg: InputPin,
     i2c_bus: Mutex<I2c>,
     mcp3208: RefCell<Mcp3208<SimpleHalSpiDevice>>,
-    mct8316a: Mutex<SimMct8316a>,
+    // a4963: RefCell<A4963Driver<SimpleHalSpiDevice>>,
+    a4963: Option<A4963Driver<SimpleHalSpiDevice>>,
+    // mct8316a: Mutex<SimMct8316a>,
 }
 
 impl RaspiHardware {
@@ -35,8 +39,17 @@ impl RaspiHardware {
         info!("Running on {}", device.model());
         debug!("SOC: {}", device.soc());
 
+        // let gpio = Gpio::new()?;
+
+        // let mut fault = gpio.get(25)?.into_input();
+        // fault.set_reset_on_drop(false);
+        // let mut fg = gpio.get(24)?.into_input();
+        // fg.set_reset_on_drop(false);
+
         Ok(Self {
-            _gpio: Gpio::new()?,
+            // gpio,
+            // fault,
+            // fg,
             i2c_bus: Mutex::new(I2c::with_bus(0u8)?),
             mcp3208: Mcp3208::new(
                 SimpleHalSpiDevice::new(Spi::new(
@@ -46,9 +59,14 @@ impl RaspiHardware {
                     Mode::Mode0,
                 )?),
                 3.3f64,
-            )
-            .into(),
-            mct8316a: SimMct8316a::new().into(),
+            ).into(),
+            a4963: Some(A4963Driver::new(SimpleHalSpiDevice::new(Spi::new(
+                Bus::Spi0,
+                SlaveSelect::Ss0,
+                CLOCK_1MHZ,
+                Mode::Mode3,
+            )?))),
+            // mct8316a: Mct8316AVDriver::new().into(),
             // mct8316a: SimMct8316a::new().into(),
         })
     }
@@ -83,16 +101,36 @@ impl Hardware for RaspiHardware {
         Ok(PwmWrapper::new(Pwm::with_pwmchip(PWMCHIP, CHANNEL)?)?)
     }
 
-    fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
-        trace!("RaspiHardware::new_mct8316a(self: {self:?})");
-        Ok(Mct8316AVDriver::new(
-            MutexDevice::new(&self.mct8316a),
-            0b0000000,
-        ))
+    // fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
+    //     trace!("RaspiHardware::new_mct8316a(self: {self:?})");
+    //     Ok(Mct8316AVDriver::new(
+    //         MutexDevice::new(&self.mct8316a),
+    //         // MutexDevice::new(&self.i2c_bus),
+    //         0b0000000,
+    //     ))
+    // }
+
+    fn new_a4963<'a, 'b>(&'a mut self) -> Result<impl A4963 + Send + 'b> {
+        trace!("RaspiHardware::new_a4963(self: {self:?})");
+        Ok(self.a4963.take().ok_or_else(|| anyhow!("Cannot construct two A4963 instances"))?)
     }
 
     fn get_battery_voltage(&self) -> Result<f64> {
+        const LOW_RESISTOR: f64 = 100.0;
+        const HIGH_RESISTOR: f64 = 374.0;
         trace!("RaspiHardware::get_battery_voltage(self: {self:?})");
-        self.mcp3208.borrow_mut().read_single(1)
+        let sensed = self.mcp3208.borrow_mut().read_single(PininChannel::BatterySense as u8)?;
+        let battery = sensed * (HIGH_RESISTOR + LOW_RESISTOR) / LOW_RESISTOR;
+        Ok(battery)
     }
+
+    // fn get_fault(&self) -> Result<bool> {
+    //     trace!("RaspiHardware::get_fault(self: {self:?})");
+    //     Ok(self.fault.is_high())
+    // }
+
+    // fn get_fg(&self) -> Result<bool> {
+    //     trace!("RaspiHardware::get_fg(self: {self:?})");
+    //     Ok(self.fg.is_high())
+    // }
 }

flight/src/hardware/sim/hardware.rs

@@ -68,4 +68,12 @@ impl Hardware for SimHardware {
         trace!("SimHardware::get_battery_voltage(self: {self:?})");
         Ok(self.battery_voltage)
     }
+
+    // fn get_nfault(&self) -> anyhow::Result<bool> {
+    //     Ok(true)
+    // }
+    //
+    // fn get_fg(&self) -> anyhow::Result<bool> {
+    //     Ok(true)
+    // }
 }

flight/src/hardware/sim/mct8316a.rs

@@ -3,8 +3,10 @@ use anyhow::anyhow;
 use embedded_hal::i2c::{ErrorType, I2c, Operation, SevenBitAddress};
 use log::trace;
 use std::collections::HashMap;
+use crate::CRC_8_CCITT;
+use log::warn;
 
-const CRC: crc::Crc<u8> = crc::Crc::<u8>::new(&crc::CRC_8_SMBUS);
+const CRC: crc::Crc<u8> = crc::Crc::<u8>::new(&CRC_8_CCITT);
 
 #[derive(Debug)]
 pub struct SimMct8316a {
@@ -67,7 +69,7 @@ impl I2c for SimMct8316a {
                         if data_length == 2 {
                             todo!("Unimplemented");
                         } else {
-                            let written_value = u32::from_be_bytes([
+                            let written_value = u32::from_le_bytes([
                                 write_buffer[3],
                                 write_buffer[4],
                                 write_buffer[5],
@@ -85,6 +87,7 @@ impl I2c for SimMct8316a {
                             3
                         )));
                     }
+                    crc.update(&[(i2c_addr << 1) | 0b0]);
                     crc.update(write_buffer);
                 }
                 Operation::Read(read_buffer) => {
@@ -104,7 +107,7 @@ impl I2c for SimMct8316a {
                         todo!("Unimplemented");
                     } else if data_length == 4 {
                         let value = *self.data.get(&address).unwrap_or(&0);
-                        read_buffer[0..4].copy_from_slice(&value.to_be_bytes());
+                        read_buffer[0..4].copy_from_slice(&value.to_le_bytes());
                     } else {
                         todo!("Unimplemented");
                     }

flight/src/hardware/sim/mod.rs

@@ -3,7 +3,8 @@ mod mcp23017;
 #[cfg(not(feature = "raspi"))]
 mod pwm;
 
-pub(super) mod mct8316a;
+// #[cfg(not(feature = "raspi"))]
+// pub mod mct8316a;
 
 #[cfg(not(feature = "raspi"))]
 pub mod hardware;

flight/src/lib.rs

@@ -1,26 +1,31 @@
 #![warn(clippy::all, clippy::pedantic)]
 
+use std::f64::consts::{PI, TAU};
 use crate::comms::{CommsState, CommsTask};
 use crate::hardware::Hardware;
 use crate::hardware::initialize;
 use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task};
-use crate::hardware::mct8316a::Mct8316a;
 use crate::hardware::pin::PinDevice;
 use crate::rcs::RcsTask;
 use crate::scheduler::Scheduler;
 use crate::state_vector::StateVector;
 use anyhow::Result;
-use embedded_hal::pwm::SetDutyCycle;
-use log::info;
+use embedded_hal::pwm::{ErrorType, SetDutyCycle};
+use log::{error, info};
 use nautilus_common::add_ctrlc_handler_arc;
 use nautilus_common::telemetry::{SwitchBank, TelemetryMessage};
 use std::sync::Arc;
 use std::sync::atomic::{AtomicBool, Ordering};
 use std::thread::sleep;
 use std::time::Duration;
+use crc::Algorithm;
+use nautilus_common::command::set_pwm::SetPwm;
+use crate::hardware::a4963::A4963;
 
 mod hardware;
 
+pub const CRC_8_CCITT: Algorithm<u8> = Algorithm { width: 8, poly: 0x07, init: 0xFF, refin: false, refout: false, xorout: 0x00, check: 0xfb, residue: 0xFF };
+
 fn new_shutdown_handler(running: &Arc<AtomicBool>) -> impl Fn(()) {
     let running = running.clone();
     move |()| running.store(false, Ordering::Relaxed)
@@ -41,32 +46,34 @@ pub fn run() -> Result<()> {
 
     let state_vector = StateVector::new();
 
-    let hal = initialize()?;
+    let mut hal = initialize()?;
 
+    let mut a4963 = hal.new_a4963()?;
     let mut mcp23017_a = hal.new_mcp23017_a()?;
     let mut mcp23017_b = hal.new_mcp23017_b()?;
     let mut pwm0 = hal.new_pwm0()?;
-    let mut mct8316 = hal.new_mct8316a()?;
 
     info!("Battery Voltage: {}", hal.get_battery_voltage()?);
 
-    pwm0.set_duty_cycle_percent(100)?;
+    pwm0.set_duty_cycle_percent(0)?;
 
     mcp23017_a.init()?;
     mcp23017_b.init()?;
-    mct8316.init()?;
+    a4963.init()?;
+    
+    pwm0.set_duty_cycle_percent(100)?;
 
     Scheduler::scope(running.clone(), |s| {
         let task_a = s.run_cyclic(
             "mcp23017-a-task",
-            Mcp23017Task::new(mcp23017_a, &state_vector),
+            Mcp23017Task::new(mcp23017_a, &state_vector, true),
             10,
         )?;
         let a_id = task_a.get_id();
 
         let task_b = s.run_cyclic(
             "mcp23017-b-task",
-            Mcp23017Task::new(mcp23017_b, &state_vector),
+            Mcp23017Task::new(mcp23017_b, &state_vector, false),
             10,
         )?;
         let b_id = task_b.get_id();
@@ -78,6 +85,14 @@ pub fn run() -> Result<()> {
         comms.add_command_handler("/mcp23017a/set", task_a.new_set_pin_callback())?;
         comms.add_command_handler("/mcp23017b/set", task_b.new_set_pin_callback())?;
         comms.add_command_handler("/rcs/set", rcs.new_set_rcs_callback())?;
+        comms.add_command_handler("/pwm/set", move |pwm: SetPwm| {
+            match pwm0.set_duty_cycle_percent(pwm.duty_cycle) {
+                Ok(_) => {}
+                Err(err) => {
+                    error!("Failed to set PWM duty cycle: {err}");
+                }
+            }
+        })?;
         let comms = s.run_cyclic("comms-task", comms, 10)?;
         let comms_id = *comms;
 
@@ -105,8 +120,10 @@ pub fn run() -> Result<()> {
         )?;
 
         info!("Starting Main Loop");
+
         while running.load(Ordering::Relaxed) {
             sleep(Duration::from_millis(100));
+            a4963.check_health()?;
         }
 
         anyhow::Ok(())
@@ -116,8 +133,8 @@ pub fn run() -> Result<()> {
 
     // Explicitly drop these to allow the borrow checker to be sure that
     // dropping the hal is safe
-    drop(pwm0);
-    drop(mct8316);
+    // drop(pwm0);
+    // drop(mct8316);
 
     drop(hal);