From d552fe3627b6e816cfc75f89e3b521b9501d8789 Mon Sep 17 00:00:00 2001 From: Sergey Savelyev Date: Sat, 18 Oct 2025 16:32:22 -0700 Subject: [PATCH] initial work on mct8316a --- Cargo.lock | 180 ++++++- config.txt | 60 +++ flight/Cargo.toml | 11 +- flight/src/hardware/mcp23017/driver.rs | 2 +- flight/src/hardware/mct8316a/closed_loop.rs | 485 ++++++++++++++++++ .../src/hardware/mct8316a/constant_power.rs | 40 ++ .../src/hardware/mct8316a/constant_speed.rs | 31 ++ flight/src/hardware/mct8316a/driver.rs | 285 ++++++++++ flight/src/hardware/mct8316a/eeprom.rs | 357 +++++++++++++ flight/src/hardware/mct8316a/isd_config.rs | 108 ++++ flight/src/hardware/mct8316a/mod.rs | 17 + flight/src/hardware/mct8316a/motor_startup.rs | 363 +++++++++++++ flight/src/hardware/mct8316a/phase_profile.rs | 49 ++ flight/src/hardware/mct8316a/trap_config.rs | 140 +++++ flight/src/hardware/mod.rs | 9 + flight/src/hardware/raspi/mod.rs | 31 +- flight/src/hardware/raspi/pwm.rs | 55 ++ flight/src/lib.rs | 12 +- flight/src/logger.rs | 22 +- flight/src/main.rs | 6 +- 20 files changed, 2221 insertions(+), 42 deletions(-) create mode 100644 config.txt create mode 100644 flight/src/hardware/mct8316a/closed_loop.rs create mode 100644 flight/src/hardware/mct8316a/constant_power.rs create mode 100644 flight/src/hardware/mct8316a/constant_speed.rs create mode 100644 flight/src/hardware/mct8316a/driver.rs create mode 100644 flight/src/hardware/mct8316a/eeprom.rs create mode 100644 flight/src/hardware/mct8316a/isd_config.rs create mode 100644 flight/src/hardware/mct8316a/mod.rs create mode 100644 flight/src/hardware/mct8316a/motor_startup.rs create mode 100644 flight/src/hardware/mct8316a/phase_profile.rs create mode 100644 flight/src/hardware/mct8316a/trap_config.rs create mode 100644 flight/src/hardware/raspi/pwm.rs diff --git a/Cargo.lock b/Cargo.lock index ca7d15c..b49651f 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2,12 +2,6 @@ # It is not intended for manual editing. version = 4 -[[package]] -name = "android-tzdata" -version = "0.1.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e999941b234f3131b00bc13c22d06e8c5ff726d1b6318ac7eb276997bbb4fef0" - [[package]] name = "android_system_properties" version = "0.1.5" @@ -67,11 +61,10 @@ checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" [[package]] name = "chrono" -version = "0.4.40" +version = "0.4.42" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1a7964611d71df112cb1730f2ee67324fcf4d0fc6606acbbe9bfe06df124637c" +checksum = "145052bdd345b87320e369255277e3fb5152762ad123a901ef5c262dd38fe8d2" dependencies = [ - "android-tzdata", "iana-time-zone", "js-sys", "num-traits", @@ -79,12 +72,37 @@ dependencies = [ "windows-link", ] +[[package]] +name = "colored" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "117725a109d387c937a1533ce01b450cbde6b88abceea8473c4d7a85853cda3c" +dependencies = [ + "lazy_static", + "windows-sys", +] + [[package]] name = "core-foundation-sys" version = "0.8.7" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "773648b94d0e5d620f64f280777445740e61fe701025087ec8b57f45c791888b" +[[package]] +name = "crc" +version = "3.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9710d3b3739c2e349eb44fe848ad0b7c8cb1e42bd87ee49371df2f7acaf3e675" +dependencies = [ + "crc-catalog", +] + +[[package]] +name = "crc-catalog" +version = "2.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "19d374276b40fb8bbdee95aef7c7fa6b5316ec764510eb64b8dd0e2ed0d7e7f5" + [[package]] name = "critical-section" version = "1.2.0" @@ -156,9 +174,106 @@ version = "0.7.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4316185f709b23713e41e3195f90edef7fb00c3ed4adc79769cf09cc762a3b29" dependencies = [ + "colored", "log", ] +[[package]] +name = "glam" +version = "0.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "333928d5eb103c5d4050533cec0384302db6be8ef7d3cebd30ec6a35350353da" + +[[package]] +name = "glam" +version = "0.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3abb554f8ee44336b72d522e0a7fe86a29e09f839a36022fa869a7dfe941a54b" + +[[package]] +name = "glam" +version = "0.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4126c0479ccf7e8664c36a2d719f5f2c140fbb4f9090008098d2c291fa5b3f16" + +[[package]] +name = "glam" +version = "0.17.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e01732b97afd8508eee3333a541b9f7610f454bb818669e66e90f5f57c93a776" + +[[package]] +name = "glam" +version = "0.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "525a3e490ba77b8e326fb67d4b44b4bd2f920f44d4cc73ccec50adc68e3bee34" + +[[package]] +name = "glam" +version = "0.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b8509e6791516e81c1a630d0bd7fbac36d2fa8712a9da8662e716b52d5051ca" + +[[package]] +name = "glam" +version = "0.20.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f43e957e744be03f5801a55472f593d43fabdebf25a4585db250f04d86b1675f" + +[[package]] +name = "glam" +version = "0.21.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "518faa5064866338b013ff9b2350dc318e14cc4fcd6cb8206d7e7c9886c98815" + +[[package]] +name = "glam" +version = "0.22.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "12f597d56c1bd55a811a1be189459e8fad2bbc272616375602443bdfb37fa774" + +[[package]] +name = "glam" +version = "0.23.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e4afd9ad95555081e109fe1d21f2a30c691b5f0919c67dfa690a2e1eb6bd51c" + +[[package]] +name = "glam" +version = "0.24.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b5418c17512bdf42730f9032c74e1ae39afc408745ebb2acf72fbc4691c17945" + +[[package]] +name = "glam" +version = "0.25.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "151665d9be52f9bb40fc7966565d39666f2d1e69233571b71b87791c7e0528b3" + +[[package]] +name = "glam" +version = "0.27.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e05e7e6723e3455f4818c7b26e855439f7546cf617ef669d1adedb8669e5cb9" + +[[package]] +name = "glam" +version = "0.28.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "779ae4bf7e8421cf91c0b3b64e7e8b40b862fba4d393f59150042de7c4965a94" + +[[package]] +name = "glam" +version = "0.29.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8babf46d4c1c9d92deac9f7be466f76dfc4482b6452fc5024b5e8daf6ffeb3ee" + +[[package]] +name = "glam" +version = "0.30.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e12d847aeb25f41be4c0ec9587d624e9cd631bc007a8fd7ce3f5851e064c6460" + [[package]] name = "hex" version = "0.4.3" @@ -199,6 +314,12 @@ dependencies = [ "wasm-bindgen", ] +[[package]] +name = "lazy_static" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" + [[package]] name = "libc" version = "0.2.171" @@ -207,9 +328,9 @@ checksum = "c19937216e9d3aa9956d9bb8dfc0b0c8beb6058fc4f7a4dc4d850edf86a237d6" [[package]] name = "log" -version = "0.4.27" +version = "0.4.28" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94" +checksum = "34080505efa8e45a4b816c349525ebe327ceaa8559756f0356cba97ef3bf7432" [[package]] name = "matrixmultiply" @@ -223,11 +344,27 @@ dependencies = [ [[package]] name = "nalgebra" -version = "0.33.2" +version = "0.34.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "26aecdf64b707efd1310e3544d709c5c0ac61c13756046aaaba41be5c4f66a3b" +checksum = "c4d5b3eff5cd580f93da45e64715e8c20a3996342f1e466599cf7a267a0c2f5f" dependencies = [ "approx", + "glam 0.14.0", + "glam 0.15.2", + "glam 0.16.0", + "glam 0.17.3", + "glam 0.18.0", + "glam 0.19.0", + "glam 0.20.5", + "glam 0.21.3", + "glam 0.22.0", + "glam 0.23.0", + "glam 0.24.2", + "glam 0.25.0", + "glam 0.27.0", + "glam 0.28.0", + "glam 0.29.3", + "glam 0.30.8", "matrixmultiply", "nalgebra-macros", "num-complex 0.4.6", @@ -239,9 +376,9 @@ dependencies = [ [[package]] name = "nalgebra-macros" -version = "0.2.2" +version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "254a5372af8fc138e36684761d3c0cdb758a4410e938babcff1c860ce14ddbfc" +checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889" dependencies = [ "proc-macro2", "quote", @@ -258,6 +395,7 @@ version = "0.0.1" dependencies = [ "anyhow", "chrono", + "crc", "embedded-hal 1.0.0", "embedded-hal-bus", "embedded-hal-mock", @@ -488,18 +626,18 @@ dependencies = [ [[package]] name = "thiserror" -version = "2.0.12" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "567b8a2dae586314f7be2a752ec7474332959c6460e02bde30d702a66d488708" +checksum = "3467d614147380f2e4e374161426ff399c91084acd2363eaf549172b3d5e60c0" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "2.0.12" +version = "2.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7f7cf42b4507d8ea322120659672cf1b9dbb93f8f2d4ecfd6e51350ff5b17a1d" +checksum = "6c5e1be1c48b9172ee610da68fd9cd2770e7a4056cb3fc98710ee6906f0c7960" dependencies = [ "proc-macro2", "quote", @@ -603,9 +741,9 @@ dependencies = [ [[package]] name = "windows-link" -version = "0.1.1" +version = "0.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "76840935b766e1b0a05c0066835fb9ec80071d4c09a16f6bd5f7e655e3c14c38" +checksum = "45e46c0661abb7180e7b9c281db115305d49ca1709ab8242adf09666d2173c65" [[package]] name = "windows-sys" diff --git a/config.txt b/config.txt new file mode 100644 index 0000000..34797f7 --- /dev/null +++ b/config.txt @@ -0,0 +1,60 @@ +# For more options and information see +# http://rptl.io/configtxt +# Some settings may impact device functionality. See link above for details + + +# Enable SPI1 with 3 Chip Select Pins +dtparam=spi=on +dtoverlay=spi1-3cs + +# Enable I2C0 +dtparam=i2c0=on + + + + +# Enable audio (loads snd_bcm2835) +dtparam=audio=off + +# Additional overlays and parameters are documented +# /boot/firmware/overlays/README + +# Automatically load overlays for detected cameras +camera_auto_detect=1 + +# Automatically load overlays for detected DSI displays +display_auto_detect=1 + +# Automatically load initramfs files, if found +auto_initramfs=1 + +# Enable DRM VC4 V3D driver +dtoverlay=vc4-kms-v3d +max_framebuffers=2 + +# Don't have the firmware create an initial video= setting in cmdline.txt. +# Use the kernel's default instead. +disable_fw_kms_setup=1 + +# Run in 64-bit mode +arm_64bit=1 + +# Disable compensation for displays with overscan +disable_overscan=1 + +# Run as fast as firmware / board allows +arm_boost=1 + +[cm4] +# Enable host mode on the 2711 built-in XHCI USB controller. +# This line should be removed if the legacy DWC2 controller is required +# (e.g. for USB device mode) or if USB support is not required. +otg_mode=1 + +[cm5] +dtoverlay=dwc2,dr_mode=host + +[all] + +# Enable PWM0 +dtoverlay=pwm-2chan,pin=12,func=4,pin2=13,func2=4 diff --git a/flight/Cargo.toml b/flight/Cargo.toml index 3d62180..8f9eb34 100644 --- a/flight/Cargo.toml +++ b/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" } diff --git a/flight/src/hardware/mcp23017/driver.rs b/flight/src/hardware/mcp23017/driver.rs index 2f563ce..7aea830 100644 --- a/flight/src/hardware/mcp23017/driver.rs +++ b/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, diff --git a/flight/src/hardware/mct8316a/closed_loop.rs b/flight/src/hardware/mct8316a/closed_loop.rs new file mode 100644 index 0000000..96530b4 --- /dev/null +++ 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, +} + + diff --git a/flight/src/hardware/mct8316a/constant_power.rs b/flight/src/hardware/mct8316a/constant_power.rs new file mode 100644 index 0000000..f4bc4fa --- /dev/null +++ b/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, +} + diff --git a/flight/src/hardware/mct8316a/constant_speed.rs b/flight/src/hardware/mct8316a/constant_speed.rs new file mode 100644 index 0000000..fd5e2b3 --- /dev/null +++ b/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, +} diff --git a/flight/src/hardware/mct8316a/driver.rs b/flight/src/hardware/mct8316a/driver.rs new file mode 100644 index 0000000..06ada93 --- /dev/null +++ b/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 = crc::Crc::::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 +where + I2C: I2c + Send + Sync, + I2C::Error: Send, + I2C::Error: Sync, + I2C::Error: 'static, +{ + i2c: Mutex, + address: u8, +} + +impl Debug for Mct8316AVDriver +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 Mct8316AVDriver +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 Mct8316a for Mct8316AVDriver +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(()) + } +} diff --git a/flight/src/hardware/mct8316a/eeprom.rs b/flight/src/hardware/mct8316a/eeprom.rs new file mode 100644 index 0000000..6252f66 --- /dev/null +++ b/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, + 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) -> Result { + trace!("Mct8316AVEeprom::load()"); + + // driver.write(0x0000E6, Mct8316AVData::Four(0x40000000))?; + + Ok(Self { + driver, + modified: false, + }) + } + + pub fn set_isd_config(self, isd_config: IsdConfig) -> Result { + 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 { + 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 { + 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 { + 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 { + 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 { + 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 { + 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 { + 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 { + 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 { + 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 { + 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 { + 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 { + 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 { + 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(()) + } +} \ No newline at end of file diff --git a/flight/src/hardware/mct8316a/isd_config.rs b/flight/src/hardware/mct8316a/isd_config.rs new file mode 100644 index 0000000..956b847 --- /dev/null +++ b/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, +} diff --git a/flight/src/hardware/mct8316a/mod.rs b/flight/src/hardware/mct8316a/mod.rs new file mode 100644 index 0000000..7db6975 --- /dev/null +++ b/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; diff --git a/flight/src/hardware/mct8316a/motor_startup.rs b/flight/src/hardware/mct8316a/motor_startup.rs new file mode 100644 index 0000000..6283f21 --- /dev/null +++ b/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, +} + + + diff --git a/flight/src/hardware/mct8316a/phase_profile.rs b/flight/src/hardware/mct8316a/phase_profile.rs new file mode 100644 index 0000000..871c15d --- /dev/null +++ b/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, +} diff --git a/flight/src/hardware/mct8316a/trap_config.rs b/flight/src/hardware/mct8316a/trap_config.rs new file mode 100644 index 0000000..8265e59 --- /dev/null +++ b/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, + } + } +} + diff --git a/flight/src/hardware/mod.rs b/flight/src/hardware/mod.rs index 92f8516..25776cb 100644 --- a/flight/src/hardware/mod.rs +++ b/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 + Sync; + fn new_mcp23017_a(&self) -> Result; fn new_mcp23017_b(&self) -> Result; + fn new_pwm0(&self) -> Result; + + fn new_mct8316a(&self) -> Result; + fn get_battery_voltage(&self) -> Result; } @@ -33,3 +41,4 @@ mod error; pub mod mcp23017; mod mcp3208; pub mod channelization; +pub(crate) mod mct8316a; diff --git a/flight/src/hardware/raspi/mod.rs b/flight/src/hardware/raspi/mod.rs index bdbc37e..a5a7142 100644 --- a/flight/src/hardware/raspi/mod.rs +++ b/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, mcp3208: RefCell>, } @@ -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 { + trace!("RaspiHardware::new_mcp23017_a()"); Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100000)) } fn new_mcp23017_b(&self) -> Result { + trace!("RaspiHardware::new_mcp23017_b()"); Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100001)) } + fn new_pwm0(&self) -> Result { + 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 { + trace!("RaspiHardware::new_mct8316a()"); + Ok(Mct8316AVDriver::new(MutexDevice::new(&self.i2c_bus), 0b0000000)) + } + + fn get_battery_voltage(&self) -> Result { + trace!("RaspiHardware::get_battery_voltage()"); self.mcp3208.borrow_mut().read_single(1) } } diff --git a/flight/src/hardware/raspi/pwm.rs b/flight/src/hardware/raspi/pwm.rs new file mode 100644 index 0000000..8dcb4e7 --- /dev/null +++ b/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 { + 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) + } +} diff --git a/flight/src/lib.rs b/flight/src/lib.rs index 0c4f2c2..2ead01d 100644 --- a/flight/src/lib.rs +++ b/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); diff --git a/flight/src/logger.rs b/flight/src/logger.rs index 419f54c..7276595 100644 --- a/flight/src/logger.rs +++ b/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(""); 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( diff --git a/flight/src/main.rs b/flight/src/main.rs index 093d893..e736b2b 100644 --- a/flight/src/main.rs +++ b/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()); } } }