initial work on mct8316a

flight/Cargo.toml

@@ -5,17 +5,18 @@ edition = "2024"
 
 [dependencies]
 anyhow = "1.0.97"
-fern = "0.7.1"
-log = "0.4.27"
-chrono = "0.4.40"
+fern = { version = "0.7.1", features = ["colored"] }
+log = "0.4.28"
+chrono = "0.4.42"
 embedded-hal = "1.0.0"
 embedded-hal-bus = { version = "0.3.0", features = ["std"] }
 embedded-hal-mock = { version = "0.11.1", optional = true }
 rpi-pal = { version = "0.22.2", features = ["hal"], optional = true }
-nalgebra = "0.33.2"
+nalgebra = "0.34.1"
 hex = "0.4.3"
-thiserror = "2.0.12"
+thiserror = "2.0.16"
 num-traits = "0.2.19"
+crc = "3.3.0"
 
 [dev-dependencies]
 embedded-hal-mock = { version = "0.11.1" }

flight/src/hardware/mcp23017/driver.rs

@@ -42,7 +42,7 @@ where
     I2C::Error: 'static,
 {
     pub fn new(i2c: I2C, address: u8) -> Self {
-        trace!("Mcp23017Driver::new(i2c, address: {address:07b})");
+        trace!("Mcp23017Driver::new(i2c, address: 0x{address:02x})");
         Self {
             i2c: i2c.into(),
             address,

flight/src/hardware/mct8316a/closed_loop.rs

@@ -0,0 +1,485 @@
+#![allow(dead_code)]
+
+use crate::hardware::mct8316a::motor_startup::{EnableDisable, FullCurrentThreshold};
+
+#[derive(Debug, Clone)]
+pub struct ClosedLoop1 {
+    pub commutation_mode: CommutationMode,
+    pub closed_loop_acceleration_rate: ClosedLoopRate,
+    pub closed_loop_deceleration_mode: ClosedLoopDecelerationMode,
+    pub closed_loop_deceleration_rate: ClosedLoopRate,
+    pub pwm_frequency: PwmFrequency,
+    pub pwm_modulation: PwmModulation,
+    pub pwm_mode: PwmMode,
+    pub lead_angle_polarity: LeadAnglePolarity,
+    pub lead_angle: f32,
+}
+
+impl Default for ClosedLoop1 {
+    fn default() -> Self {
+        Self {
+            commutation_mode: CommutationMode::Degrees120,
+            closed_loop_acceleration_rate: ClosedLoopRate::VoltsPerSecond0_005,
+            closed_loop_deceleration_mode: ClosedLoopDecelerationMode::DecelerationRate,
+            closed_loop_deceleration_rate: ClosedLoopRate::VoltsPerSecond0_005,
+            pwm_frequency: PwmFrequency::Kilohertz5,
+            pwm_modulation: PwmModulation::HighSide,
+            pwm_mode: PwmMode::SingleEnded,
+            lead_angle_polarity: LeadAnglePolarity::Negative,
+            lead_angle: 0.0,
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum CommutationMode {
+    Degrees120 = 0x0,
+    Variable120to150 = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum ClosedLoopRate {
+    VoltsPerSecond0_005 = 0x00,
+    VoltsPerSecond0_01 = 0x01,
+    VoltsPerSecond0_025 = 0x02,
+    VoltsPerSecond0_05 = 0x03,
+    VoltsPerSecond0_1 = 0x04,
+    VoltsPerSecond0_25 = 0x05,
+    VoltsPerSecond0_5 = 0x06,
+    VoltsPerSecond1 = 0x07,
+    VoltsPerSecond2_5 = 0x08,
+    VoltsPerSecond5 = 0x09,
+    VoltsPerSecond7_5 = 0x0A,
+    VoltsPerSecond10 = 0x0B,
+    VoltsPerSecond12_5 = 0x0C,
+    VoltsPerSecond15 = 0x0D,
+    VoltsPerSecond20 = 0x0E,
+    VoltsPerSecond30 = 0x0F,
+    VoltsPerSecond40 = 0x10,
+    VoltsPerSecond50 = 0x11,
+    VoltsPerSecond60 = 0x12,
+    VoltsPerSecond75 = 0x13,
+    VoltsPerSecond100 = 0x14,
+    VoltsPerSecond125 = 0x15,
+    VoltsPerSecond150 = 0x16,
+    VoltsPerSecond175 = 0x17,
+    VoltsPerSecond200 = 0x18,
+    VoltsPerSecond250 = 0x19,
+    VoltsPerSecond300 = 0x1A,
+    VoltsPerSecond400 = 0x1B,
+    VoltsPerSecond500 = 0x1C,
+    VoltsPerSecond750 = 0x1D,
+    VoltsPerSecond1000 = 0x1E,
+    VoltsPerSecond32767 = 0x1F,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum ClosedLoopDecelerationMode {
+    DecelerationRate = 0x0,
+    AccerlerationRate = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum PwmFrequency {
+    Kilohertz5 = 0x00,
+    Kilohertz6 = 0x01,
+    Kilohertz7 = 0x02,
+    Kilohertz8 = 0x03,
+    Kilohertz9 = 0x04,
+    Kilohertz10 = 0x05,
+    Kilohertz11 = 0x06,
+    Kilohertz12 = 0x07,
+    Kilohertz13 = 0x08,
+    Kilohertz14 = 0x09,
+    Kilohertz15 = 0x0A,
+    Kilohertz16 = 0x0B,
+    Kilohertz17 = 0x0C,
+    Kilohertz18 = 0x0D,
+    Kilohertz19 = 0x0E,
+    Kilohertz20 = 0x0F,
+    Kilohertz25 = 0x10,
+    Kilohertz30 = 0x11,
+    Kilohertz35 = 0x12,
+    Kilohertz40 = 0x13,
+    Kilohertz45 = 0x14,
+    Kilohertz50 = 0x15,
+    Kilohertz55 = 0x16,
+    Kilohertz60 = 0x17,
+    Kilohertz65 = 0x18,
+    Kilohertz70 = 0x19,
+    Kilohertz75 = 0x1A,
+    Kilohertz80 = 0x1B,
+    Kilohertz85 = 0x1C,
+    Kilohertz90 = 0x1D,
+    Kilohertz95 = 0x1E,
+    Kilohertz100 = 0x1F,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum PwmModulation {
+    HighSide = 0x0,
+    LowSide = 0x1,
+    Mixed = 0x2,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum PwmMode {
+    SingleEnded = 0x0,
+    Complementary = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum LeadAnglePolarity {
+    Negative = 0x0,
+    Positive = 0x1,
+}
+
+#[derive(Debug, Clone)]
+pub struct ClosedLoop2 {
+    pub speed_feedback_mode: SpeedFeedbackMode,
+    pub speed_feedback_division: SpeedFeedbackDivision,
+    pub speed_feedback_config: SpeedFeedbackConfig,
+    pub bemf_threshold: BemfThreshold,
+    pub motor_stop_mode: MotorStopMode,
+    pub motor_stop_brake_time: MotorStopBrakeTime,
+    pub active_low_high_brake_threshold: DutyCycleThreshold,
+    pub brake_pin_threshold: DutyCycleThreshold,
+    pub avs_enable: EnableDisable,
+    pub cycle_current_limit: FullCurrentThreshold,
+}
+
+impl Default for ClosedLoop2 {
+    fn default() -> Self {
+        Self {
+            speed_feedback_mode: SpeedFeedbackMode::Always,
+            speed_feedback_division: SpeedFeedbackDivision::Pole2Divide3,
+            speed_feedback_config: SpeedFeedbackConfig::AboveBEMFThreshold,
+            bemf_threshold: BemfThreshold::MilliVolt1,
+            motor_stop_mode: MotorStopMode::HighImpedance,
+            motor_stop_brake_time: MotorStopBrakeTime::Milliseconds1,
+            active_low_high_brake_threshold: DutyCycleThreshold::Immediate,
+            brake_pin_threshold: DutyCycleThreshold::Immediate,
+            avs_enable: EnableDisable::Disable,
+            cycle_current_limit: FullCurrentThreshold::NotApplicable,
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum SpeedFeedbackMode {
+    Always = 0x0,
+    ClosedLoopOnly = 0x1,
+    FirstOpenLoop = 0x2,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum SpeedFeedbackDivision {
+    Pole2Divide3 = 0x0,
+    Pole2 = 0x1,
+    Pole4 = 0x2,
+    Pole6 = 0x3,
+    Pole8 = 0x4,
+    Pole10 = 0x5,
+    Pole12 = 0x6,
+    Pole14 = 0x7,
+    Pole16 = 0x8,
+    Pole18 = 0x9,
+    Pole20 = 0xA,
+    Pole22 = 0xB,
+    Pole24 = 0xC,
+    Pole26 = 0xD,
+    Pole28 = 0xE,
+    Pole30 = 0xF,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum SpeedFeedbackConfig {
+    AboveBEMFThreshold = 0x0,
+    WhenDriven = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum BemfThreshold {
+    MilliVolt1 = 0x0,
+    MilliVolt2 = 0x1,
+    MilliVolt5 = 0x2,
+    MilliVolt10 = 0x3,
+    MilliVolt20 = 0x4,
+    MilliVolt30 = 0x5,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum MotorStopMode {
+    HighImpedance = 0x0,
+    Recirculation = 0x1,
+    LowSideBraking = 0x2,
+    HighSideBraking = 0x3,
+    ActiveSpinDown = 0x4,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum MotorStopBrakeTime {
+    Milliseconds1 = 0x0,
+    Milliseconds2 = 0x1,
+    Milliseconds5 = 0x2,
+    Milliseconds10 = 0x3,
+    Milliseconds15 = 0x4,
+    Milliseconds25 = 0x5,
+    Milliseconds50 = 0x6,
+    Milliseconds75 = 0x7,
+    Milliseconds100 = 0x8,
+    Milliseconds250 = 0x9,
+    Milliseconds500 = 0xA,
+    Milliseconds1000 = 0xB,
+    Milliseconds2500 = 0xC,
+    Milliseconds5000 = 0xD,
+    Milliseconds10000 = 0xE,
+    Milliseconds15000 = 0xF,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum DutyCycleThreshold {
+    Immediate = 0x0,
+    Percent50 = 0x1,
+    Percent25 = 0x2,
+    Percent15 = 0x3,
+    Percent10 = 0x4,
+    Percent7_5 = 0x5,
+    Percent5 = 0x6,
+    Percent2_5 = 0x7,
+}
+
+
+#[derive(Debug, Clone)]
+pub struct ClosedLoop3 {
+    pub degauss_samples: DegaussSamples,
+    pub degauss_upper_bound: DegaussUpperBound,
+    pub degauss_lower_bound: DegaussLowerBound,
+    pub integration_cycle_low_threshold: IntegrationCycleLowThreshold,
+    pub integration_cycle_high_threshold: IntegrationCycleHighThreshold,
+    pub integration_duty_cycle_low_threshold: IntegrationDutyCycleThreshold,
+    pub integration_duty_cycle_high_threshold: IntegrationDutyCycleThreshold,
+    pub bemf_threshold1: IntegrationBemfThreshold,
+    pub bemf_threshold2: IntegrationBemfThreshold,
+    pub commutation_method: CommutationMethod,
+    pub degauss_window: DegaussWindow,
+    pub degauss_enable: EnableDisable,
+}
+
+impl Default for ClosedLoop3 {
+    fn default() -> Self {
+        Self {
+            degauss_samples: DegaussSamples::Samples2,
+            degauss_upper_bound: DegaussUpperBound::Volts0_15,
+            degauss_lower_bound: DegaussLowerBound::Volts0_09,
+            integration_cycle_low_threshold: IntegrationCycleLowThreshold::Samples3,
+            integration_cycle_high_threshold: IntegrationCycleHighThreshold::Samples4,
+            integration_duty_cycle_low_threshold: IntegrationDutyCycleThreshold::Percent12,
+            integration_duty_cycle_high_threshold: IntegrationDutyCycleThreshold::Percent12,
+            bemf_threshold1: IntegrationBemfThreshold::Value0,
+            bemf_threshold2: IntegrationBemfThreshold::Value0,
+            commutation_method: CommutationMethod::ZC,
+            degauss_window: DegaussWindow::Degrees22_5,
+            degauss_enable: EnableDisable::Disable,
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum DegaussSamples {
+    Samples2 = 0x0,
+    Samples3 = 0x1,
+    Samples4 = 0x2,
+    Samples5 = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum DegaussUpperBound {
+    Volts0_09 = 0x0,
+    Volts0_12 = 0x1,
+    Volts0_15 = 0x2,
+    Volts0_18 = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum DegaussLowerBound {
+    Volts0_03 = 0x0,
+    Volts0_06 = 0x1,
+    Volts0_09 = 0x2,
+    Volts0_12 = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IntegrationCycleLowThreshold {
+    Samples3 = 0x0,
+    Samples4 = 0x1,
+    Samples6 = 0x2,
+    Samples8 = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IntegrationCycleHighThreshold {
+    Samples4 = 0x0,
+    Samples6 = 0x1,
+    Samples8 = 0x2,
+    Samples10 = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IntegrationDutyCycleThreshold {
+    Percent12 = 0x0,
+    Percent15 = 0x1,
+    Percent18 = 0x2,
+    Percent20 = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IntegrationBemfThreshold {
+    Value0 = 0x000,
+    Value25 = 0x001,
+    Value50 = 0x002,
+    Value75 = 0x003,
+    Value100 = 0x004,
+    Value125 = 0x005,
+    Value150 = 0x006,
+    Value175 = 0x007,
+    Value200 = 0x008,
+    Value225 = 0x009,
+    Value250 = 0x00A,
+    Value275 = 0x00B,
+    Value300 = 0x00C,
+    Value325 = 0x00D,
+    Value350 = 0x00E,
+    Value375 = 0x00F,
+    Value400 = 0x010,
+    Value425 = 0x011,
+    Value450 = 0x012,
+    Value475 = 0x013,
+    Value500 = 0x014,
+    Value525 = 0x015,
+    Value550 = 0x016,
+    Value575 = 0x017,
+    Value600 = 0x018,
+    Value625 = 0x019,
+    Value650 = 0x01A,
+    Value675 = 0x01B,
+    Value700 = 0x01C,
+    Value725 = 0x01D,
+    Value750 = 0x01E,
+    Value775 = 0x01F,
+    Value800 = 0x020,
+    Value850 = 0x021,
+    Value900 = 0x022,
+    Value950 = 0x023,
+    Value1000 = 0x024,
+    Value1050 = 0x025,
+    Value1100 = 0x026,
+    Value1150 = 0x027,
+    Value1200 = 0x028,
+    Value1250 = 0x029,
+    Value1300 = 0x02A,
+    Value1350 = 0x02B,
+    Value1400 = 0x02C,
+    Value1450 = 0x02D,
+    Value1500 = 0x02E,
+    Value1550 = 0x02F,
+    Value1600 = 0x030,
+    Value1700 = 0x031,
+    Value1800 = 0x032,
+    Value1900 = 0x033,
+    Value2000 = 0x034,
+    Value2100 = 0x035,
+    Value2200 = 0x036,
+    Value2300 = 0x037,
+    Value2400 = 0x038,
+    Value2600 = 0x039,
+    Value2800 = 0x03A,
+    Value3000 = 0x03B,
+    Value3200 = 0x03C,
+    Value3400 = 0x03D,
+    Value3600 = 0x03E,
+    Value3800 = 0x03F,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum CommutationMethod {
+    ZC = 0x0,
+    Integration = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum DegaussWindow {
+    Degrees22_5 = 0x0,
+    Degrees10 = 0x1,
+    Degrees15 = 0x2,
+    Degrees18 = 0x3,
+    Degrees30 = 0x4,
+    Degrees37_5 = 0x5,
+    Degrees45 = 0x6,
+    Degrees60 = 0x7,
+}
+
+#[derive(Debug, Clone)]
+pub struct ClosedLoop4 {
+    pub wcomp_blanking: EnableDisable,
+    pub fast_deceleration_duty_window: LowerPercentLimit,
+    pub fast_deceleration_duty_threshold: UpperPercentLimit,
+    pub dynamic_brake_current_lower_threshold: FullCurrentThreshold,
+    pub dynamic_braking_current: EnableDisable,
+    pub fast_deceleration: EnableDisable,
+    pub fast_deceleration_current_theshold: FullCurrentThreshold,
+    pub fast_brake_delta: FastBrakeDelta,
+}
+
+impl Default for ClosedLoop4 {
+    fn default() -> Self {
+        Self {
+            wcomp_blanking: EnableDisable::Disable,
+            fast_deceleration_duty_window: LowerPercentLimit::Percent0,
+            fast_deceleration_duty_threshold: UpperPercentLimit::Percent100,
+            dynamic_brake_current_lower_threshold: FullCurrentThreshold::NotApplicable,
+            dynamic_braking_current: EnableDisable::Disable,
+            fast_deceleration: EnableDisable::Disable,
+            fast_deceleration_current_theshold: FullCurrentThreshold::NotApplicable,
+            fast_brake_delta: FastBrakeDelta::Percent0_5,
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum LowerPercentLimit {
+    Percent0 = 0x0,
+    Percent2_5 = 0x1,
+    Percent5 = 0x2,
+    Percent7_5 = 0x3,
+    Percent10 = 0x4,
+    Percent15 = 0x5,
+    Percent20 = 0x6,
+    Percent25 = 0x7,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum UpperPercentLimit {
+    Percent100 = 0x0,
+    Percent95 = 0x1,
+    Percent90 = 0x2,
+    Percent85 = 0x3,
+    Percent80 = 0x4,
+    Percent75 = 0x5,
+    Percent70 = 0x6,
+    Percent65 = 0x7,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum FastBrakeDelta {
+    Percent0_5 = 0x0,
+    Percent1 = 0x1,
+    Percent1_5 = 0x2,
+    Percent2 = 0x3,
+    Percent2_5 = 0x4,
+    Percent3 = 0x5,
+    Percent4 = 0x6,
+    Percent5 = 0x7,
+}
+
+

flight/src/hardware/mct8316a/constant_power.rs

@@ -0,0 +1,40 @@
+#![allow(dead_code)]
+
+use crate::hardware::mct8316a::motor_startup::EnableDisable;
+
+#[derive(Debug, Clone)]
+pub struct ConstantPower {
+    pub max_speed: f64,
+    pub dead_time_compensation: EnableDisable,
+    pub max_power: f64,
+    pub power_hysteresis: PowerHysteresis,
+    pub mode: PowerMode,
+}
+
+impl Default for ConstantPower {
+    fn default() -> Self {
+        Self {
+            max_speed: 0.0,
+            dead_time_compensation: EnableDisable::Disable,
+            max_power: 0.0,
+            power_hysteresis: PowerHysteresis::Percent5,
+            mode: PowerMode::Disabled,
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum PowerHysteresis {
+    Percent5 = 0x0,
+    Percent7_5 = 0x1,
+    Percent10 = 0x2,
+    Percent12_5 = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum PowerMode {
+    Disabled = 0x0,
+    ClosedLoop = 0x1,
+    PowerLimit = 0x2,
+}
+

flight/src/hardware/mct8316a/constant_speed.rs

@@ -0,0 +1,31 @@
+#![allow(dead_code)]
+
+use crate::hardware::mct8316a::closed_loop::{LowerPercentLimit, UpperPercentLimit};
+
+#[derive(Debug, Clone)]
+pub struct ConstantSpeed {
+    pub speed_power_kp: f64,
+    pub speed_power_ki: f64,
+    pub speed_power_upper_limit: UpperPercentLimit,
+    pub speed_power_lower_limit: LowerPercentLimit,
+    pub mode: ClosedLoopMode,
+}
+
+impl Default for ConstantSpeed {
+    fn default() -> Self {
+        Self {
+            speed_power_kp: 0.0,
+            speed_power_ki: 0.0,
+            speed_power_upper_limit: UpperPercentLimit::Percent100,
+            speed_power_lower_limit: LowerPercentLimit::Percent0,
+            mode: ClosedLoopMode::Disabled,
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum ClosedLoopMode {
+    Disabled = 0x0,
+    SpeedLoop = 0x1,
+    PowerLoop = 0x2,
+}

flight/src/hardware/mct8316a/driver.rs

@@ -0,0 +1,285 @@
+use std::fmt::{Debug, Display, Formatter};
+use std::sync::Mutex;
+use embedded_hal::i2c::{I2c, Operation};
+use log::trace;
+use crate::hardware::mct8316a::Mct8316a;
+use anyhow::{bail, ensure, Result};
+use crate::hardware::error::I2cError;
+use crate::hardware::mct8316a::closed_loop::{ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4};
+use crate::hardware::mct8316a::constant_power::ConstantPower;
+use crate::hardware::mct8316a::constant_speed::ConstantSpeed;
+use crate::hardware::mct8316a::eeprom::Mct8316AVEeprom;
+use crate::hardware::mct8316a::isd_config::{BrakeConfig, BrakeMode, IsdConfig, IsdConfigTimeValue, ResyncMinimumThreshold, StartupBreakTime, Threshold};
+use crate::hardware::mct8316a::motor_startup::{AlignRampRate, AlignTime, FullCurrentThreshold, IpdAdvanceAngle, IpdClockFrequency, IpdCurrentThreshold, IpdReleaseMode, IpdRepeat, MotorStartup1, MotorStartup2, MotorStartupMethod, SlowFirstCycleFrequency};
+use crate::hardware::mct8316a::phase_profile::{ThreePhase150DegreeProfile, TwoPhase150DegreeProfile};
+use crate::hardware::mct8316a::trap_config::{TrapConfig1, TrapConfig2};
+
+const CRC: crc::Crc<u8> = crc::Crc::<u8>::new(&crc::CRC_8_SMBUS);
+
+#[allow(dead_code)]
+#[derive(Copy, Clone, Eq, PartialEq, Debug)]
+pub(super) enum Mct8316AVData {
+    Two(u16),
+    Four(u32),
+    Eight(u64),
+}
+
+impl Display for Mct8316AVData {
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        match self {
+            Mct8316AVData::Two(val) => write!(f, "{val:04x}"),
+            Mct8316AVData::Four(val) => write!(f, "{val:08x}"),
+            Mct8316AVData::Eight(val) => write!(f, "{val:016x}"),
+        }
+    }
+}
+
+enum OperationRW {
+    Read,
+    Write,
+}
+
+fn control_word(
+    operation_rw: OperationRW,
+    crc: bool,
+    data: Mct8316AVData,
+    address: u32,
+) -> [u8; 3] {
+    let mut control_word = [0u8; _];
+
+    control_word[0] |= match operation_rw {
+        OperationRW::Read => 0x80,
+        OperationRW::Write => 0x00,
+    };
+    control_word[0] |= match crc {
+        true => 0x40,
+        false => 0x00,
+    };
+    control_word[0] |= match data {
+        Mct8316AVData::Two(_) => 0x00,
+        Mct8316AVData::Four(_) => 0x10,
+        Mct8316AVData::Eight(_) => 0x20,
+    };
+    control_word[0] |= ((address >> 16) & 0x0F) as u8;
+    control_word[1] |= ((address >> 8) & 0xFF) as u8;
+    control_word[2] |= ((address >> 0) & 0xFF) as u8;
+
+    control_word
+}
+
+pub struct Mct8316AVDriver<I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    i2c: Mutex<I2C>,
+    address: u8,
+}
+
+impl<I2C> Debug for Mct8316AVDriver<I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        write!(f, "Mct8316AVDriver {{ address: {} }}", self.address)
+    }
+}
+
+impl<I2C> Mct8316AVDriver<I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    pub fn new(i2c: I2C, address: u8) -> Self {
+        trace!("Mct8316AVDriver::new(i2c, address: 0x{address:02x})");
+        Self {
+            i2c: i2c.into(),
+            address,
+        }
+    }
+
+    pub(super) fn write(&self, address: u32, data: Mct8316AVData) -> Result<()> {
+        trace!("Mct8316AVDriver::write(self: {self:?}, address: {address:06x}, data: {data})");
+
+        // 1 for target byte (for crc check)
+        // 3 for control word
+        // 8 for data
+        // 1 for crc
+        let mut write_data = [0u8; 1+3+8+1];
+        write_data[0] = (self.address << 1) | 0b0;
+        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());
+                2
+            },
+            Mct8316AVData::Four(val) => {
+                write_data[4..8].copy_from_slice(&val.to_be_bytes());
+                4
+            },
+            Mct8316AVData::Eight(val) => {
+                write_data[4..12].copy_from_slice(&val.to_be_bytes());
+                8
+            },
+        };
+        let crc_result = CRC.checksum(&write_data[0..(4 + data_length)]);
+        write_data[4+data_length] = crc_result;
+
+        // match self.i2c.lock() {
+        //     Ok(mut lock) => lock.write(self.address, &write_data).map_err(I2cError)?,
+        //     Err(_) => bail!("Lock was poisoned"),
+        // }
+
+        Ok(())
+    }
+
+    pub(super) fn read(&self, address: u32, data: &mut Mct8316AVData) -> Result<()> {
+        trace!("Mct8316AVDriver::read(self: {self:?}, address: {address:06x}, data: {data})");
+        // 1 target id
+        // 3 control word
+        // 1 target id
+        // 8 data bytes
+        // 1 crc
+        let mut read_data = [0u8; 1+3+1+8+1];
+        read_data[0] = (self.address << 1) | 0b0;
+        read_data[1..4].copy_from_slice(&control_word(
+            OperationRW::Read,
+            true,
+            *data,
+            address
+        ));
+        read_data[4] = (self.address << 1) | 0b1;
+        let data_length = match data {
+            Mct8316AVData::Two(_) => 2,
+            Mct8316AVData::Four(_) => 4,
+            Mct8316AVData::Eight(_) => 8,
+        };
+
+        let (left, right) = read_data.split_at_mut(5);
+
+        let mut i2c_ops = [
+            Operation::Write(&left[1..4]),
+            Operation::Read(&mut right[..(data_length+1)])
+        ];
+
+        // match self.i2c.lock() {
+        //     Ok(mut lock) => lock.transaction(self.address, &mut i2c_ops).map_err(I2cError)?,
+        //     Err(_) => bail!("Lock was poisoned"),
+        // }
+        drop(i2c_ops);
+
+        let expected_crc = CRC.checksum(&read_data[0..(5+data_length)]);
+
+        // ensure!(expected_crc == read_data[5+data_length], "CRC Mismatch");
+
+        match data {
+            Mct8316AVData::Two(val) => {
+                *val = u16::from_be_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]]);
+            }
+            Mct8316AVData::Eight(val) => {
+                *val = u64::from_be_bytes([read_data[5], read_data[6],
+                    read_data[7], read_data[8],
+                    read_data[9], read_data[10],
+                    read_data[11], read_data[12]]);
+            }
+        }
+
+        Ok(())
+    }
+}
+
+impl<I2C> Mct8316a for Mct8316AVDriver<I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    fn init(&mut self) -> Result<()> {
+        trace!("Mct8316AVDriver::init(self: {self:?})");
+
+        // Settings taken from Table 3-3
+        // https://www.ti.com/lit/ug/sllu336a/sllu336a.pdf?ts=1759001083565
+        Mct8316AVEeprom::load(self)?
+            .set_isd_config(IsdConfig {
+                enable_isd: true,
+                enable_brake: true,
+                enable_high_impedance: false,
+                enable_reverse_drive: true,
+                enable_resynchronization: true,
+                enable_stationary_brake: true,
+                stationary_detect_threshold: Threshold::MilliVolt25,
+                brake_mode: BrakeMode::LowOn,
+                brake_config: BrakeConfig::BrakeTimeExit,
+                brake_current_threshold: Threshold::MilliVolt15,
+                brake_time: IsdConfigTimeValue::Millisecond500,
+                high_impedence_time: IsdConfigTimeValue::Millisecond10,
+                startup_break_time: StartupBreakTime::Millisecond100,
+                resync_minimum_threshold: ResyncMinimumThreshold::Computed,
+            })?
+            .set_motor_startup1(MotorStartup1 {
+                motor_startup_method: MotorStartupMethod::DoubleAlign,
+                align_ramp_rate: AlignRampRate::VoltsPerSecond250,
+                align_time: AlignTime::Millisecond200,
+                align_current_threshold: FullCurrentThreshold::Volts0_4,
+                ipd_clock_frequency: IpdClockFrequency::Hertz500,
+                ipd_current_threshold: IpdCurrentThreshold::Volts0_4,
+                ipd_release_mode: IpdReleaseMode::Tristate,
+                ipd_advance_angle: IpdAdvanceAngle::Degrees60,
+                ipd_repeat: IpdRepeat::Average2,
+                slow_first_cycle_frequency: SlowFirstCycleFrequency::Hertz5,
+            })?
+            .set_motor_startup2(MotorStartup2 {
+                ..MotorStartup2::default()
+            })?
+            .set_closed_loop1(ClosedLoop1 {
+                ..ClosedLoop1::default()
+            })?
+            .set_closed_loop2(ClosedLoop2 {
+                ..ClosedLoop2::default()
+            })?
+            .set_closed_loop3(ClosedLoop3 {
+                ..ClosedLoop3::default()
+            })?
+            .set_closed_loop4(ClosedLoop4 {
+                ..ClosedLoop4::default()
+            })?
+            .set_constant_speed(ConstantSpeed {
+                ..ConstantSpeed::default()
+            })?
+            .set_constant_power(ConstantPower {
+                ..ConstantPower::default()
+            })?
+            .set_two_phase_profile(TwoPhase150DegreeProfile {
+                ..TwoPhase150DegreeProfile::default()
+            })?
+            .set_three_phase_profile(ThreePhase150DegreeProfile {
+               ..ThreePhase150DegreeProfile::default()
+            })?
+            .set_trap_config1(TrapConfig1 {
+                ..TrapConfig1::default()
+            })?
+            .set_trap_config2(TrapConfig2 {
+                ..TrapConfig2::default()
+            })?
+            .commit()?;
+
+        Ok(())
+    }
+}

flight/src/hardware/mct8316a/eeprom.rs

@@ -0,0 +1,357 @@
+use embedded_hal::i2c::I2c;
+use anyhow::{bail, Result};
+use log::trace;
+use crate::hardware::mct8316a::closed_loop::{ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4};
+use crate::hardware::mct8316a::constant_power::ConstantPower;
+use crate::hardware::mct8316a::constant_speed::ConstantSpeed;
+use crate::hardware::mct8316a::driver::Mct8316AVData;
+use crate::hardware::mct8316a::isd_config::IsdConfig;
+use crate::hardware::mct8316a::Mct8316AVDriver;
+use crate::hardware::mct8316a::motor_startup::{MotorStartup1, MotorStartup2};
+use crate::hardware::mct8316a::phase_profile::{ThreePhase150DegreeProfile, TwoPhase150DegreeProfile};
+use crate::hardware::mct8316a::trap_config::{TrapConfig1, TrapConfig2};
+
+pub struct Mct8316AVEeprom<'a, I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    driver: &'a mut Mct8316AVDriver<I2C>,
+    modified: bool,
+}
+
+impl<'a, I2C> Mct8316AVEeprom<'a, I2C>
+where
+    I2C: I2c + Send + Sync,
+    I2C::Error: Send,
+    I2C::Error: Sync,
+    I2C::Error: 'static,
+{
+    pub fn load(driver: &'a mut Mct8316AVDriver<I2C>) -> Result<Self> {
+        trace!("Mct8316AVEeprom::load()");
+
+        // driver.write(0x0000E6, Mct8316AVData::Four(0x40000000))?;
+
+        Ok(Self {
+            driver,
+            modified: false,
+        })
+    }
+
+    pub fn set_isd_config(self, isd_config: IsdConfig) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_isd_config(isd_config: {isd_config:?})");
+
+        let mut expected_value =
+            if isd_config.enable_isd { 0x40000000 } else { 0 }
+                | if isd_config.enable_brake { 0x20000000 } else { 0 }
+                | if isd_config.enable_high_impedance { 0x10000000 } else { 0 }
+                | if isd_config.enable_reverse_drive { 0x08000000 } else { 0 }
+                | if isd_config.enable_resynchronization { 0x04000000 } else { 0 }
+                | if isd_config.enable_stationary_brake { 0x02000000 } else { 0 }
+                | (((isd_config.stationary_detect_threshold as u32) & 0x7) << 22)
+                | ((isd_config.brake_mode as u32) & 0x1) << 21
+                | ((isd_config.brake_config as u32) & 0x1) << 20
+                | ((isd_config.brake_current_threshold as u32) & 0x7) << 17
+                | ((isd_config.brake_time as u32) & 0xF) << 13
+                | ((isd_config.high_impedence_time as u32) & 0xF) << 9
+                | ((isd_config.startup_break_time as u32) & 0x7) << 6
+                | ((isd_config.resync_minimum_threshold as u32) & 0x7) << 3;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x80, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_motor_startup1(self, motor_startup1: MotorStartup1) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_motor_startup1(motor_startup1: {motor_startup1:?})");
+
+        let mut expected_value =
+            ((motor_startup1.motor_startup_method as u32) & 0x3) << 29
+                | ((motor_startup1.align_ramp_rate as u32) & 0xF) << 25
+                | ((motor_startup1.align_time as u32) & 0xF) << 21
+                | ((motor_startup1.align_current_threshold as u32) & 0xF) << 17
+                | ((motor_startup1.ipd_clock_frequency as u32) & 0x7) << 14
+                | ((motor_startup1.ipd_current_threshold as u32) & 0xF) << 10
+                | ((motor_startup1.ipd_release_mode as u32) & 0x3) << 8
+                | ((motor_startup1.ipd_advance_angle as u32) & 0x3) << 6
+                | ((motor_startup1.ipd_repeat as u32) & 0x3) << 4
+                | ((motor_startup1.slow_first_cycle_frequency as u32) & 0xF) << 0;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x82, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_motor_startup2(self, motor_startup2: MotorStartup2) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_motor_startup2(motor_startup2: {motor_startup2:?})");
+
+        let mut expected_value =
+            ((motor_startup2.open_loop_current_limit_mode as u32) & 0x1) << 30
+                | ((motor_startup2.open_loop_duty_cycle as u32) & 0x7) << 27
+                | ((motor_startup2.open_loop_current_limit as u32) & 0xF) << 23
+                | ((motor_startup2.open_loop_acceleration1 as u32) & 0x1F) << 18
+                | ((motor_startup2.open_loop_acceleration2 as u32) & 0x1F) << 13
+                | ((motor_startup2.open_closed_handoff_threshold as u32) & 0x1F) << 8
+                | ((motor_startup2.auto_handoff as u32) & 0x1) << 7
+                | ((motor_startup2.first_cycle_frequency_select as u32) & 0x1) << 6
+                | ((motor_startup2.minimum_duty_cycle as u32) & 0xF) << 2;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x84, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_closed_loop1(self, closed_loop1: ClosedLoop1) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_closed_loop1(closed_loop1: {closed_loop1:?})");
+
+        let lead_angle = (closed_loop1.lead_angle / 0.12f32).round().clamp(0.0f32, u8::MAX as f32) as u8;
+
+        let mut expected_value =
+            ((closed_loop1.commutation_mode as u32) & 0x2) << 29
+                | ((closed_loop1.closed_loop_acceleration_rate as u32) & 0x1F) << 24
+                | ((closed_loop1.closed_loop_deceleration_mode as u32) & 0x1) << 23
+                | ((closed_loop1.closed_loop_deceleration_mode as u32) & 0x1F) << 18
+                | ((closed_loop1.pwm_frequency as u32) & 0x1F) << 13
+                | ((closed_loop1.pwm_modulation as u32) & 0x3) << 11
+                | ((closed_loop1.pwm_mode as u32) & 0x1) << 10
+                | ((closed_loop1.lead_angle_polarity as u32) & 0x1) << 9
+                | (lead_angle as u32) << 1;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x86, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_closed_loop2(self, closed_loop2: ClosedLoop2) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_closed_loop2(closed_loop2: {closed_loop2:?})");
+
+        let mut expected_value =
+            ((closed_loop2.speed_feedback_mode as u32) & 0x2) << 29
+                | ((closed_loop2.speed_feedback_division as u32) & 0xF) << 25
+                | ((closed_loop2.speed_feedback_config as u32) & 0x1) << 24
+                | ((closed_loop2.bemf_threshold as u32) & 0x7) << 21
+                | ((closed_loop2.motor_stop_mode as u32) & 0x7) << 18
+                | ((closed_loop2.motor_stop_brake_time as u32) & 0xF) << 14
+                | ((closed_loop2.active_low_high_brake_threshold as u32) & 0x7) << 11
+                | ((closed_loop2.brake_pin_threshold as u32) & 0x7) << 8
+                | ((closed_loop2.avs_enable as u32) & 0x1) << 7
+                | ((closed_loop2.cycle_current_limit as u32) & 0xF) << 3;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x88, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_closed_loop3(self, closed_loop3: ClosedLoop3) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_closed_loop3(closed_loop3: {closed_loop3:?})");
+
+        let mut expected_value =
+            ((closed_loop3.degauss_samples as u32) & 0x2) << 29
+                | ((closed_loop3.degauss_upper_bound as u32) & 0x3) << 27
+                | ((closed_loop3.degauss_lower_bound as u32) & 0x3) << 25
+                | ((closed_loop3.integration_cycle_low_threshold as u32) & 0x3) << 23
+                | ((closed_loop3.integration_cycle_high_threshold as u32) & 0x3) << 21
+                | ((closed_loop3.integration_duty_cycle_low_threshold as u32) & 0x3) << 19
+                | ((closed_loop3.integration_duty_cycle_high_threshold as u32) & 0x3) << 17
+                | ((closed_loop3.bemf_threshold2 as u32) & 0x3F) << 11
+                | ((closed_loop3.bemf_threshold1 as u32) & 0x3F) << 5
+                | ((closed_loop3.commutation_method as u32) & 0x1) << 4
+                | ((closed_loop3.degauss_window as u32) & 0x7) << 1
+                | ((closed_loop3.degauss_enable as u32) & 0x1) << 0;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x8A, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_closed_loop4(self, closed_loop4: ClosedLoop4) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_closed_loop4(closed_loop4: {closed_loop4:?})");
+
+        let mut expected_value =
+            ((closed_loop4.wcomp_blanking as u32) & 0x1) << 19
+                | ((closed_loop4.fast_deceleration_duty_window as u32) & 0x7) << 16
+                | ((closed_loop4.fast_deceleration_duty_threshold as u32) & 0x7) << 13
+                | ((closed_loop4.dynamic_brake_current_lower_threshold as u32) & 0x7) << 9
+                | ((closed_loop4.dynamic_braking_current as u32) & 0x1) << 8
+                | ((closed_loop4.fast_deceleration as u32) & 0x1) << 7
+                | ((closed_loop4.fast_deceleration_current_theshold as u32) & 0xF) << 3
+                | ((closed_loop4.fast_brake_delta as u32) & 0x7) << 0;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x8C, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_constant_speed(self, constant_speed: ConstantSpeed) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_constant_speed(constant_speed: {constant_speed:?})");
+
+        let speed_power_kp = (constant_speed.speed_power_kp * 10000f64).round().clamp(0.0f64, 0x3FF as f64) as u32;
+        let speed_power_ki = (constant_speed.speed_power_ki * 1000000f64).round().clamp(0.0f64, 0xFFF as f64) as u32;
+
+        let mut expected_value =
+            speed_power_kp << 20
+                | speed_power_ki << 8
+                | ((constant_speed.speed_power_upper_limit as u32) & 0x7) << 5
+                | ((constant_speed.speed_power_lower_limit as u32) & 0x7) << 2
+                | ((constant_speed.mode as u32) & 0x3) << 9;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x8E, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_constant_power(self, constant_power: ConstantPower) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_constant_power(constant_power: {constant_power:?})");
+
+        let max_speed = (constant_power.max_speed * 16f64).round().clamp(0.0f64, 0xFFFF as f64) as u32;
+        let max_power = (constant_power.max_power * 4f64).round().clamp(0.0f64, 0x3FF as f64) as u32;
+
+        let mut expected_value =
+            max_speed << 15
+                | ((constant_power.dead_time_compensation as u32) & 0x1) << 14
+                | max_power << 4
+                | ((constant_power.power_hysteresis as u32) & 0x3) << 2
+                | ((constant_power.mode as u32) & 0x3) << 0;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x90, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_two_phase_profile(self, profile: TwoPhase150DegreeProfile) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_two_phase_profile(profile: {profile:?})");
+
+        let mut expected_value =
+            ((profile.steps[0] as u32) & 0x7) << 28
+                | ((profile.steps[1] as u32) & 0x7) << 25
+                | ((profile.steps[2] as u32) & 0x7) << 22
+                | ((profile.steps[3] as u32) & 0x7) << 19
+                | ((profile.steps[4] as u32) & 0x7) << 16
+                | ((profile.steps[5] as u32) & 0x7) << 13
+                | ((profile.steps[6] as u32) & 0x7) << 10
+                | ((profile.steps[7] as u32) & 0x7) << 7;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x96, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_three_phase_profile(self, profile: ThreePhase150DegreeProfile) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_three_phase_profile(profile: {profile:?})");
+
+        let mut expected_value =
+            ((profile.steps[0] as u32) & 0x7) << 28
+                | ((profile.steps[1] as u32) & 0x7) << 25
+                | ((profile.steps[2] as u32) & 0x7) << 22
+                | ((profile.steps[3] as u32) & 0x7) << 19
+                | ((profile.steps[4] as u32) & 0x7) << 16
+                | ((profile.steps[5] as u32) & 0x7) << 13
+                | ((profile.steps[6] as u32) & 0x7) << 10
+                | ((profile.steps[7] as u32) & 0x7) << 7
+                | ((profile.lead_angle as u32) & 0x3) << 5;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x98, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_trap_config1(self, trap_config1: TrapConfig1) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_trap_config1(trap_config1: {trap_config1:?})");
+
+        let mut expected_value =
+            ((trap_config1.open_loop_handoff_cycles as u32) & 0x3) << 22
+                | ((trap_config1.avs_negative_current_limit as u32) & 0x7) << 16
+                | ((trap_config1.avs_limit_hysteresis as u32) & 0x1) << 15
+                | ((trap_config1.isd_bemf_threshold as u32) & 0x1F) << 10
+                | ((trap_config1.isd_cycle_threshold as u32) & 0x7) << 7
+                | ((trap_config1.open_loop_zc_detection_threshold as u32) & 0x7) << 2
+                | ((trap_config1.fast_startup_div_factor as u32) & 0x3) << 0
+            ;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x9A, Mct8316AVData::Four(expected_value))
+    }
+
+    pub fn set_trap_config2(self, trap_config2: TrapConfig2) -> Result<Self> {
+        trace!("Mct8316AVEeprom::set_trap_config2(trap_config2: {trap_config2:?})");
+
+        let mut expected_value =
+            ((trap_config2.blanking_time_microseconds as u32) & 0xF) << 27
+                | ((trap_config2.comparator_deglitch_time_microseconds as u32) & 0x7) << 24
+                | 1u32 << 21
+                | ((trap_config2.align_duty_cycle as u32) & 0x7) << 18;
+
+        // Set parity bit correctly
+        if expected_value.count_ones() % 2 == 1 {
+            expected_value |= 0x80000000;
+        }
+
+        self.assert_register(0x9C, Mct8316AVData::Four(expected_value))
+    }
+
+    fn assert_register(self, address: u32, value: Mct8316AVData) -> Result<Self> {
+        trace!("Mct8316AVEeprom::assert_register(address: {address:06x}, value: {value})");
+
+        let mut read_value = value.clone();
+
+        self.driver.read(address, &mut read_value)?;
+
+        if read_value == value {
+            Ok(self)
+        } else {
+            self.driver.write(address, value)?;
+
+            Ok(Self {
+                modified: true,
+                ..self
+            })
+        }
+    }
+
+    pub fn commit(self) -> Result<()> {
+        trace!("Mct8316AVEeprom::commit()");
+        if self.modified {
+            bail!("TODO");
+        }
+        Ok(())
+    }
+}

flight/src/hardware/mct8316a/isd_config.rs

@@ -0,0 +1,108 @@
+#![allow(dead_code)]
+
+#[derive(Debug, Clone)]
+pub struct IsdConfig {
+    pub enable_isd: bool,
+    pub enable_brake: bool,
+    pub enable_high_impedance: bool,
+    pub enable_reverse_drive: bool,
+    pub enable_resynchronization: bool,
+    pub enable_stationary_brake: bool,
+    pub stationary_detect_threshold: Threshold,
+    pub brake_mode: BrakeMode,
+    pub brake_config: BrakeConfig,
+    pub brake_current_threshold: Threshold,
+    pub brake_time: IsdConfigTimeValue,
+    pub high_impedence_time: IsdConfigTimeValue,
+    pub startup_break_time: StartupBreakTime,
+    pub resync_minimum_threshold: ResyncMinimumThreshold,
+}
+
+impl Default for IsdConfig {
+    fn default() -> Self {
+        Self {
+            enable_isd: false,
+            enable_brake: false,
+            enable_high_impedance: false,
+            enable_reverse_drive: false,
+            enable_resynchronization: false,
+            enable_stationary_brake: false,
+            stationary_detect_threshold: Threshold::MilliVolt5,
+            brake_mode: BrakeMode::LowOn,
+            brake_config: BrakeConfig::BrakeTimeExit,
+            brake_current_threshold: Threshold::MilliVolt5,
+            brake_time: IsdConfigTimeValue::Millisecond10,
+            high_impedence_time: IsdConfigTimeValue::Millisecond10,
+            startup_break_time: StartupBreakTime::Millisecond1,
+            resync_minimum_threshold: ResyncMinimumThreshold::Computed,
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum Threshold {
+    MilliVolt5 = 0x0,
+    MilliVolt10 = 0x1,
+    MilliVolt15 = 0x2,
+    MilliVolt20 = 0x3,
+    MilliVolt25 = 0x4,
+    MilliVolt30 = 0x5,
+    MilliVolt50 = 0x6,
+    MilliVolt100 = 0x7,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum BrakeMode {
+    LowOn = 0x0,
+    HighOn = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum BrakeConfig {
+    BrakeTimeExit = 0x0,
+    CurrentThresholdExit = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IsdConfigTimeValue {
+    Millisecond10 = 0x0,
+    Millisecond50 = 0x1,
+    Millisecond100 = 0x2,
+    Millisecond200 = 0x3,
+    Millisecond300 = 0x4,
+    Millisecond400 = 0x5,
+    Millisecond500 = 0x6,
+    Millisecond750 = 0x7,
+    Second1 = 0x8,
+    Second2 = 0x9,
+    Second3 = 0xA,
+    Second4 = 0xB,
+    Second5 = 0xC,
+    Second7_5 = 0xD,
+    Second10 = 0xE,
+    Second15 = 0xF,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum StartupBreakTime {
+    Millisecond1 = 0x0,
+    Millisecond10 = 0x1,
+    Millisecond25 = 0x2,
+    Millisecond50 = 0x3,
+    Millisecond100 = 0x4,
+    Millisecond250 = 0x5,
+    Millisecond500 = 0x6,
+    Millisecond1000 = 0x7,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum ResyncMinimumThreshold {
+    Computed = 0x0,
+    MilliVolt300 = 0x1,
+    MilliVolt400 = 0x2,
+    MilliVolt500 = 0x3,
+    MilliVolt600 = 0x4,
+    MilliVolt800 = 0x5,
+    MilliVolt1000 = 0x6,
+    MilliVolt1250 = 0x7,
+}

flight/src/hardware/mct8316a/mod.rs

@@ -0,0 +1,17 @@
+mod driver;
+mod eeprom;
+mod isd_config;
+mod motor_startup;
+mod closed_loop;
+mod constant_speed;
+mod constant_power;
+mod phase_profile;
+mod trap_config;
+
+use anyhow::Result;
+
+pub trait Mct8316a {
+    fn init(&mut self) -> Result<()>;
+}
+
+pub use driver::Mct8316AVDriver;

flight/src/hardware/mct8316a/motor_startup.rs

@@ -0,0 +1,363 @@
+#![allow(dead_code)]
+
+#[derive(Debug, Clone)]
+pub struct MotorStartup1 {
+    pub motor_startup_method: MotorStartupMethod,
+    pub align_ramp_rate: AlignRampRate,
+    pub align_time: AlignTime,
+    pub align_current_threshold: FullCurrentThreshold,
+    pub ipd_clock_frequency: IpdClockFrequency,
+    pub ipd_current_threshold: IpdCurrentThreshold,
+    pub ipd_release_mode: IpdReleaseMode,
+    pub ipd_advance_angle: IpdAdvanceAngle,
+    pub ipd_repeat: IpdRepeat,
+    pub slow_first_cycle_frequency: SlowFirstCycleFrequency,
+}
+
+impl Default for MotorStartup1 {
+    fn default() -> Self {
+        Self {
+            motor_startup_method: MotorStartupMethod::Align,
+            align_ramp_rate: AlignRampRate::VoltsPerSecond0_1,
+            align_time: AlignTime::Millisecond5,
+            align_current_threshold: FullCurrentThreshold::NotApplicable,
+            ipd_clock_frequency: IpdClockFrequency::Hertz50,
+            ipd_current_threshold: IpdCurrentThreshold::NotApplicable,
+            ipd_release_mode: IpdReleaseMode::Brake,
+            ipd_advance_angle: IpdAdvanceAngle::Degrees0,
+            ipd_repeat: IpdRepeat::Once,
+            slow_first_cycle_frequency: SlowFirstCycleFrequency::Hertz0_05,
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum MotorStartupMethod {
+    Align = 0x0,
+    DoubleAlign = 0x1,
+    IPD = 0x2,
+    SlowFirstCycle = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum AlignRampRate {
+    VoltsPerSecond0_1 = 0x0,
+    VoltsPerSecond0_2 = 0x1,
+    VoltsPerSecond0_5 = 0x2,
+    VoltsPerSecond1 = 0x3,
+    VoltsPerSecond2_5 = 0x4,
+    VoltsPerSecond5 = 0x5,
+    VoltsPerSecond7_5 = 0x6,
+    VoltsPerSecond10 = 0x7,
+    VoltsPerSecond25 = 0x8,
+    VoltsPerSecond50 = 0x9,
+    VoltsPerSecond75 = 0xA,
+    VoltsPerSecond100 = 0xB,
+    VoltsPerSecond250 = 0xC,
+    VoltsPerSecond500 = 0xD,
+    VoltsPerSecond750 = 0xE,
+    VoltsPerSecond1000 = 0xF,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum AlignTime {
+    Millisecond5 = 0x0,
+    Millisecond10 = 0x1,
+    Millisecond25 = 0x2,
+    Millisecond50 = 0x3,
+    Millisecond75 = 0x4,
+    Millisecond100 = 0x5,
+    Millisecond200 = 0x6,
+    Millisecond400 = 0x7,
+    Millisecond600 = 0x8,
+    Millisecond800 = 0x9,
+    Second1 = 0xA,
+    Second2 = 0xB,
+    Second4 = 0xC,
+    Second6 = 0xD,
+    Second8 = 0xE,
+    Second10 = 0xF,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum FullCurrentThreshold {
+    NotApplicable = 0x0,
+    Volts0_1 = 0x1,
+    Volts0_2 = 0x2,
+    Volts0_3 = 0x3,
+    Volts0_4 = 0x4,
+    Volts0_5 = 0x5,
+    Volts0_6 = 0x6,
+    Volts0_7 = 0x7,
+    Volts0_8 = 0x8,
+    Volts0_9 = 0x9,
+    Volts1_0 = 0xA,
+    Volts1_1 = 0xB,
+    Volts1_2 = 0xC,
+    Volts1_3 = 0xD,
+    Volts1_4 = 0xE,
+    Volts1_5 = 0xF,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IpdClockFrequency {
+    Hertz50 = 0x0,
+    Hertz100 = 0x1,
+    Hertz250 = 0x2,
+    Hertz500 = 0x3,
+    Hertz1000 = 0x4,
+    Hertz2000 = 0x5,
+    Hertz5000 = 0x6,
+    Hertz10000 = 0x7,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IpdCurrentThreshold {
+    NotApplicable = 0x0,
+    Volts0_2 = 0x2,
+    Volts0_3 = 0x3,
+    Volts0_4 = 0x4,
+    Volts0_5 = 0x5,
+    Volts0_6 = 0x6,
+    Volts0_7 = 0x7,
+    Volts0_8 = 0x8,
+    Volts0_9 = 0x9,
+    Volts1_0 = 0xA,
+    Volts1_1 = 0xB,
+    Volts1_2 = 0xC,
+    Volts1_3 = 0xD,
+    Volts1_4 = 0xE,
+    Volts1_5 = 0xF,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IpdReleaseMode {
+    Brake = 0x0,
+    Tristate = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IpdAdvanceAngle {
+    Degrees0 = 0x0,
+    Degrees30 = 0x1,
+    Degrees60 = 0x2,
+    Degrees90 = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IpdRepeat {
+    Once = 0x0,
+    Average2 = 0x1,
+    Average3 = 0x2,
+    Average4 = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum SlowFirstCycleFrequency {
+    Hertz0_05 = 0x0,
+    Hertz0_1 = 0x1,
+    Hertz0_25 = 0x2,
+    Hertz0_5 = 0x3,
+    Hertz1 = 0x4,
+    Hertz2 = 0x5,
+    Hertz3 = 0x6,
+    Hertz5 = 0x7,
+    Hertz10 = 0x8,
+    Hertz15 = 0x9,
+    Hertz25 = 0xB,
+    Hertz50 = 0xC,
+    Hertz100 = 0xD,
+    Hertz150 = 0xE,
+    Hertz200 = 0xF,
+}
+
+#[derive(Debug, Clone)]
+pub struct MotorStartup2 {
+    pub open_loop_current_limit_mode: OpenLoopCurrentLimitMode,
+    pub open_loop_duty_cycle: DutyCycle,
+    pub open_loop_current_limit: FullCurrentThreshold,
+    pub open_loop_acceleration1: OpenLoopAcceleration1,
+    pub open_loop_acceleration2: OpenLoopAcceleration2,
+    pub open_closed_handoff_threshold: OpenClosedHandoffThreshold,
+    pub auto_handoff: EnableDisable,
+    pub first_cycle_frequency_select: FirstCycleFrequencySelect,
+    pub minimum_duty_cycle: MinimumDutyCycle,
+}
+
+impl Default for MotorStartup2 {
+    fn default() -> Self {
+        Self {
+            open_loop_current_limit_mode: OpenLoopCurrentLimitMode::OpenLoopCurrentLimit,
+            open_loop_duty_cycle: DutyCycle::Percent10,
+            open_loop_current_limit: FullCurrentThreshold::NotApplicable,
+            open_loop_acceleration1: OpenLoopAcceleration1::HertzPerSecond0_005,
+            open_loop_acceleration2: OpenLoopAcceleration2::HertzPerSecondSecond0_005,
+            open_closed_handoff_threshold: OpenClosedHandoffThreshold::Hertz1,
+            auto_handoff: EnableDisable::Disable,
+            first_cycle_frequency_select: FirstCycleFrequencySelect::SlowFirstCycle,
+            minimum_duty_cycle: MinimumDutyCycle::Percent1_5,
+        }
+    }
+}
+
+
+#[derive(Debug, Copy, Clone)]
+pub enum OpenLoopCurrentLimitMode {
+    OpenLoopCurrentLimit = 0x0,
+    CurrentLimit = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum DutyCycle {
+    Percent10 = 0x0,
+    Percent15 = 0x1,
+    Percent20 = 0x2,
+    Percent25 = 0x3,
+    Percent30 = 0x4,
+    Percent40 = 0x5,
+    Percent50 = 0x6,
+    Percent100 = 0x7,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum OpenLoopAcceleration1 {
+    HertzPerSecond0_005 = 0x00,
+    HertzPerSecond0_01 = 0x01,
+    HertzPerSecond0_025 = 0x02,
+    HertzPerSecond0_05 = 0x03,
+    HertzPerSecond0_1 = 0x04,
+    HertzPerSecond0_25 = 0x05,
+    HertzPerSecond0_5 = 0x06,
+    HertzPerSecond1 = 0x07,
+    HertzPerSecond2_5 = 0x08,
+    HertzPerSecond5 = 0x09,
+    HertzPerSecond7_5 = 0x0A,
+    HertzPerSecond10 = 0x0B,
+    HertzPerSecond12_5 = 0x0C,
+    HertzPerSecond15 = 0x0D,
+    HertzPerSecond20 = 0x0E,
+    HertzPerSecond30 = 0x0F,
+    HertzPerSecond40 = 0x10,
+    HertzPerSecond50 = 0x11,
+    HertzPerSecond60 = 0x12,
+    HertzPerSecond75 = 0x13,
+    HertzPerSecond100 = 0x14,
+    HertzPerSecond125 = 0x15,
+    HertzPerSecond150 = 0x16,
+    HertzPerSecond175 = 0x17,
+    HertzPerSecond200 = 0x18,
+    HertzPerSecond250 = 0x19,
+    HertzPerSecond300 = 0x1A,
+    HertzPerSecond400 = 0x1B,
+    HertzPerSecond500 = 0x1C,
+    HertzPerSecond750 = 0x1D,
+    HertzPerSecond1000 = 0x1E,
+    Unlimited = 0x1F,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum OpenLoopAcceleration2 {
+    HertzPerSecondSecond0_005 = 0x00,
+    HertzPerSecondSecond0_01 = 0x01,
+    HertzPerSecondSecond0_025 = 0x02,
+    HertzPerSecondSecond0_05 = 0x03,
+    HertzPerSecondSecond0_1 = 0x04,
+    HertzPerSecondSecond0_25 = 0x05,
+    HertzPerSecondSecond0_5 = 0x06,
+    HertzPerSecondSecond1 = 0x07,
+    HertzPerSecondSecond2_5 = 0x08,
+    HertzPerSecondSecond5 = 0x09,
+    HertzPerSecondSecond7_5 = 0x0A,
+    HertzPerSecondSecond10 = 0x0B,
+    HertzPerSecondSecond12_5 = 0x0C,
+    HertzPerSecondSecond15 = 0x0D,
+    HertzPerSecondSecond20 = 0x0E,
+    HertzPerSecondSecond30 = 0x0F,
+    HertzPerSecondSecond40 = 0x10,
+    HertzPerSecondSecond50 = 0x11,
+    HertzPerSecondSecond60 = 0x12,
+    HertzPerSecondSecond75 = 0x13,
+    HertzPerSecondSecond100 = 0x14,
+    HertzPerSecondSecond125 = 0x15,
+    HertzPerSecondSecond150 = 0x16,
+    HertzPerSecondSecond175 = 0x17,
+    HertzPerSecondSecond200 = 0x18,
+    HertzPerSecondSecond250 = 0x19,
+    HertzPerSecondSecond300 = 0x1A,
+    HertzPerSecondSecond400 = 0x1B,
+    HertzPerSecondSecond500 = 0x1C,
+    HertzPerSecondSecond750 = 0x1D,
+    HertzPerSecondSecond1000 = 0x1E,
+    Unlimited = 0x1F,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum OpenClosedHandoffThreshold {
+    Hertz1 = 0x00,
+    Hertz4 = 0x01,
+    Hertz8 = 0x02,
+    Hertz12 = 0x03,
+    Hertz16 = 0x04,
+    Hertz20 = 0x05,
+    Hertz24 = 0x06,
+    Hertz28 = 0x07,
+    Hertz32 = 0x08,
+    Hertz36 = 0x09,
+    Hertz40 = 0x0A,
+    Hertz45 = 0x0B,
+    Hertz50 = 0x0C,
+    Hertz55 = 0x0D,
+    Hertz60 = 0x0E,
+    Hertz65 = 0x0F,
+    Hertz70 = 0x10,
+    Hertz75 = 0x11,
+    Hertz80 = 0x12,
+    Hertz85 = 0x13,
+    Hertz90 = 0x14,
+    Hertz100 = 0x15,
+    Hertz150 = 0x16,
+    Hertz200 = 0x17,
+    Hertz250 = 0x18,
+    Hertz300 = 0x19,
+    Hertz350 = 0x1A,
+    Hertz400 = 0x1B,
+    Hertz450 = 0x1C,
+    Hertz500 = 0x1D,
+    Hertz550 = 0x1E,
+    Hertz600 = 0x1F,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum EnableDisable {
+    Disable = 0x0,
+    Enable = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum FirstCycleFrequencySelect {
+    SlowFirstCycle = 0x0,
+    Hertz0 = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum MinimumDutyCycle {
+    Percent1_5 = 0x0,
+    Percent2 = 0x1,
+    Percent3 = 0x2,
+    Percent4 = 0x3,
+    Percent5 = 0x4,
+    Percent6 = 0x5,
+    Percent7 = 0x6,
+    Percent8 = 0x7,
+    Percent9 = 0x8,
+    Percent10 = 0x9,
+    Percent12 = 0xA,
+    Percent15 = 0xB,
+    Percent17_5 = 0xC,
+    Percent20 = 0xD,
+    Percent25 = 0xE,
+    Percent30 = 0xF,
+}
+
+
+

flight/src/hardware/mct8316a/phase_profile.rs

@@ -0,0 +1,49 @@
+#![allow(dead_code)]
+
+#[derive(Debug, Clone)]
+pub struct TwoPhase150DegreeProfile {
+    pub steps: [ProfileSetting; 8],
+}
+
+impl Default for TwoPhase150DegreeProfile {
+    fn default() -> Self {
+        Self {
+            steps: [ProfileSetting::Percent0; _],
+        }
+    }
+}
+
+#[derive(Debug, Clone)]
+pub struct ThreePhase150DegreeProfile {
+    pub steps: [ProfileSetting; 8],
+    pub lead_angle: ThreePhaseLeadAngle,
+}
+
+impl Default for ThreePhase150DegreeProfile {
+    fn default() -> Self {
+        Self {
+            steps: [ProfileSetting::Percent0; _],
+            lead_angle: ThreePhaseLeadAngle::Degrees0,
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum ProfileSetting {
+    Percent0 = 0x0,
+    Percent50 = 0x1,
+    Percent75 = 0x2,
+    Percent83_75 = 0x3,
+    Percent87_5 = 0x4,
+    Percent93_75 = 0x5,
+    Percent97_5 = 0x6,
+    Percent99 = 0x7,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum ThreePhaseLeadAngle {
+    Degrees0 = 0x0,
+    Degrees5 = 0x1,
+    Degrees10 = 0x2,
+    Degrees15 = 0x3,
+}

flight/src/hardware/mct8316a/trap_config.rs

@@ -0,0 +1,140 @@
+#![allow(dead_code)]
+
+use crate::hardware::mct8316a::motor_startup::DutyCycle;
+
+#[allow(dead_code)]
+
+#[derive(Debug, Clone)]
+pub struct TrapConfig1 {
+    pub open_loop_handoff_cycles: OpenLoopHandoffCycles,
+    pub avs_negative_current_limit: AVSNegativeCurrentLimit,
+    pub avs_limit_hysteresis: AVSLimitHysteresis,
+    pub isd_bemf_threshold: IsdBemfThreshold,
+    pub isd_cycle_threshold: IsdCycleThreshold,
+    pub open_loop_zc_detection_threshold: OpenLoopZcDetectionThreshold,
+    pub fast_startup_div_factor: FastStartupDivFactor,
+}
+
+impl Default for TrapConfig1 {
+    fn default() -> Self {
+        Self {
+            open_loop_handoff_cycles: OpenLoopHandoffCycles::Cycles3,
+            avs_negative_current_limit: AVSNegativeCurrentLimit::Limit0,
+            avs_limit_hysteresis: AVSLimitHysteresis::Limit20,
+            isd_bemf_threshold: IsdBemfThreshold::Limit0,
+            isd_cycle_threshold: IsdCycleThreshold::Limit2,
+            open_loop_zc_detection_threshold: OpenLoopZcDetectionThreshold::Degrees5,
+            fast_startup_div_factor: FastStartupDivFactor::Value1,
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum OpenLoopHandoffCycles {
+    Cycles3 = 0x0,
+    Cycles6 = 0x1,
+    Cycles12 = 0x2,
+    Cycles24 = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum AVSNegativeCurrentLimit {
+    Limit0 = 0x0,
+    LimitNeg40 = 0x1,
+    LimitNeg30 = 0x2,
+    LimitNeg20 = 0x3,
+    LimitNeg10 = 0x4,
+    Limit10 = 0x5,
+    Limit20 = 0x6,
+    Limit30 = 0x7,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum AVSLimitHysteresis {
+    Limit20 = 0x0,
+    Limit10 = 0x1,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IsdBemfThreshold {
+    Limit0 = 0x00,
+    Limit200 = 0x01,
+    Limit400 = 0x02,
+    Limit600 = 0x03,
+    Limit800 = 0x04,
+    Limit1000 = 0x05,
+    Limit1200 = 0x06,
+    Limit1400 = 0x07,
+    Limit1600 = 0x08,
+    Limit1800 = 0x09,
+    Limit2000 = 0x0A,
+    Limit2200 = 0x0B,
+    Limit2400 = 0x0C,
+    Limit2600 = 0x0D,
+    Limit2800 = 0x0E,
+    Limit3000 = 0x0F,
+    Limit3200 = 0x10,
+    Limit3400 = 0x11,
+    Limit3600 = 0x12,
+    Limit3800 = 0x13,
+    Limit4000 = 0x14,
+    Limit4200 = 0x15,
+    Limit4400 = 0x16,
+    Limit4600 = 0x17,
+    Limit4800 = 0x18,
+    Limit5000 = 0x19,
+    Limit5200 = 0x1A,
+    Limit5400 = 0x1B,
+    Limit5600 = 0x1C,
+    Limit5800 = 0x1D,
+    Limit6000 = 0x1E,
+    Limit6200 = 0x1F,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum IsdCycleThreshold {
+    Limit2 = 0x0,
+    Limit5 = 0x1,
+    Limit8 = 0x2,
+    Limit11 = 0x3,
+    Limit14 = 0x4,
+    Limit17 = 0x5,
+    Limit20 = 0x6,
+    Limit23 = 0x7,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum OpenLoopZcDetectionThreshold {
+    Degrees5 = 0x0,
+    Degrees8 = 0x1,
+    Degrees12 = 0x2,
+    Degrees15 = 0x3,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub enum FastStartupDivFactor {
+    Value1 = 0x0,
+    Value2 = 0x1,
+    Value4 = 0x2,
+    Value8 = 0x3,
+}
+
+#[derive(Debug, Clone)]
+pub struct TrapConfig2 {
+    /// Max of 0xF (15us)
+    pub blanking_time_microseconds: u8,
+    /// Max of 0x7 (7us)
+    pub comparator_deglitch_time_microseconds: u8,
+    pub align_duty_cycle: DutyCycle,
+}
+
+impl Default for TrapConfig2 {
+    fn default() -> Self {
+        Self {
+            blanking_time_microseconds: 0,
+            comparator_deglitch_time_microseconds: 0,
+            align_duty_cycle: DutyCycle::Percent10,
+        }
+    }
+}
+

flight/src/hardware/mod.rs

@@ -1,10 +1,18 @@
 use crate::hardware::mcp23017::Mcp23017;
 use anyhow::Result;
+use embedded_hal::pwm::SetDutyCycle;
+use crate::hardware::mct8316a::Mct8316a;
 
 pub trait Hardware {
+    type Pwm: SetDutyCycle<Error: std::error::Error + Sync + Send> + Sync;
+
     fn new_mcp23017_a(&self) -> Result<impl Mcp23017 + Sync>;
     fn new_mcp23017_b(&self) -> Result<impl Mcp23017 + Sync>;
 
+    fn new_pwm0(&self) -> Result<Self::Pwm>;
+
+    fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync>;
+
     fn get_battery_voltage(&self) -> Result<f64>;
 }
 
@@ -33,3 +41,4 @@ mod error;
 pub mod mcp23017;
 mod mcp3208;
 pub mod channelization;
+pub(crate) mod mct8316a;

flight/src/hardware/raspi/mod.rs

@@ -1,20 +1,25 @@
+mod pwm;
+
 use crate::hardware::mcp23017::{Mcp23017, Mcp23017Driver};
 use crate::hardware::mcp3208::Mcp3208;
 use crate::hardware::Hardware;
 use anyhow::Result;
 use embedded_hal_bus::i2c::MutexDevice;
 use log::{debug, info, trace};
-// use rpi_pal::gpio::Gpio;
+use rpi_pal::gpio::Gpio;
 use rpi_pal::i2c::I2c;
 use rpi_pal::spi::SimpleHalSpiDevice;
 use rpi_pal::spi::{Bus, Mode, SlaveSelect, Spi};
 use std::cell::RefCell;
 use std::sync::Mutex;
+use rpi_pal::pwm::Pwm;
+use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a};
+use crate::hardware::raspi::pwm::PwmWrapper;
 
 const CLOCK_1MHZ: u32 = 1_000_000;
 
 pub struct RaspiHardware {
-    // gpio: Gpio,
+    _gpio: Gpio,
     i2c_bus: Mutex<I2c>,
     mcp3208: RefCell<Mcp3208<SimpleHalSpiDevice>>,
 }
@@ -28,7 +33,7 @@ impl RaspiHardware {
         debug!("SOC: {}", device.soc());
 
         Ok(Self {
-            // gpio: Gpio::new()?,
+            _gpio: Gpio::new()?,
             i2c_bus: Mutex::new(I2c::with_bus(0u8)?),
             mcp3208: Mcp3208::new(SimpleHalSpiDevice::new(Spi::new(
                 Bus::Spi1,
@@ -41,15 +46,35 @@ impl RaspiHardware {
 }
 
 impl Hardware for RaspiHardware {
+    type Pwm = PwmWrapper;
+
     fn new_mcp23017_a(&self) -> Result<impl Mcp23017> {
+        trace!("RaspiHardware::new_mcp23017_a()");
         Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100000))
     }
 
     fn new_mcp23017_b(&self) -> Result<impl Mcp23017> {
+        trace!("RaspiHardware::new_mcp23017_b()");
         Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100001))
     }
 
+    fn new_pwm0(&self) -> Result<Self::Pwm> {
+        trace!("RaspiHardware::new_pwm0()");
+        // Unfortunately the current version of rpi_pal assumes an older version
+        // of the kernel where pwmchip for RPi5 was 2
+        const PWMCHIP: u8 = 0;
+        const CHANNEL: u8 = 0;
+        Ok(PwmWrapper::new(Pwm::with_pwmchip(PWMCHIP, CHANNEL)?)?)
+    }
+
+    fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
+        trace!("RaspiHardware::new_mct8316a()");
+        Ok(Mct8316AVDriver::new(MutexDevice::new(&self.i2c_bus), 0b0000000))
+    }
+
+
     fn get_battery_voltage(&self) -> Result<f64> {
+        trace!("RaspiHardware::get_battery_voltage()");
         self.mcp3208.borrow_mut().read_single(1)
     }
 }

flight/src/hardware/raspi/pwm.rs

@@ -0,0 +1,55 @@
+use std::fmt::{Display, Formatter};
+use std::time::Duration;
+use embedded_hal::pwm::{ErrorKind, ErrorType, SetDutyCycle};
+use log::trace;
+use rpi_pal::pwm::Pwm;
+
+const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz
+
+pub struct PwmWrapper {
+    pwm: Pwm
+}
+
+impl PwmWrapper {
+    pub fn new(pwm: Pwm) -> anyhow::Result<Self> {
+        trace!("PwmWrapper::new(pwm: {pwm:?})");
+        pwm.set_period(PWM_PERIOD)?;
+        pwm.enable()?;
+        Ok(Self {
+            pwm
+        })
+    }
+}
+
+#[derive(Debug)]
+pub struct ErrorWrapper(rpi_pal::pwm::Error);
+
+impl embedded_hal::pwm::Error for ErrorWrapper {
+    fn kind(&self) -> ErrorKind {
+        ErrorKind::Other
+    }
+}
+
+impl Display for ErrorWrapper {
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        self.0.fmt(f)
+    }
+}
+
+impl std::error::Error for ErrorWrapper {}
+
+impl ErrorType for PwmWrapper {
+    type Error = ErrorWrapper;
+}
+
+impl SetDutyCycle for PwmWrapper {
+    fn max_duty_cycle(&self) -> u16 {
+        trace!("PwmWrapper::max_duty_cycle()");
+        u16::MAX
+    }
+
+    fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
+        trace!("PwmWrapper::set_duty_cycle(duty: {duty})");
+        self.pwm.set_duty_cycle((duty as f64) / (u16::MAX as f64)).map_err(ErrorWrapper)
+    }
+}

flight/src/lib.rs

@@ -3,7 +3,6 @@ use crate::hardware::initialize;
 use crate::hardware::mcp23017::Mcp23017OutputPin;
 use crate::hardware::mcp23017::{Mcp23017, Mcp23017Task};
 use crate::hardware::Hardware;
-use crate::logger::setup_logger;
 use crate::on_drop::on_drop;
 use anyhow::Result;
 use embedded_hal::digital::PinState;
@@ -12,12 +11,12 @@ use std::sync::atomic::{AtomicBool, Ordering};
 use std::thread;
 use std::thread::sleep;
 use std::time::Duration;
+use crate::hardware::mct8316a::Mct8316a;
+use embedded_hal::pwm::SetDutyCycle;
 
 mod hardware;
-mod logger;
 
 pub fn run() -> Result<()> {
-    setup_logger()?;
     info!(
         "Project Nautilus Flight Software {}",
         env!("CARGO_PKG_VERSION")
@@ -27,11 +26,16 @@ pub fn run() -> Result<()> {
 
     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)?;
+
     mcp23017_a.init()?;
     mcp23017_b.init()?;
+    mct8316.init()?;
 
     let running = AtomicBool::new(true);
 
@@ -67,6 +71,8 @@ pub fn run() -> Result<()> {
     // dropping the hal is safe
     drop(mcp23017_a);
     drop(mcp23017_b);
+    drop(pwm0);
+    drop(mct8316);
 
     drop(hal);
 

flight/src/logger.rs

@@ -1,8 +1,9 @@
 use anyhow::Result;
-use log::debug;
+use log::{debug, LevelFilter};
 use std::fs::create_dir_all;
 use std::str::FromStr;
 use std::{env, thread};
+use fern::colors::{Color, ColoredLevelConfig};
 
 pub fn setup_logger() -> Result<()> {
     let log_file = env::var("LOG_FILE").or_else(|_| {
@@ -19,18 +20,23 @@ pub fn setup_logger() -> Result<()> {
         Err(_) => log::LevelFilter::Info,
     };
 
+    let colors = ColoredLevelConfig::new()
+        .error(Color::Red)
+        .warn(Color::Yellow)
+        .info(Color::White)
+        .debug(Color::White)
+        .trace(Color::BrightBlack);
+
     fern::Dispatch::new()
-        .format(|out, message, record| {
+        .format(move |out, message, record| {
             let binding = thread::current();
             let thread_name = binding.name().unwrap_or("<unnamed>");
 
             out.finish(format_args!(
-                "[{}][{}][{}][{}] {}",
-                record.level(),
-                chrono::Local::now().format("%Y-%m-%d %H:%M:%S%.9f"),
-                thread_name,
-                record.target(),
-                message,
+                "[{time} {level} {thread_name} {target}] {message}",
+                level=colors.color(record.level()),
+                time=chrono::Local::now().format("%Y-%m-%dT%H:%M:%S%.9f"),
+                target=record.target(),
             ))
         })
         .chain(

flight/src/main.rs

@@ -1,11 +1,15 @@
 use log::error;
 use nautilus_flight::run;
+use crate::logger::setup_logger;
+
+mod logger;
 
 fn main() {
+    setup_logger().expect("Failed to setup logger");
     match run() {
         Ok(_) => {}
         Err(err) => {
-            error!("An unhandled error occurred: {}", err);
+            error!("An unhandled error occurred: {}\n\n{}", err, err.backtrace());
         }
     }
 }