diff --git a/Cargo.lock b/Cargo.lock index b49651f..7d28ee4 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -13,9 +13,9 @@ dependencies = [ [[package]] name = "anyhow" -version = "1.0.97" +version = "1.0.100" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dcfed56ad506cb2c684a14971b8861fdc3baaaae314b9e5f9bb532cbe3ba7a4f" +checksum = "a23eb6b1614318a8071c9b2521f36b424b2c83db5eb3a0fead4a6c0809af6e61" [[package]] name = "approx" @@ -32,6 +32,12 @@ version = "1.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" +[[package]] +name = "bitflags" +version = "2.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2261d10cca569e4643e526d8dc2e62e433cc8aba21ab764233731f8d369bf394" + [[package]] name = "bumpalo" version = "3.17.0" @@ -59,6 +65,12 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +[[package]] +name = "cfg_aliases" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "613afe47fcd5fac7ccf1db93babcb082c5994d996f20b8b159f2ad1658eb5724" + [[package]] name = "chrono" version = "0.4.42" @@ -79,7 +91,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "117725a109d387c937a1533ce01b450cbde6b88abceea8473c4d7a85853cda3c" dependencies = [ "lazy_static", - "windows-sys", + "windows-sys 0.59.0", ] [[package]] @@ -109,6 +121,23 @@ version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" +[[package]] +name = "ctrlc" +version = "3.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "881c5d0a13b2f1498e2306e82cbada78390e152d4b1378fb28a84f4dcd0dc4f3" +dependencies = [ + "dispatch", + "nix", + "windows-sys 0.61.1", +] + +[[package]] +name = "dispatch" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bd0c93bb4b0c6d9b77f4435b0ae98c24d17f1c45b2ff844c6151a07256ca923b" + [[package]] name = "embedded-hal" version = "0.2.7" @@ -396,6 +425,7 @@ dependencies = [ "anyhow", "chrono", "crc", + "ctrlc", "embedded-hal 1.0.0", "embedded-hal-bus", "embedded-hal-mock", @@ -427,6 +457,18 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" +[[package]] +name = "nix" +version = "0.30.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "74523f3a35e05aba87a1d978330aef40f67b0304ac79c1c00b294c9830543db6" +dependencies = [ + "bitflags", + "cfg-if", + "cfg_aliases", + "libc", +] + [[package]] name = "num" version = "0.3.1" @@ -610,7 +652,7 @@ version = "1.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "17db5ecef7e0bebeb8bf8bc4c4b554e05e0205d7008f10bb37787892e7a6507b" dependencies = [ - "windows-sys", + "windows-sys 0.59.0", ] [[package]] @@ -626,18 +668,18 @@ dependencies = [ [[package]] name = "thiserror" -version = "2.0.16" +version = "2.0.17" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3467d614147380f2e4e374161426ff399c91084acd2363eaf549172b3d5e60c0" +checksum = "f63587ca0f12b72a0600bcba1d40081f830876000bb46dd2337a3051618f4fc8" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "2.0.16" +version = "2.0.17" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6c5e1be1c48b9172ee610da68fd9cd2770e7a4056cb3fc98710ee6906f0c7960" +checksum = "3ff15c8ecd7de3849db632e14d18d2571fa09dfc5ed93479bc4485c7a517c913" dependencies = [ "proc-macro2", "quote", @@ -754,6 +796,15 @@ dependencies = [ "windows-targets", ] +[[package]] +name = "windows-sys" +version = "0.61.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6f109e41dd4a3c848907eb83d5a42ea98b3769495597450cf6d153507b166f0f" +dependencies = [ + "windows-link", +] + [[package]] name = "windows-targets" version = "0.52.6" diff --git a/flight/Cargo.toml b/flight/Cargo.toml index 8f9eb34..ac7d34f 100644 --- a/flight/Cargo.toml +++ b/flight/Cargo.toml @@ -4,9 +4,9 @@ version = "0.0.1" edition = "2024" [dependencies] -anyhow = "1.0.97" +anyhow = "1.0.100" fern = { version = "0.7.1", features = ["colored"] } -log = "0.4.28" +log = { version = "0.4.28", features = ["max_level_trace", "release_max_level_debug"] } chrono = "0.4.42" embedded-hal = "1.0.0" embedded-hal-bus = { version = "0.3.0", features = ["std"] } @@ -14,9 +14,10 @@ embedded-hal-mock = { version = "0.11.1", optional = true } rpi-pal = { version = "0.22.2", features = ["hal"], optional = true } nalgebra = "0.34.1" hex = "0.4.3" -thiserror = "2.0.16" +thiserror = "2.0.17" num-traits = "0.2.19" crc = "3.3.0" +ctrlc = { version = "3.5" } [dev-dependencies] embedded-hal-mock = { version = "0.11.1" } diff --git a/flight/src/hardware/mct8316a/closed_loop.rs b/flight/src/hardware/mct8316a/closed_loop.rs index 96530b4..db3dca6 100644 --- a/flight/src/hardware/mct8316a/closed_loop.rs +++ b/flight/src/hardware/mct8316a/closed_loop.rs @@ -1,6 +1,6 @@ #![allow(dead_code)] -use crate::hardware::mct8316a::motor_startup::{EnableDisable, FullCurrentThreshold}; +use crate::hardware::mct8316a::motor_startup::FullCurrentThreshold; #[derive(Debug, Clone)] pub struct ClosedLoop1 { @@ -76,7 +76,7 @@ pub enum ClosedLoopRate { #[derive(Debug, Copy, Clone)] pub enum ClosedLoopDecelerationMode { DecelerationRate = 0x0, - AccerlerationRate = 0x1, + AccelerationRate = 0x1, } #[derive(Debug, Copy, Clone)] @@ -144,7 +144,7 @@ pub struct ClosedLoop2 { pub motor_stop_brake_time: MotorStopBrakeTime, pub active_low_high_brake_threshold: DutyCycleThreshold, pub brake_pin_threshold: DutyCycleThreshold, - pub avs_enable: EnableDisable, + pub avs_enable: bool, pub cycle_current_limit: FullCurrentThreshold, } @@ -159,7 +159,7 @@ impl Default for ClosedLoop2 { motor_stop_brake_time: MotorStopBrakeTime::Milliseconds1, active_low_high_brake_threshold: DutyCycleThreshold::Immediate, brake_pin_threshold: DutyCycleThreshold::Immediate, - avs_enable: EnableDisable::Disable, + avs_enable: false, cycle_current_limit: FullCurrentThreshold::NotApplicable, } } @@ -259,11 +259,11 @@ pub struct ClosedLoop3 { 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 bemf_threshold1: IntegrationBemfThreshold, pub commutation_method: CommutationMethod, pub degauss_window: DegaussWindow, - pub degauss_enable: EnableDisable, + pub degauss_enable: bool, } impl Default for ClosedLoop3 { @@ -276,11 +276,11 @@ impl Default for ClosedLoop3 { 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, + bemf_threshold1: IntegrationBemfThreshold::Value0, commutation_method: CommutationMethod::ZC, degauss_window: DegaussWindow::Degrees22_5, - degauss_enable: EnableDisable::Disable, + degauss_enable: false, } } } @@ -421,26 +421,26 @@ pub enum DegaussWindow { #[derive(Debug, Clone)] pub struct ClosedLoop4 { - pub wcomp_blanking: EnableDisable, + pub wcomp_blanking_enable: bool, 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 dynamic_braking_current_enable: bool, + pub fast_deceleration_enable: bool, + pub fast_deceleration_current_threshold: FullCurrentThreshold, pub fast_brake_delta: FastBrakeDelta, } impl Default for ClosedLoop4 { fn default() -> Self { Self { - wcomp_blanking: EnableDisable::Disable, + wcomp_blanking_enable: false, 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, + dynamic_braking_current_enable: false, + fast_deceleration_enable: false, + fast_deceleration_current_threshold: FullCurrentThreshold::NotApplicable, fast_brake_delta: FastBrakeDelta::Percent0_5, } } diff --git a/flight/src/hardware/mct8316a/constant_power.rs b/flight/src/hardware/mct8316a/constant_power.rs index f4bc4fa..049e12e 100644 --- a/flight/src/hardware/mct8316a/constant_power.rs +++ b/flight/src/hardware/mct8316a/constant_power.rs @@ -1,11 +1,9 @@ #![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 dead_time_compensation_enable: bool, pub max_power: f64, pub power_hysteresis: PowerHysteresis, pub mode: PowerMode, @@ -15,7 +13,7 @@ impl Default for ConstantPower { fn default() -> Self { Self { max_speed: 0.0, - dead_time_compensation: EnableDisable::Disable, + dead_time_compensation_enable: false, max_power: 0.0, power_hysteresis: PowerHysteresis::Percent5, mode: PowerMode::Disabled, diff --git a/flight/src/hardware/mct8316a/device_config.rs b/flight/src/hardware/mct8316a/device_config.rs new file mode 100644 index 0000000..7d18179 --- /dev/null +++ b/flight/src/hardware/mct8316a/device_config.rs @@ -0,0 +1,59 @@ +#![allow(dead_code)] + +#[derive(Debug, Clone)] +pub struct DeviceConfig { + // Max of 0x7FFF + pub max_frequency: u16, + pub stl_enable: bool, + pub ssm_enable: bool, + pub device_mode: DeviceMode, + pub pwm_range_select: PwmRangeSelect, + pub clock_source: ClockSource, + pub external_clock_enable: bool, + pub external_clock_frequency: ExternalClockFrequency, +} + +impl Default for DeviceConfig { + fn default() -> Self { + Self { + max_frequency: 0, + stl_enable: false, + ssm_enable: false, + device_mode: DeviceMode::Standby, + pwm_range_select: PwmRangeSelect::Hertz325To95kHz, + clock_source: ClockSource::InternalOscillator, + external_clock_enable: false, + external_clock_frequency: ExternalClockFrequency::KHertz8, + } + } +} + +#[derive(Debug, Copy, Clone)] +pub enum DeviceMode { + Standby = 0x0, + Sleep = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum PwmRangeSelect { + Hertz325To95kHz = 0x0, + Hertz10To325Hz = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum ClockSource { + InternalOscillator = 0x0, + ExternalClock = 0x3, +} + +#[derive(Debug, Copy, Clone)] +pub enum ExternalClockFrequency { + KHertz8 = 0x0, + KHertz16 = 0x1, + KHertz32 = 0x2, + KHertz64 = 0x3, + KHertz128 = 0x4, + KHertz256 = 0x5, + KHertz512 = 0x6, + KHertz1024 = 0x7, +} diff --git a/flight/src/hardware/mct8316a/driver.rs b/flight/src/hardware/mct8316a/driver.rs index 06ada93..853b0b0 100644 --- a/flight/src/hardware/mct8316a/driver.rs +++ b/flight/src/hardware/mct8316a/driver.rs @@ -1,18 +1,22 @@ -use std::fmt::{Debug, Display, Formatter}; -use std::sync::Mutex; -use embedded_hal::i2c::{I2c, Operation}; -use log::trace; +use crate::hardware::error::I2cError; +use crate::hardware::mct8316a::closed_loop::{BemfThreshold, ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4, ClosedLoopDecelerationMode, ClosedLoopRate, CommutationMethod, CommutationMode, DegaussLowerBound, DegaussSamples, DegaussUpperBound, DegaussWindow, DutyCycleThreshold, FastBrakeDelta, IntegrationBemfThreshold, IntegrationCycleHighThreshold, IntegrationCycleLowThreshold, IntegrationDutyCycleThreshold, LeadAnglePolarity, LowerPercentLimit, MotorStopBrakeTime, MotorStopMode, PwmFrequency, PwmMode, PwmModulation, SpeedFeedbackConfig, SpeedFeedbackDivision, SpeedFeedbackMode, UpperPercentLimit}; +use crate::hardware::mct8316a::constant_power::{ConstantPower, PowerHysteresis, PowerMode}; +use crate::hardware::mct8316a::constant_speed::{ClosedLoopMode, ConstantSpeed}; +use crate::hardware::mct8316a::device_config::{ClockSource, DeviceConfig, DeviceMode, ExternalClockFrequency, PwmRangeSelect}; +use crate::hardware::mct8316a::eeprom::Mct8316AVEeprom; +use crate::hardware::mct8316a::fault_config::{AbnormalSpeedLock, AbnormalSpeedLockThreshold, AutomaticRetries, CycleByCycleCurrentLimit, FaultConfig1, FaultConfig2, LockDetectionCurrentLimit, LockDetectionCurrentLimitDeglitchTime, LockMinSpeed, LockMode, LockRetryTime, LossSyncTimes, MaxMotorVoltage, MinMotorVoltage, MotorVoltageMode, NoMotorDetectDeglitchTime, NoMotorThreshold, ZeroDutyThreshold}; +use crate::hardware::mct8316a::gate_driver_config::{BuckCurrentLimit, BuckSlewRate, BuckVoltageSelection, CurrentSenseAmplifier, DemagComparatorThreshold, GateDriverConfig1, GateDriverConfig2, OvercurrentFaultMode, OvercurrentProtectionDeglitchTime, OvercurrentProtectionLevel, OvercurrentProtectionRetryTime, OvervoltageProtectionLevel, SlewRate, TargetDelay}; +use crate::hardware::mct8316a::isd_config::{BrakeConfig, BrakeMode, IsdConfig, IsdConfigTimeValue, ResyncMinimumThreshold, StartupBreakTime, Threshold}; +use crate::hardware::mct8316a::motor_startup::{AlignRampRate, AlignTime, DutyCycle, FirstCycleFrequencySelect, FullCurrentThreshold, IpdAdvanceAngle, IpdClockFrequency, IpdCurrentThreshold, IpdReleaseMode, IpdRepeat, MinimumDutyCycle, MotorStartup1, MotorStartup2, MotorStartupMethod, OpenClosedHandoffThreshold, OpenLoopAcceleration1, OpenLoopAcceleration2, OpenLoopCurrentLimitMode, SlowFirstCycleFrequency}; +use crate::hardware::mct8316a::phase_profile::{ProfileSetting, ThreePhase150DegreeProfile, ThreePhaseLeadAngle, TwoPhase150DegreeProfile}; +use crate::hardware::mct8316a::pin_config::{BrakeInputConfig, DirectionInputConfig, ExternalWatchdogFaultMode, ExternalWatchdogFrequency, ExternalWatchdogSource, Pin36Config, Pin37_38Config, PinConfig1, PinConfig2, SleepTime, SpeedInputConfig}; +use crate::hardware::mct8316a::trap_config::{AVSLimitHysteresis, AVSNegativeCurrentLimit, FastStartupDivFactor, IsdBemfThreshold, IsdCycleThreshold, OpenLoopHandoffCycles, OpenLoopZcDetectionThreshold, TrapConfig1, TrapConfig2}; 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}; +use embedded_hal::i2c::{I2c, Operation}; +use log::trace; +use std::fmt::{Debug, Display, Formatter}; +use std::sync::Mutex; const CRC: crc::Crc = crc::Crc::::new(&crc::CRC_8_SMBUS); @@ -112,35 +116,35 @@ where // 3 for control word // 8 for data // 1 for crc - let mut write_data = [0u8; 1+3+8+1]; + 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 + 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; + 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"), - // } + match self.i2c.lock() { + Ok(mut lock) => lock.write(self.address, &write_data).map_err(I2cError)?, + Err(_) => bail!("Lock was poisoned"), + } Ok(()) } @@ -152,13 +156,13 @@ where // 1 target id // 8 data bytes // 1 crc - let mut read_data = [0u8; 1+3+1+8+1]; + 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 + address, )); read_data[4] = (self.address << 1) | 0b1; let data_length = match data { @@ -171,18 +175,18 @@ where let mut i2c_ops = [ Operation::Write(&left[1..4]), - Operation::Read(&mut right[..(data_length+1)]) + 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"), - // } + 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)]); + let expected_crc = CRC.checksum(&read_data[0..(5 + data_length)]); - // ensure!(expected_crc == read_data[5+data_length], "CRC Mismatch"); + ensure!(expected_crc == read_data[5+data_length], "CRC Mismatch"); match data { Mct8316AVData::Two(val) => { @@ -224,7 +228,7 @@ where enable_reverse_drive: true, enable_resynchronization: true, enable_stationary_brake: true, - stationary_detect_threshold: Threshold::MilliVolt25, + stationary_detect_threshold: Threshold::MilliVolt20, brake_mode: BrakeMode::LowOn, brake_config: BrakeConfig::BrakeTimeExit, brake_current_threshold: Threshold::MilliVolt15, @@ -235,48 +239,203 @@ where })? .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, + align_ramp_rate: AlignRampRate::VoltsPerSecond10, + align_time: AlignTime::Millisecond100, + align_current_threshold: FullCurrentThreshold::Volts0_3, + ipd_clock_frequency: IpdClockFrequency::Hertz50, 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, + ipd_release_mode: IpdReleaseMode::Brake, + ipd_advance_angle: IpdAdvanceAngle::Degrees90, + ipd_repeat: IpdRepeat::Average3, + slow_first_cycle_frequency: SlowFirstCycleFrequency::Hertz1, })? .set_motor_startup2(MotorStartup2 { - ..MotorStartup2::default() + open_loop_current_limit_mode: OpenLoopCurrentLimitMode::OpenLoopCurrentLimit, + open_loop_duty_cycle: DutyCycle::Percent20, + open_loop_current_limit: FullCurrentThreshold::Volts0_4, + open_loop_acceleration1: OpenLoopAcceleration1::HertzPerSecond2_5, + open_loop_acceleration2: OpenLoopAcceleration2::HertzPerSecondSecond2_5, + open_closed_handoff_threshold: OpenClosedHandoffThreshold::Hertz70, + auto_handoff_enable: true, + first_cycle_frequency_select: FirstCycleFrequencySelect::SlowFirstCycle, + minimum_duty_cycle: MinimumDutyCycle::Percent8, })? .set_closed_loop1(ClosedLoop1 { - ..ClosedLoop1::default() + commutation_mode: CommutationMode::Degrees120, + closed_loop_acceleration_rate: ClosedLoopRate::VoltsPerSecond12_5, + closed_loop_deceleration_mode: ClosedLoopDecelerationMode::DecelerationRate, + closed_loop_deceleration_rate: ClosedLoopRate::VoltsPerSecond12_5, + pwm_frequency: PwmFrequency::Kilohertz25, + pwm_modulation: PwmModulation::Mixed, + pwm_mode: PwmMode::SingleEnded, + lead_angle_polarity: LeadAnglePolarity::Positive, + lead_angle: 0.0, })? .set_closed_loop2(ClosedLoop2 { - ..ClosedLoop2::default() + speed_feedback_mode: SpeedFeedbackMode::Always, + speed_feedback_division: SpeedFeedbackDivision::Pole2, + speed_feedback_config: SpeedFeedbackConfig::AboveBEMFThreshold, + bemf_threshold: BemfThreshold::MilliVolt5, + motor_stop_mode: MotorStopMode::HighImpedance, + motor_stop_brake_time: MotorStopBrakeTime::Milliseconds100, + active_low_high_brake_threshold: DutyCycleThreshold::Percent10, + brake_pin_threshold: DutyCycleThreshold::Percent10, + avs_enable: true, + cycle_current_limit: FullCurrentThreshold::Volts0_6, })? .set_closed_loop3(ClosedLoop3 { - ..ClosedLoop3::default() + degauss_samples: DegaussSamples::Samples4, + degauss_upper_bound: DegaussUpperBound::Volts0_12, + degauss_lower_bound: DegaussLowerBound::Volts0_09, + integration_cycle_low_threshold: IntegrationCycleLowThreshold::Samples4, + integration_cycle_high_threshold: IntegrationCycleHighThreshold::Samples8, + integration_duty_cycle_low_threshold: IntegrationDutyCycleThreshold::Percent15, + integration_duty_cycle_high_threshold: IntegrationDutyCycleThreshold::Percent18, + bemf_threshold2: IntegrationBemfThreshold::Value0, + bemf_threshold1: IntegrationBemfThreshold::Value775, + commutation_method: CommutationMethod::ZC, + degauss_window: DegaussWindow::Degrees22_5, + degauss_enable: false, })? .set_closed_loop4(ClosedLoop4 { - ..ClosedLoop4::default() + wcomp_blanking_enable: true, + fast_deceleration_duty_window: LowerPercentLimit::Percent10, + fast_deceleration_duty_threshold: UpperPercentLimit::Percent65, + dynamic_brake_current_lower_threshold: FullCurrentThreshold::Volts0_4, + dynamic_braking_current_enable: true, + fast_deceleration_enable: false, + fast_deceleration_current_threshold: FullCurrentThreshold::Volts0_8, + fast_brake_delta: FastBrakeDelta::Percent2_5, })? .set_constant_speed(ConstantSpeed { - ..ConstantSpeed::default() + speed_power_kp: 0.001f64, + speed_power_ki: 0.000005f64, + speed_power_upper_limit: UpperPercentLimit::Percent100, + speed_power_lower_limit: LowerPercentLimit::Percent10, + mode: ClosedLoopMode::Disabled, })? .set_constant_power(ConstantPower { - ..ConstantPower::default() + max_speed: 3000f64, + dead_time_compensation_enable: true, + max_power: 50f64, + power_hysteresis: PowerHysteresis::Percent7_5, + mode: PowerMode::Disabled, })? .set_two_phase_profile(TwoPhase150DegreeProfile { - ..TwoPhase150DegreeProfile::default() + steps: [ + ProfileSetting::Percent97_5, + ProfileSetting::Percent93_75, + ProfileSetting::Percent83_75, + ProfileSetting::Percent83_75, + ProfileSetting::Percent83_75, + ProfileSetting::Percent75, + ProfileSetting::Percent50, + ProfileSetting::Percent50, + ], })? .set_three_phase_profile(ThreePhase150DegreeProfile { - ..ThreePhase150DegreeProfile::default() + steps: [ + ProfileSetting::Percent83_75, + ProfileSetting::Percent87_5, + ProfileSetting::Percent87_5, + ProfileSetting::Percent93_75, + ProfileSetting::Percent93_75, + ProfileSetting::Percent99, + ProfileSetting::Percent99, + ProfileSetting::Percent99, + ], + lead_angle: ThreePhaseLeadAngle::Degrees0, })? .set_trap_config1(TrapConfig1 { - ..TrapConfig1::default() + open_loop_handoff_cycles: OpenLoopHandoffCycles::Cycles6, + avs_negative_current_limit: AVSNegativeCurrentLimit::Limit0, + avs_limit_hysteresis: AVSLimitHysteresis::Limit10, + isd_bemf_threshold: IsdBemfThreshold::Limit1600, + isd_cycle_threshold: IsdCycleThreshold::Limit11, + open_loop_zc_detection_threshold: OpenLoopZcDetectionThreshold::Degrees8, + fast_startup_div_factor: FastStartupDivFactor::Value4, })? .set_trap_config2(TrapConfig2 { - ..TrapConfig2::default() + blanking_time_microseconds: 0b0111, + comparator_deglitch_time_microseconds: 0b010, + align_duty_cycle: DutyCycle::Percent15, + })? + .set_fault_config1(FaultConfig1 { + no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime::Milliseconds100, + cycle_by_cycle_current_limit: CycleByCycleCurrentLimit::RecoverNextPwmFaultInactiveRecirculation, + lock_detection_current_limit: LockDetectionCurrentLimit::Volts1_4, + lock_detection_current_limit_mode: LockMode::ReportOnly, + lock_detection_current_limit_deglitch_time: LockDetectionCurrentLimitDeglitchTime::Milliseconds75, + cycle_by_cycle_pwm_limit: 0, + motor_lock_mode: LockMode::RecoverRetryTristated, + lock_retry_time: LockRetryTime::Milliseconds5000, + })? + .set_fault_config2(FaultConfig2 { + lock_abnormal_speed_enable: true, + lock_loss_of_sync_enable: true, + lock_no_motor_enable: true, + abnormal_speed_lock_threshold: AbnormalSpeedLockThreshold::Hertz4000, + loss_sync_times: LossSyncTimes::Count6, + no_motor_threshold: NoMotorThreshold::Volts0_0075, + max_motor_voltage_mode: MotorVoltageMode::AutomaticClear, + max_motor_voltage: MaxMotorVoltage::Volts50, + min_motor_voltage_mode: MotorVoltageMode::AutomaticClear, + min_motor_voltage: MinMotorVoltage::NoLimit, + automatic_retries: AutomaticRetries::NoLimit, + lock_min_speed: LockMinSpeed::Hertz0_5, + abnormal_speed_lock: AbnormalSpeedLock::Ratio6, + zero_duty_threshold: ZeroDutyThreshold::Percent1_5, + })? + .set_pin_config1(PinConfig1 { + dacout1_address: 0x5ae, + dacout2_address: 0x40c, + brake_input_config: BrakeInputConfig::HardwarePinBrake, + direction_input_config: DirectionInputConfig::HardwarePinDirection, + // Modification from default + speed_input_config: SpeedInputConfig::Pwm, + })? + .set_pin_config2(PinConfig2 { + pin36config: Pin36Config::Dacout2, + pin37_38config: Pin37_38Config::Pin37Dacout1Pin38Dacout2, + sleep_time: SleepTime::Sleep50micros, + enable_external_watchdog: false, + external_watchdog_source: ExternalWatchdogSource::I2c, + external_watchdog_fault_mode: ExternalWatchdogFaultMode::ReportOnly, + external_watchdog_frequency: ExternalWatchdogFrequency::Hertz10, + })? + .set_device_config(DeviceConfig { + max_frequency: 0x7FFF, + stl_enable: false, + ssm_enable: false, + device_mode: DeviceMode::Standby, + pwm_range_select: PwmRangeSelect::Hertz325To95kHz, + clock_source: ClockSource::InternalOscillator, + external_clock_enable: false, + external_clock_frequency: ExternalClockFrequency::KHertz8, + })? + .set_gate_driver_config1(GateDriverConfig1 { + slew_rate: SlewRate::VoltsPerMicro200, + overvoltage_protection_level: OvervoltageProtectionLevel::Volts32, + overvoltage_protection_enable: true, + overtemperature_warning_enable: false, + overcurrent_protection_deglitch_time: OvercurrentProtectionDeglitchTime::Microsecond0_2, + overcurrent_protection_retry_time: OvercurrentProtectionRetryTime::Millisecond5, + overcurrent_protection_level: OvercurrentProtectionLevel::Amps16, + overcurrent_fault_mode: OvercurrentFaultMode::Latch, + low_demag_comparator_threshold: DemagComparatorThreshold::Milliamps100, + high_demag_comparator_threshold: DemagComparatorThreshold::Milliamps100, + synchronous_rectification_enable: false, + asynchronous_rectification_enable: false, + current_sense_amplifier: CurrentSenseAmplifier::VoltsPerAmp0_15, + })? + .set_gate_driver_config2(GateDriverConfig2 { + driver_delay_compensation_enable: false, + target_delay: TargetDelay::Automatic, + buck_slew_rate: BuckSlewRate::VoltsPerMicrosecond1000, + buck_power_sequencing_disable: false, + buck_current_limit: BuckCurrentLimit::Milliamps600, + buck_voltage_selection: BuckVoltageSelection::Volts3_3, + buck_disable: false, })? .commit()?; diff --git a/flight/src/hardware/mct8316a/eeprom.rs b/flight/src/hardware/mct8316a/eeprom.rs index 6252f66..f4bde5a 100644 --- a/flight/src/hardware/mct8316a/eeprom.rs +++ b/flight/src/hardware/mct8316a/eeprom.rs @@ -1,15 +1,21 @@ -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::device_config::DeviceConfig; use crate::hardware::mct8316a::driver::Mct8316AVData; +use crate::hardware::mct8316a::fault_config::{FaultConfig1, FaultConfig2}; +use crate::hardware::mct8316a::gate_driver_config::{GateDriverConfig1, GateDriverConfig2}; 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::pin_config::{PinConfig1, PinConfig2}; use crate::hardware::mct8316a::trap_config::{TrapConfig1, TrapConfig2}; +use crate::hardware::mct8316a::Mct8316AVDriver; +use anyhow::Result; +use embedded_hal::i2c::I2c; +use log::trace; +use std::thread::sleep; +use std::time::Duration; pub struct Mct8316AVEeprom<'a, I2C> where @@ -32,7 +38,9 @@ where pub fn load(driver: &'a mut Mct8316AVDriver) -> Result { trace!("Mct8316AVEeprom::load()"); - // driver.write(0x0000E6, Mct8316AVData::Four(0x40000000))?; + driver.write(0x0000E6, Mct8316AVData::Four(0x40000000))?; + // Wait 100ms for the EEPROM operation to complete + sleep(Duration::from_millis(100)); Ok(Self { driver, @@ -43,7 +51,7 @@ where pub fn set_isd_config(self, isd_config: IsdConfig) -> Result { trace!("Mct8316AVEeprom::set_isd_config(isd_config: {isd_config:?})"); - let mut expected_value = + let 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 } @@ -59,18 +67,13 @@ where | ((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 { trace!("Mct8316AVEeprom::set_motor_startup1(motor_startup1: {motor_startup1:?})"); - let mut expected_value = + let 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 @@ -82,33 +85,23 @@ where | ((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 { trace!("Mct8316AVEeprom::set_motor_startup2(motor_startup2: {motor_startup2:?})"); - let mut expected_value = + let 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 + | if motor_startup2.auto_handoff_enable { 1u32 << 7 } else { 0u32 } | ((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)) } @@ -117,29 +110,24 @@ where let lead_angle = (closed_loop1.lead_angle / 0.12f32).round().clamp(0.0f32, u8::MAX as f32) as u8; - let mut expected_value = + let 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.closed_loop_deceleration_rate 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 { trace!("Mct8316AVEeprom::set_closed_loop2(closed_loop2: {closed_loop2:?})"); - let mut expected_value = + let 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 @@ -148,21 +136,16 @@ where | ((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 + | if closed_loop2.avs_enable { 1u32 << 7 } else { 0u32 } | ((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 { trace!("Mct8316AVEeprom::set_closed_loop3(closed_loop3: {closed_loop3:?})"); - let mut expected_value = + let 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 @@ -174,12 +157,7 @@ where | ((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; - } + | if closed_loop3.degauss_enable { 1 } else { 0u32 }; self.assert_register(0x8A, Mct8316AVData::Four(expected_value)) } @@ -187,21 +165,16 @@ where pub fn set_closed_loop4(self, closed_loop4: ClosedLoop4) -> Result { trace!("Mct8316AVEeprom::set_closed_loop4(closed_loop4: {closed_loop4:?})"); - let mut expected_value = - ((closed_loop4.wcomp_blanking as u32) & 0x1) << 19 + let expected_value = + if closed_loop4.wcomp_blanking_enable { 1u32 << 19 } else { 0u32 } | ((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 + | if closed_loop4.dynamic_braking_current_enable { 1u32 << 8 } else { 0u32 } + | if closed_loop4.fast_deceleration_enable { 1u32 << 7 } else { 0u32 } + | ((closed_loop4.fast_deceleration_current_threshold 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)) } @@ -211,18 +184,13 @@ where 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 = + let 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)) } @@ -232,25 +200,20 @@ where 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 = + let expected_value = max_speed << 15 - | ((constant_power.dead_time_compensation as u32) & 0x1) << 14 + | if constant_power.dead_time_compensation_enable { 1u32 << 14 } else { 0u32 } | 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 { trace!("Mct8316AVEeprom::set_two_phase_profile(profile: {profile:?})"); - let mut expected_value = + let expected_value = ((profile.steps[0] as u32) & 0x7) << 28 | ((profile.steps[1] as u32) & 0x7) << 25 | ((profile.steps[2] as u32) & 0x7) << 22 @@ -260,18 +223,13 @@ where | ((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 { trace!("Mct8316AVEeprom::set_three_phase_profile(profile: {profile:?})"); - let mut expected_value = + let expected_value = ((profile.steps[0] as u32) & 0x7) << 28 | ((profile.steps[1] as u32) & 0x7) << 25 | ((profile.steps[2] as u32) & 0x7) << 22 @@ -282,18 +240,13 @@ where | ((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 { trace!("Mct8316AVEeprom::set_trap_config1(trap_config1: {trap_config1:?})"); - let mut expected_value = + let 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 @@ -303,31 +256,138 @@ where | ((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 { trace!("Mct8316AVEeprom::set_trap_config2(trap_config2: {trap_config2:?})"); - let mut expected_value = + let 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)) } + pub fn set_fault_config1(self, fault_config1: FaultConfig1) -> Result { + trace!("Mct8316AVEeprom::set_fault_config1(fault_config1: {fault_config1:?})"); + + let expected_value = + ((fault_config1.no_motor_detect_deglitch_time as u32) & 0x7) << 27 + | ((fault_config1.cycle_by_cycle_current_limit as u32) & 0xF) << 23 + | ((fault_config1.lock_detection_current_limit as u32) & 0xF) << 19 + | ((fault_config1.lock_detection_current_limit_mode as u32) & 0xF) << 15 + | ((fault_config1.lock_detection_current_limit_deglitch_time as u32) & 0xF) << 11 + | ((fault_config1.cycle_by_cycle_pwm_limit as u32) & 0x7) << 8 + | ((fault_config1.motor_lock_mode as u32) & 0xF) << 3 + | ((fault_config1.lock_retry_time as u32) & 0x7) << 0; + + self.assert_register(0x92, Mct8316AVData::Four(expected_value)) + } + + pub fn set_fault_config2(self, fault_config2: FaultConfig2) -> Result { + trace!("Mct8316AVEeprom::set_fault_config2(fault_config2: {fault_config2:?})"); + + let expected_value = + if fault_config2.lock_abnormal_speed_enable { 1u32 << 30 } else { 0u32 } + | if fault_config2.lock_loss_of_sync_enable { 1u32 << 29 } else { 0u32 } + | if fault_config2.lock_no_motor_enable { 1u32 << 28 } else { 0u32 } + | ((fault_config2.abnormal_speed_lock_threshold as u32) & 0xF) << 24 + | ((fault_config2.loss_sync_times as u32) & 0x7) << 21 + | ((fault_config2.no_motor_threshold as u32) & 0x7) << 18 + | ((fault_config2.max_motor_voltage_mode as u32) & 0x1) << 17 + | ((fault_config2.max_motor_voltage as u32) & 0x7) << 14 + | ((fault_config2.min_motor_voltage_mode as u32) & 0x1) << 13 + | ((fault_config2.min_motor_voltage as u32) & 0x7) << 10 + | ((fault_config2.automatic_retries as u32) & 0x7) << 7 + | ((fault_config2.lock_min_speed as u32) & 0x7) << 4 + | ((fault_config2.abnormal_speed_lock as u32) & 0x3) << 2 + | ((fault_config2.zero_duty_threshold as u32) & 0x3) << 0; + + self.assert_register(0x94, Mct8316AVData::Four(expected_value)) + } + + pub fn set_pin_config1(self, pin_config1: PinConfig1) -> Result { + trace!("Mct8316AVEeprom::set_pin_config1(pin_config1: {pin_config1:?})"); + + let expected_value = + ((pin_config1.dacout1_address as u32) & 0xFFF) << 19 + | ((pin_config1.dacout2_address as u32) & 0xFFF) << 7 + | ((pin_config1.brake_input_config as u32) & 0x3) << 5 + | ((pin_config1.direction_input_config as u32) & 0x3) << 3 + | ((pin_config1.speed_input_config as u32) & 0x3) << 1; + + self.assert_register(0xA4, Mct8316AVData::Four(expected_value)) + } + + pub fn set_pin_config2(self, pin_config2: PinConfig2) -> Result { + trace!("Mct8316AVEeprom::set_pin_config2(pin_config2: {pin_config2:?})"); + + let expected_value = + ((pin_config2.pin36config as u32) & 0x3) << 29 + | ((pin_config2.pin37_38config as u32) & 0x1) << 27 + | ((pin_config2.sleep_time as u32) & 0x3) << 18 + | if pin_config2.enable_external_watchdog { 1u32 << 17 } else { 0u32 } + | ((pin_config2.external_watchdog_source as u32) & 0x1) << 16 + | ((pin_config2.external_watchdog_fault_mode as u32) & 0x1) << 15 + | ((pin_config2.external_watchdog_frequency as u32) & 0x3) << 13; + + self.assert_register(0xA6, Mct8316AVData::Four(expected_value)) + } + + pub fn set_device_config(self, device_config: DeviceConfig) -> Result { + trace!("Mct8316AVEeprom::set_device_config(device_config: {device_config:?})"); + + let expected_value = + ((device_config.max_frequency as u32) & 0x7FFF) << 16 + | if device_config.stl_enable { 1u32 << 15 } else { 0u32 } + | if device_config.ssm_enable { 1u32 << 14 } else { 0u32 } + | ((device_config.device_mode as u32) & 0x1) << 11 + | ((device_config.pwm_range_select as u32) & 0x1) << 10 + | ((device_config.clock_source as u32) & 0x3) << 8 + | if device_config.external_clock_enable { 1u32 << 6 } else { 0u32 } + | ((device_config.external_clock_frequency as u32) & 0x7) << 3; + + self.assert_register(0xA8, Mct8316AVData::Four(expected_value)) + } + + pub fn set_gate_driver_config1(self, gate_driver_config1: GateDriverConfig1) -> Result { + trace!("Mct8316AVEeprom::set_gate_driver_config1(gate_driver_config1: {gate_driver_config1:?})"); + + let expected_value = + ((gate_driver_config1.slew_rate as u32) & 0x3) << 26 + | ((gate_driver_config1.overvoltage_protection_level as u32) & 0x1) << 19 + | if gate_driver_config1.overvoltage_protection_enable { 1u32 << 18 } else { 0u32 } + | if gate_driver_config1.overtemperature_warning_enable { 1u32 << 16 } else { 0u32 } + | ((gate_driver_config1.overcurrent_protection_deglitch_time as u32) & 0x3) << 12 + | ((gate_driver_config1.overcurrent_protection_retry_time as u32) & 0x1) << 11 + | ((gate_driver_config1.overcurrent_protection_level as u32) & 0x1) << 10 + | ((gate_driver_config1.overcurrent_fault_mode as u32) & 0x3) << 8 + | ((gate_driver_config1.low_demag_comparator_threshold as u32) & 0x1) << 5 + | ((gate_driver_config1.high_demag_comparator_threshold as u32) & 0x1) << 4 + | if gate_driver_config1.synchronous_rectification_enable { 1u32 << 3 } else { 0u32 } + | if gate_driver_config1.asynchronous_rectification_enable { 1u32 << 2 } else { 0u32 } + | ((gate_driver_config1.current_sense_amplifier as u32) & 0x3) << 0; + + self.assert_register(0xAC, Mct8316AVData::Four(expected_value)) + } + + pub fn set_gate_driver_config2(self, gate_driver_config2: GateDriverConfig2) -> Result { + trace!("Mct8316AVEeprom::set_gate_driver_config2(gate_driver_config2: {gate_driver_config2:?})"); + + let expected_value = + if gate_driver_config2.driver_delay_compensation_enable { 1u32 << 30 } else { 0u32 } + | ((gate_driver_config2.target_delay as u32) & 0xF) << 26 + | ((gate_driver_config2.buck_slew_rate as u32) & 0x1) << 25 + | if gate_driver_config2.buck_power_sequencing_disable { 1u32 << 24 } else { 0u32 } + | ((gate_driver_config2.buck_current_limit as u32) & 0x1) << 23 + | ((gate_driver_config2.buck_voltage_selection as u32) & 0x3) << 21 + | if gate_driver_config2.buck_disable { 1u32 << 20 } else { 0u32 }; + + self.assert_register(0xAE, Mct8316AVData::Four(expected_value)) + } + fn assert_register(self, address: u32, value: Mct8316AVData) -> Result { trace!("Mct8316AVEeprom::assert_register(address: {address:06x}, value: {value})"); @@ -335,6 +395,10 @@ where self.driver.read(address, &mut read_value)?; + if let Mct8316AVData::Four(value) = &mut read_value { + *value &= !0x8000_0000u32; + } + if read_value == value { Ok(self) } else { @@ -350,7 +414,9 @@ where pub fn commit(self) -> Result<()> { trace!("Mct8316AVEeprom::commit()"); if self.modified { - bail!("TODO"); + self.driver.write(0x0000E6, Mct8316AVData::Four(0x80000000))?; + // Wait for EEPROM operation to complete + sleep(Duration::from_millis(100)); } Ok(()) } diff --git a/flight/src/hardware/mct8316a/fault_config.rs b/flight/src/hardware/mct8316a/fault_config.rs new file mode 100644 index 0000000..a244a82 --- /dev/null +++ b/flight/src/hardware/mct8316a/fault_config.rs @@ -0,0 +1,275 @@ +#![allow(dead_code)] + +#[derive(Debug, Clone)] +pub struct FaultConfig1 { + pub no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime, + pub cycle_by_cycle_current_limit: CycleByCycleCurrentLimit, + pub lock_detection_current_limit: LockDetectionCurrentLimit, + pub lock_detection_current_limit_mode: LockMode, + pub lock_detection_current_limit_deglitch_time: LockDetectionCurrentLimitDeglitchTime, + /// Max of 0x7 + pub cycle_by_cycle_pwm_limit: u8, + pub motor_lock_mode: LockMode, + pub lock_retry_time: LockRetryTime, +} + +impl Default for FaultConfig1 { + fn default() -> Self { + Self { + no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime::Milliseconds1, + cycle_by_cycle_current_limit: CycleByCycleCurrentLimit::RecoverNextPwmFaultActiveRecirculation, + lock_detection_current_limit: LockDetectionCurrentLimit::NotApplicable, + lock_detection_current_limit_mode: LockMode::LatchFaultTristated, + lock_detection_current_limit_deglitch_time: LockDetectionCurrentLimitDeglitchTime::Milliseconds1, + cycle_by_cycle_pwm_limit: 0, + motor_lock_mode: LockMode::LatchFaultTristated, + lock_retry_time: LockRetryTime::Milliseconds100, + } + } +} + +#[derive(Debug, Copy, Clone)] +pub enum NoMotorDetectDeglitchTime { + Milliseconds1 = 0x0, + Milliseconds10 = 0x1, + Milliseconds25 = 0x2, + Milliseconds50 = 0x3, + Milliseconds100 = 0x4, + Milliseconds250 = 0x5, + Milliseconds500 = 0x6, + Milliseconds1000 = 0x7, +} + +#[derive(Debug, Copy, Clone)] +pub enum CycleByCycleCurrentLimit { + RecoverNextPwmFaultActiveRecirculation = 0x0, + RecoverNextPwmFaultInactiveRecirculation = 0x1, + RecoverVsoxFaultActiveRecirculation = 0x2, + RecoverVsoxFaultInactiveRecirculation = 0x3, + RecoverRetryFaultActiveRecirculation = 0x4, + RecoverRetryFaultInactiveRecirculation = 0x5, + ReportOnly = 0x6, + Disabled = 0xF, +} + +#[derive(Debug, Copy, Clone)] +pub enum LockDetectionCurrentLimit { + 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 LockMode { + LatchFaultTristated = 0x0, + LatchFaultRecirculation = 0x1, + LatchFaultHighSideBrake = 0x2, + LatchFaultLowSideBrake = 0x3, + RecoverRetryTristated = 0x4, + RecoverRetryRecirculation = 0x5, + RecoverRetryHighSideBrake = 0x6, + RecoverRetryLowSideBrake = 0x7, + ReportOnly = 0x8, + Disabled = 0xF, +} + +#[derive(Debug, Copy, Clone)] +pub enum LockDetectionCurrentLimitDeglitchTime { + Milliseconds1 = 0x0, + Milliseconds2 = 0x1, + Milliseconds5 = 0x2, + Milliseconds10 = 0x3, + Milliseconds25 = 0x4, + Milliseconds50 = 0x5, + Milliseconds75 = 0x6, + Milliseconds100 = 0x7, + Milliseconds250 = 0x8, + Milliseconds500 = 0x9, + Seconds1 = 0xA, + Seconds2_5 = 0xB, + Seconds5 = 0xC, + Seconds10 = 0xD, + Seconds25 = 0xE, + Seconds50 = 0xF, +} + +#[derive(Debug, Copy, Clone)] +pub enum LockRetryTime { + Milliseconds100 = 0x0, + Milliseconds500 = 0x1, + Milliseconds1000 = 0x2, + Milliseconds2000 = 0x3, + Milliseconds3000 = 0x4, + Milliseconds5000 = 0x5, + Milliseconds7500 = 0x6, + Milliseconds10000 = 0x7, +} + +#[derive(Debug, Clone)] +pub struct FaultConfig2 { + /// Lock 1 + pub lock_abnormal_speed_enable: bool, + /// Lock 2 + pub lock_loss_of_sync_enable: bool, + /// Lock 3 + pub lock_no_motor_enable: bool, + pub abnormal_speed_lock_threshold: AbnormalSpeedLockThreshold, + pub loss_sync_times: LossSyncTimes, + pub no_motor_threshold: NoMotorThreshold, + pub max_motor_voltage_mode: MotorVoltageMode, + pub max_motor_voltage: MaxMotorVoltage, + pub min_motor_voltage_mode: MotorVoltageMode, + pub min_motor_voltage: MinMotorVoltage, + pub automatic_retries: AutomaticRetries, + pub lock_min_speed: LockMinSpeed, + pub abnormal_speed_lock: AbnormalSpeedLock, + pub zero_duty_threshold: ZeroDutyThreshold, +} + +impl Default for FaultConfig2 { + fn default() -> Self { + Self { + lock_abnormal_speed_enable: false, + lock_loss_of_sync_enable: false, + lock_no_motor_enable: false, + abnormal_speed_lock_threshold: AbnormalSpeedLockThreshold::Hertz250, + loss_sync_times: LossSyncTimes::Count2, + no_motor_threshold: NoMotorThreshold::Volts0_005, + max_motor_voltage_mode: MotorVoltageMode::Latch, + max_motor_voltage: MaxMotorVoltage::NoLimit, + min_motor_voltage_mode: MotorVoltageMode::Latch, + min_motor_voltage: MinMotorVoltage::NoLimit, + automatic_retries: AutomaticRetries::NoLimit, + lock_min_speed: LockMinSpeed::Hertz0_5, + abnormal_speed_lock: AbnormalSpeedLock::Ratio2, + zero_duty_threshold: ZeroDutyThreshold::Percent1, + } + } +} + +#[derive(Debug, Copy, Clone)] +pub enum AbnormalSpeedLockThreshold { + Hertz250 = 0x0, + Hertz500 = 0x1, + Hertz750 = 0x2, + Hertz1000 = 0x3, + Hertz1250 = 0x4, + Hertz1500 = 0x5, + Hertz1750 = 0x6, + Hertz2000 = 0x7, + Hertz2250 = 0x8, + Hertz2500 = 0x9, + Hertz2750 = 0xA, + Hertz3000 = 0xB, + Hertz3250 = 0xC, + Hertz3500 = 0xD, + Hertz3750 = 0xE, + Hertz4000 = 0xF, +} + +#[derive(Debug, Copy, Clone)] +pub enum LossSyncTimes { + Count2 = 0x0, + Count3 = 0x1, + Count4 = 0x2, + Count5 = 0x3, + Count6 = 0x4, + Count7 = 0x5, + Count8 = 0x6, + Count9 = 0x7, +} + +#[derive(Debug, Copy, Clone)] +pub enum NoMotorThreshold { + Volts0_005 = 0x0, + Volts0_0075 = 0x1, + Volts0_010 = 0x2, + Volts0_0125 = 0x3, + Volts0_020 = 0x4, + Volts0_025 = 0x5, + Volts0_030 = 0x6, + Volts0_04 = 0x7, +} + +#[derive(Debug, Copy, Clone)] +pub enum MotorVoltageMode { + Latch = 0x0, + AutomaticClear = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum MaxMotorVoltage { + NoLimit = 0x0, + Volts20 = 0x1, + Volts25 = 0x2, + Volts30 = 0x3, + Volts35 = 0x4, + Volts40 = 0x5, + Volts50 = 0x6, + Volts60 = 0x7, +} + +#[derive(Debug, Copy, Clone)] +pub enum MinMotorVoltage { + NoLimit = 0x0, + Volts6 = 0x1, + Volts7 = 0x2, + Volts8 = 0x3, + Volts9 = 0x4, + Volts10 = 0x5, + Volts12 = 0x6, + Volts15 = 0x7, +} + +#[derive(Debug, Copy, Clone)] +pub enum AutomaticRetries { + NoLimit = 0x0, + Count2 = 0x1, + Count3 = 0x2, + Count5 = 0x3, + Count7 = 0x4, + Count10 = 0x5, + Count15 = 0x6, + Count20 = 0x7, +} + +#[derive(Debug, Copy, Clone)] +pub enum LockMinSpeed { + Hertz0_5 = 0x0, + Hertz1 = 0x1, + Hertz2 = 0x2, + Hertz3 = 0x3, + Hertz5 = 0x4, + Hertz10 = 0x5, + Hertz15 = 0x6, + Hertz20 = 0x7, +} + +#[derive(Debug, Copy, Clone)] +pub enum AbnormalSpeedLock { + Ratio2 = 0x0, + Ratio4 = 0x1, + Ratio6 = 0x2, + Ratio8 = 0x3, +} + +#[derive(Debug, Copy, Clone)] +pub enum ZeroDutyThreshold { + Percent1 = 0x0, + Percent1_5 = 0x1, + Percent2 = 0x2, + Percent2_5 = 0x3, +} diff --git a/flight/src/hardware/mct8316a/gate_driver_config.rs b/flight/src/hardware/mct8316a/gate_driver_config.rs new file mode 100644 index 0000000..0eee023 --- /dev/null +++ b/flight/src/hardware/mct8316a/gate_driver_config.rs @@ -0,0 +1,159 @@ +#![allow(dead_code)] + +#[derive(Debug, Clone)] +pub struct GateDriverConfig1 { + pub slew_rate: SlewRate, + pub overvoltage_protection_level: OvervoltageProtectionLevel, + pub overvoltage_protection_enable: bool, + pub overtemperature_warning_enable: bool, + pub overcurrent_protection_deglitch_time: OvercurrentProtectionDeglitchTime, + pub overcurrent_protection_retry_time: OvercurrentProtectionRetryTime, + pub overcurrent_protection_level: OvercurrentProtectionLevel, + pub overcurrent_fault_mode: OvercurrentFaultMode, + pub low_demag_comparator_threshold: DemagComparatorThreshold, + pub high_demag_comparator_threshold: DemagComparatorThreshold, + pub synchronous_rectification_enable: bool, + pub asynchronous_rectification_enable: bool, + pub current_sense_amplifier: CurrentSenseAmplifier, +} + +impl Default for GateDriverConfig1 { + fn default() -> Self { + Self { + slew_rate: SlewRate::VoltsPerMicro25, + overvoltage_protection_level: OvervoltageProtectionLevel::Volts32, + overvoltage_protection_enable: false, + overtemperature_warning_enable: false, + overcurrent_protection_deglitch_time: OvercurrentProtectionDeglitchTime::Microsecond0_2, + overcurrent_protection_retry_time: OvercurrentProtectionRetryTime::Millisecond5, + overcurrent_protection_level: OvercurrentProtectionLevel::Amps16, + overcurrent_fault_mode: OvercurrentFaultMode::Latch, + low_demag_comparator_threshold: DemagComparatorThreshold::Milliamps100, + high_demag_comparator_threshold: DemagComparatorThreshold::Milliamps100, + synchronous_rectification_enable: false, + asynchronous_rectification_enable: false, + current_sense_amplifier: CurrentSenseAmplifier::VoltsPerAmp0_15, + } + } +} + +#[derive(Debug, Copy, Clone)] +pub enum SlewRate { + VoltsPerMicro25 = 0x0, + VoltsPerMicro50 = 0x1, + VoltsPerMicro125 = 0x2, + VoltsPerMicro200 = 0x3, +} + +#[derive(Debug, Copy, Clone)] +pub enum OvervoltageProtectionLevel { + Volts32 = 0x0, + Volts20 = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum OvercurrentProtectionDeglitchTime { + Microsecond0_2 = 0x0, + Microsecond0_6 = 0x1, + Microsecond1_1 = 0x2, + Microsecond1_6 = 0x3, +} + +#[derive(Debug, Copy, Clone)] +pub enum OvercurrentProtectionRetryTime { + Millisecond5 = 0x0, + Millisecond500 = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum OvercurrentProtectionLevel { + Amps16 = 0x0, + Amps24 = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum OvercurrentFaultMode { + Latch = 0x0, + AutomaticRetry = 0x1, + ReportOnly = 0x2, + None = 0x3, +} + +#[derive(Debug, Copy, Clone)] +pub enum DemagComparatorThreshold { + Milliamps100 = 0x0, + Milliamps150 = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum CurrentSenseAmplifier { + VoltsPerAmp0_15 = 0x0, + VoltsPerAmp0_3 = 0x1, + VoltsPerAmp0_6 = 0x2, + VoltsPerAmp1_2 = 0x3, +} + +#[derive(Debug, Clone)] +pub struct GateDriverConfig2 { + pub driver_delay_compensation_enable: bool, + pub target_delay: TargetDelay, + pub buck_slew_rate: BuckSlewRate, + pub buck_power_sequencing_disable: bool, + pub buck_current_limit: BuckCurrentLimit, + pub buck_voltage_selection: BuckVoltageSelection, + pub buck_disable: bool, +} + +impl Default for GateDriverConfig2 { + fn default() -> Self { + Self { + driver_delay_compensation_enable: false, + target_delay: TargetDelay::Automatic, + buck_slew_rate: BuckSlewRate::VoltsPerMicrosecond1000, + buck_power_sequencing_disable: false, + buck_current_limit: BuckCurrentLimit::Milliamps600, + buck_voltage_selection: BuckVoltageSelection::Volts3_3, + buck_disable: false, + } + } +} + +#[derive(Debug, Copy, Clone)] +pub enum TargetDelay { + Automatic = 0x0, + Microsecond0_4 = 0x1, + Microsecond0_6 = 0x2, + Microsecond0_8 = 0x3, + Microsecond1_0 = 0x4, + Microsecond1_2 = 0x5, + Microsecond1_4 = 0x6, + Microsecond1_6 = 0x7, + Microsecond1_8 = 0x8, + Microsecond2_0 = 0x9, + Microsecond2_2 = 0xA, + Microsecond2_4 = 0xB, + Microsecond2_6 = 0xC, + Microsecond2_8 = 0xD, + Microsecond3_0 = 0xE, + Microsecond3_2 = 0xF, +} + +#[derive(Debug, Copy, Clone)] +pub enum BuckSlewRate { + VoltsPerMicrosecond1000 = 0x0, + VoltsPerMicrosecond200 = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum BuckCurrentLimit { + Milliamps600 = 0x0, + Milliamps150 = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum BuckVoltageSelection { + Volts3_3 = 0x0, + Volts5_0 = 0x1, + Volts4_0 = 0x2, + Volts5_7 = 0x3, +} diff --git a/flight/src/hardware/mct8316a/mod.rs b/flight/src/hardware/mct8316a/mod.rs index 7db6975..f3faf5d 100644 --- a/flight/src/hardware/mct8316a/mod.rs +++ b/flight/src/hardware/mct8316a/mod.rs @@ -7,6 +7,10 @@ mod constant_speed; mod constant_power; mod phase_profile; mod trap_config; +mod fault_config; +mod pin_config; +mod device_config; +mod gate_driver_config; use anyhow::Result; diff --git a/flight/src/hardware/mct8316a/motor_startup.rs b/flight/src/hardware/mct8316a/motor_startup.rs index 6283f21..6a646a6 100644 --- a/flight/src/hardware/mct8316a/motor_startup.rs +++ b/flight/src/hardware/mct8316a/motor_startup.rs @@ -179,7 +179,7 @@ pub struct MotorStartup2 { pub open_loop_acceleration1: OpenLoopAcceleration1, pub open_loop_acceleration2: OpenLoopAcceleration2, pub open_closed_handoff_threshold: OpenClosedHandoffThreshold, - pub auto_handoff: EnableDisable, + pub auto_handoff_enable: bool, pub first_cycle_frequency_select: FirstCycleFrequencySelect, pub minimum_duty_cycle: MinimumDutyCycle, } @@ -193,7 +193,7 @@ impl Default for MotorStartup2 { open_loop_acceleration1: OpenLoopAcceleration1::HertzPerSecond0_005, open_loop_acceleration2: OpenLoopAcceleration2::HertzPerSecondSecond0_005, open_closed_handoff_threshold: OpenClosedHandoffThreshold::Hertz1, - auto_handoff: EnableDisable::Disable, + auto_handoff_enable: false, first_cycle_frequency_select: FirstCycleFrequencySelect::SlowFirstCycle, minimum_duty_cycle: MinimumDutyCycle::Percent1_5, } diff --git a/flight/src/hardware/mct8316a/pin_config.rs b/flight/src/hardware/mct8316a/pin_config.rs new file mode 100644 index 0000000..b894a3c --- /dev/null +++ b/flight/src/hardware/mct8316a/pin_config.rs @@ -0,0 +1,109 @@ +#![allow(dead_code)] + +#[derive(Debug, Clone)] +pub struct PinConfig1 { + pub dacout1_address: u16, + pub dacout2_address: u16, + pub brake_input_config: BrakeInputConfig, + pub direction_input_config: DirectionInputConfig, + pub speed_input_config: SpeedInputConfig, +} + +impl Default for PinConfig1 { + fn default() -> Self { + Self { + dacout1_address: 0, + dacout2_address: 0, + brake_input_config: BrakeInputConfig::HardwarePinBrake, + direction_input_config: DirectionInputConfig::HardwarePinDirection, + speed_input_config: SpeedInputConfig::Analog, + } + } +} + +#[derive(Debug, Copy, Clone)] +pub enum BrakeInputConfig { + HardwarePinBrake = 0x0, + BrakeActive = 0x1, + BrakeDisabled = 0x2, +} + +#[derive(Debug, Copy, Clone)] +pub enum DirectionInputConfig { + HardwarePinDirection = 0x0, + Clockwise = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum SpeedInputConfig { + Analog = 0x0, + Pwm = 0x1, + I2c = 0x2, + Frequency = 0x3, +} + +#[derive(Debug, Clone)] +pub struct PinConfig2 { + pub pin36config: Pin36Config, + pub pin37_38config: Pin37_38Config, + pub sleep_time: SleepTime, + pub enable_external_watchdog: bool, + pub external_watchdog_source: ExternalWatchdogSource, + pub external_watchdog_fault_mode: ExternalWatchdogFaultMode, + pub external_watchdog_frequency: ExternalWatchdogFrequency, +} + +impl Default for PinConfig2 { + fn default() -> Self { + Self { + pin36config: Pin36Config::Dacout2, + pin37_38config: Pin37_38Config::Pin37Dacout1Pin38Dacout2, + sleep_time: SleepTime::Sleep50micros, + enable_external_watchdog: false, + external_watchdog_source: ExternalWatchdogSource::I2c, + external_watchdog_fault_mode: ExternalWatchdogFaultMode::ReportOnly, + external_watchdog_frequency: ExternalWatchdogFrequency::Hertz10, + } + } +} + +#[derive(Debug, Copy, Clone)] +pub enum Pin36Config { + Dacout2 = 0x0, + SoA = 0x1, + SoB = 0x2, + SoC = 0x3, +} + +#[derive(Debug, Copy, Clone)] +pub enum Pin37_38Config { + Pin37Dacout1Pin38Dacout2 = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum SleepTime { + Sleep50micros = 0x0, + Sleep200micros = 0x1, + Sleep50millis = 0x2, + Sleep200millis = 0x3, +} + +#[derive(Debug, Copy, Clone)] +pub enum ExternalWatchdogSource { + I2c = 0x0, + Gpio = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum ExternalWatchdogFaultMode { + ReportOnly = 0x0, + LatchHighImpedance = 0x1, +} + +#[derive(Debug, Copy, Clone)] +pub enum ExternalWatchdogFrequency { + Hertz10 = 0x0, + Hertz50 = 0x1, + Hertz2 = 0x2, + Hertz1 = 0x3, +} diff --git a/flight/src/hardware/mct8316a/trap_config.rs b/flight/src/hardware/mct8316a/trap_config.rs index 8265e59..278bd61 100644 --- a/flight/src/hardware/mct8316a/trap_config.rs +++ b/flight/src/hardware/mct8316a/trap_config.rs @@ -2,8 +2,6 @@ use crate::hardware::mct8316a::motor_startup::DutyCycle; -#[allow(dead_code)] - #[derive(Debug, Clone)] pub struct TrapConfig1 { pub open_loop_handoff_cycles: OpenLoopHandoffCycles, diff --git a/flight/src/hardware/mod.rs b/flight/src/hardware/mod.rs index 25776cb..cf2f153 100644 --- a/flight/src/hardware/mod.rs +++ b/flight/src/hardware/mod.rs @@ -1,7 +1,7 @@ use crate::hardware::mcp23017::Mcp23017; +use crate::hardware::mct8316a::Mct8316a; use anyhow::Result; use embedded_hal::pwm::SetDutyCycle; -use crate::hardware::mct8316a::Mct8316a; pub trait Hardware { type Pwm: SetDutyCycle + Sync; diff --git a/flight/src/hardware/raspi/mod.rs b/flight/src/hardware/raspi/mod.rs index a5a7142..391962d 100644 --- a/flight/src/hardware/raspi/mod.rs +++ b/flight/src/hardware/raspi/mod.rs @@ -2,19 +2,19 @@ mod pwm; use crate::hardware::mcp23017::{Mcp23017, Mcp23017Driver}; use crate::hardware::mcp3208::Mcp3208; +use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a}; +use crate::hardware::raspi::pwm::PwmWrapper; 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::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::pwm::Pwm; -use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a}; -use crate::hardware::raspi::pwm::PwmWrapper; const CLOCK_1MHZ: u32 = 1_000_000; diff --git a/flight/src/hardware/raspi/pwm.rs b/flight/src/hardware/raspi/pwm.rs index 8dcb4e7..797cd20 100644 --- a/flight/src/hardware/raspi/pwm.rs +++ b/flight/src/hardware/raspi/pwm.rs @@ -1,20 +1,21 @@ -use std::fmt::{Display, Formatter}; -use std::time::Duration; use embedded_hal::pwm::{ErrorKind, ErrorType, SetDutyCycle}; use log::trace; use rpi_pal::pwm::Pwm; +use std::fmt::{Display, Formatter}; +use std::time::Duration; const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz pub struct PwmWrapper { - pwm: Pwm + pwm: Pwm, } impl PwmWrapper { - pub fn new(pwm: Pwm) -> anyhow::Result { + pub fn new(mut pwm: Pwm) -> anyhow::Result { trace!("PwmWrapper::new(pwm: {pwm:?})"); pwm.set_period(PWM_PERIOD)?; pwm.enable()?; + pwm.set_reset_on_drop(true); Ok(Self { pwm }) diff --git a/flight/src/lib.rs b/flight/src/lib.rs index 2ead01d..e9a6e47 100644 --- a/flight/src/lib.rs +++ b/flight/src/lib.rs @@ -2,26 +2,38 @@ use crate::hardware::channelization::{MCP23017_A_LED, MCP23017_B_LED}; use crate::hardware::initialize; use crate::hardware::mcp23017::Mcp23017OutputPin; use crate::hardware::mcp23017::{Mcp23017, Mcp23017Task}; +use crate::hardware::mct8316a::Mct8316a; use crate::hardware::Hardware; use crate::on_drop::on_drop; use anyhow::Result; use embedded_hal::digital::PinState; +use embedded_hal::pwm::SetDutyCycle; use log::info; use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::Arc; use std::thread; use std::thread::sleep; use std::time::Duration; -use crate::hardware::mct8316a::Mct8316a; -use embedded_hal::pwm::SetDutyCycle; mod hardware; +fn add_ctrlc_handler(flag: Arc) -> Result<()> { + ctrlc::set_handler(move || { + info!("Shutdown Requested"); + flag.store(false, Ordering::Relaxed); + })?; + Ok(()) +} + pub fn run() -> Result<()> { info!( "Project Nautilus Flight Software {}", env!("CARGO_PKG_VERSION") ); + let running = Arc::new(AtomicBool::new(true)); + add_ctrlc_handler(running.clone())?; + let hal = initialize()?; let mut mcp23017_a = hal.new_mcp23017_a()?; @@ -37,8 +49,6 @@ pub fn run() -> Result<()> { mcp23017_b.init()?; mct8316.init()?; - let running = AtomicBool::new(true); - thread::scope(|scope| { // This will automatically set running to false when it drops // This means that if the main thread exits this scope, we will @@ -57,10 +67,23 @@ pub fn run() -> Result<()> { led_pin_b.set_state_on_drop(PinState::Low); - led_pin_a.set_state(PinState::High); - sleep(Duration::from_secs(1)); - led_pin_b.set_state(PinState::High); - sleep(Duration::from_secs(1)); + loop { + led_pin_a.set_state(PinState::High); + sleep(Duration::from_secs(1)); + if !running.load(Ordering::Relaxed) { break; }; + + led_pin_b.set_state(PinState::High); + sleep(Duration::from_secs(1)); + if !running.load(Ordering::Relaxed) { break; }; + + led_pin_a.set_state(PinState::Low); + sleep(Duration::from_secs(1)); + if !running.load(Ordering::Relaxed) { break; }; + + led_pin_b.set_state(PinState::Low); + sleep(Duration::from_secs(1)); + if !running.load(Ordering::Relaxed) { break; }; + } anyhow::Ok(()) })?; diff --git a/flight/src/logger.rs b/flight/src/logger.rs index 7276595..551277b 100644 --- a/flight/src/logger.rs +++ b/flight/src/logger.rs @@ -1,9 +1,9 @@ use anyhow::Result; -use log::{debug, LevelFilter}; +use fern::colors::{Color, ColoredLevelConfig}; +use log::debug; 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(|_| { @@ -34,9 +34,9 @@ pub fn setup_logger() -> Result<()> { out.finish(format_args!( "[{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(), + level = colors.color(record.level()), + time = chrono::Local::now().format("%Y-%m-%dT%H:%M:%S%.9f"), + target = record.target(), )) }) .chain( diff --git a/flight/src/main.rs b/flight/src/main.rs index e736b2b..fba2f42 100644 --- a/flight/src/main.rs +++ b/flight/src/main.rs @@ -1,6 +1,6 @@ +use crate::logger::setup_logger; use log::error; use nautilus_flight::run; -use crate::logger::setup_logger; mod logger;