initial work on mct8316a

2025-10-18 16:32:22 -07:00
parent e8f91d0d75
commit d552fe3627
20 changed files with 2221 additions and 42 deletions
--- a/flight/src/hardware/mct8316a/closed_loop.rs
+++ b/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,
+}
+
+