initial work on mct8316a
This commit is contained in:
180
Cargo.lock
generated
180
Cargo.lock
generated
@@ -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"
|
||||
|
||||
60
config.txt
Normal file
60
config.txt
Normal file
@@ -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
|
||||
@@ -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" }
|
||||
|
||||
@@ -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,
|
||||
|
||||
485
flight/src/hardware/mct8316a/closed_loop.rs
Normal file
485
flight/src/hardware/mct8316a/closed_loop.rs
Normal file
@@ -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,
|
||||
}
|
||||
|
||||
|
||||
40
flight/src/hardware/mct8316a/constant_power.rs
Normal file
40
flight/src/hardware/mct8316a/constant_power.rs
Normal file
@@ -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,
|
||||
}
|
||||
|
||||
31
flight/src/hardware/mct8316a/constant_speed.rs
Normal file
31
flight/src/hardware/mct8316a/constant_speed.rs
Normal file
@@ -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,
|
||||
}
|
||||
285
flight/src/hardware/mct8316a/driver.rs
Normal file
285
flight/src/hardware/mct8316a/driver.rs
Normal file
@@ -0,0 +1,285 @@
|
||||
use std::fmt::{Debug, Display, Formatter};
|
||||
use std::sync::Mutex;
|
||||
use embedded_hal::i2c::{I2c, Operation};
|
||||
use log::trace;
|
||||
use crate::hardware::mct8316a::Mct8316a;
|
||||
use anyhow::{bail, ensure, Result};
|
||||
use crate::hardware::error::I2cError;
|
||||
use crate::hardware::mct8316a::closed_loop::{ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4};
|
||||
use crate::hardware::mct8316a::constant_power::ConstantPower;
|
||||
use crate::hardware::mct8316a::constant_speed::ConstantSpeed;
|
||||
use crate::hardware::mct8316a::eeprom::Mct8316AVEeprom;
|
||||
use crate::hardware::mct8316a::isd_config::{BrakeConfig, BrakeMode, IsdConfig, IsdConfigTimeValue, ResyncMinimumThreshold, StartupBreakTime, Threshold};
|
||||
use crate::hardware::mct8316a::motor_startup::{AlignRampRate, AlignTime, FullCurrentThreshold, IpdAdvanceAngle, IpdClockFrequency, IpdCurrentThreshold, IpdReleaseMode, IpdRepeat, MotorStartup1, MotorStartup2, MotorStartupMethod, SlowFirstCycleFrequency};
|
||||
use crate::hardware::mct8316a::phase_profile::{ThreePhase150DegreeProfile, TwoPhase150DegreeProfile};
|
||||
use crate::hardware::mct8316a::trap_config::{TrapConfig1, TrapConfig2};
|
||||
|
||||
const CRC: crc::Crc<u8> = crc::Crc::<u8>::new(&crc::CRC_8_SMBUS);
|
||||
|
||||
#[allow(dead_code)]
|
||||
#[derive(Copy, Clone, Eq, PartialEq, Debug)]
|
||||
pub(super) enum Mct8316AVData {
|
||||
Two(u16),
|
||||
Four(u32),
|
||||
Eight(u64),
|
||||
}
|
||||
|
||||
impl Display for Mct8316AVData {
|
||||
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
|
||||
match self {
|
||||
Mct8316AVData::Two(val) => write!(f, "{val:04x}"),
|
||||
Mct8316AVData::Four(val) => write!(f, "{val:08x}"),
|
||||
Mct8316AVData::Eight(val) => write!(f, "{val:016x}"),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
enum OperationRW {
|
||||
Read,
|
||||
Write,
|
||||
}
|
||||
|
||||
fn control_word(
|
||||
operation_rw: OperationRW,
|
||||
crc: bool,
|
||||
data: Mct8316AVData,
|
||||
address: u32,
|
||||
) -> [u8; 3] {
|
||||
let mut control_word = [0u8; _];
|
||||
|
||||
control_word[0] |= match operation_rw {
|
||||
OperationRW::Read => 0x80,
|
||||
OperationRW::Write => 0x00,
|
||||
};
|
||||
control_word[0] |= match crc {
|
||||
true => 0x40,
|
||||
false => 0x00,
|
||||
};
|
||||
control_word[0] |= match data {
|
||||
Mct8316AVData::Two(_) => 0x00,
|
||||
Mct8316AVData::Four(_) => 0x10,
|
||||
Mct8316AVData::Eight(_) => 0x20,
|
||||
};
|
||||
control_word[0] |= ((address >> 16) & 0x0F) as u8;
|
||||
control_word[1] |= ((address >> 8) & 0xFF) as u8;
|
||||
control_word[2] |= ((address >> 0) & 0xFF) as u8;
|
||||
|
||||
control_word
|
||||
}
|
||||
|
||||
pub struct Mct8316AVDriver<I2C>
|
||||
where
|
||||
I2C: I2c + Send + Sync,
|
||||
I2C::Error: Send,
|
||||
I2C::Error: Sync,
|
||||
I2C::Error: 'static,
|
||||
{
|
||||
i2c: Mutex<I2C>,
|
||||
address: u8,
|
||||
}
|
||||
|
||||
impl<I2C> Debug for Mct8316AVDriver<I2C>
|
||||
where
|
||||
I2C: I2c + Send + Sync,
|
||||
I2C::Error: Send,
|
||||
I2C::Error: Sync,
|
||||
I2C::Error: 'static,
|
||||
{
|
||||
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
|
||||
write!(f, "Mct8316AVDriver {{ address: {} }}", self.address)
|
||||
}
|
||||
}
|
||||
|
||||
impl<I2C> Mct8316AVDriver<I2C>
|
||||
where
|
||||
I2C: I2c + Send + Sync,
|
||||
I2C::Error: Send,
|
||||
I2C::Error: Sync,
|
||||
I2C::Error: 'static,
|
||||
{
|
||||
pub fn new(i2c: I2C, address: u8) -> Self {
|
||||
trace!("Mct8316AVDriver::new(i2c, address: 0x{address:02x})");
|
||||
Self {
|
||||
i2c: i2c.into(),
|
||||
address,
|
||||
}
|
||||
}
|
||||
|
||||
pub(super) fn write(&self, address: u32, data: Mct8316AVData) -> Result<()> {
|
||||
trace!("Mct8316AVDriver::write(self: {self:?}, address: {address:06x}, data: {data})");
|
||||
|
||||
// 1 for target byte (for crc check)
|
||||
// 3 for control word
|
||||
// 8 for data
|
||||
// 1 for crc
|
||||
let mut write_data = [0u8; 1+3+8+1];
|
||||
write_data[0] = (self.address << 1) | 0b0;
|
||||
write_data[1..4].copy_from_slice(&control_word(
|
||||
OperationRW::Write,
|
||||
true,
|
||||
data,
|
||||
address
|
||||
));
|
||||
let data_length = match data {
|
||||
Mct8316AVData::Two(val) => {
|
||||
write_data[4..6].copy_from_slice(&val.to_be_bytes());
|
||||
2
|
||||
},
|
||||
Mct8316AVData::Four(val) => {
|
||||
write_data[4..8].copy_from_slice(&val.to_be_bytes());
|
||||
4
|
||||
},
|
||||
Mct8316AVData::Eight(val) => {
|
||||
write_data[4..12].copy_from_slice(&val.to_be_bytes());
|
||||
8
|
||||
},
|
||||
};
|
||||
let crc_result = CRC.checksum(&write_data[0..(4 + data_length)]);
|
||||
write_data[4+data_length] = crc_result;
|
||||
|
||||
// match self.i2c.lock() {
|
||||
// Ok(mut lock) => lock.write(self.address, &write_data).map_err(I2cError)?,
|
||||
// Err(_) => bail!("Lock was poisoned"),
|
||||
// }
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(super) fn read(&self, address: u32, data: &mut Mct8316AVData) -> Result<()> {
|
||||
trace!("Mct8316AVDriver::read(self: {self:?}, address: {address:06x}, data: {data})");
|
||||
// 1 target id
|
||||
// 3 control word
|
||||
// 1 target id
|
||||
// 8 data bytes
|
||||
// 1 crc
|
||||
let mut read_data = [0u8; 1+3+1+8+1];
|
||||
read_data[0] = (self.address << 1) | 0b0;
|
||||
read_data[1..4].copy_from_slice(&control_word(
|
||||
OperationRW::Read,
|
||||
true,
|
||||
*data,
|
||||
address
|
||||
));
|
||||
read_data[4] = (self.address << 1) | 0b1;
|
||||
let data_length = match data {
|
||||
Mct8316AVData::Two(_) => 2,
|
||||
Mct8316AVData::Four(_) => 4,
|
||||
Mct8316AVData::Eight(_) => 8,
|
||||
};
|
||||
|
||||
let (left, right) = read_data.split_at_mut(5);
|
||||
|
||||
let mut i2c_ops = [
|
||||
Operation::Write(&left[1..4]),
|
||||
Operation::Read(&mut right[..(data_length+1)])
|
||||
];
|
||||
|
||||
// match self.i2c.lock() {
|
||||
// Ok(mut lock) => lock.transaction(self.address, &mut i2c_ops).map_err(I2cError)?,
|
||||
// Err(_) => bail!("Lock was poisoned"),
|
||||
// }
|
||||
drop(i2c_ops);
|
||||
|
||||
let expected_crc = CRC.checksum(&read_data[0..(5+data_length)]);
|
||||
|
||||
// ensure!(expected_crc == read_data[5+data_length], "CRC Mismatch");
|
||||
|
||||
match data {
|
||||
Mct8316AVData::Two(val) => {
|
||||
*val = u16::from_be_bytes([read_data[5], read_data[6]]);
|
||||
}
|
||||
Mct8316AVData::Four(val) => {
|
||||
*val = u32::from_be_bytes([read_data[5], read_data[6],
|
||||
read_data[7], read_data[8]]);
|
||||
}
|
||||
Mct8316AVData::Eight(val) => {
|
||||
*val = u64::from_be_bytes([read_data[5], read_data[6],
|
||||
read_data[7], read_data[8],
|
||||
read_data[9], read_data[10],
|
||||
read_data[11], read_data[12]]);
|
||||
}
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl<I2C> Mct8316a for Mct8316AVDriver<I2C>
|
||||
where
|
||||
I2C: I2c + Send + Sync,
|
||||
I2C::Error: Send,
|
||||
I2C::Error: Sync,
|
||||
I2C::Error: 'static,
|
||||
{
|
||||
fn init(&mut self) -> Result<()> {
|
||||
trace!("Mct8316AVDriver::init(self: {self:?})");
|
||||
|
||||
// Settings taken from Table 3-3
|
||||
// https://www.ti.com/lit/ug/sllu336a/sllu336a.pdf?ts=1759001083565
|
||||
Mct8316AVEeprom::load(self)?
|
||||
.set_isd_config(IsdConfig {
|
||||
enable_isd: true,
|
||||
enable_brake: true,
|
||||
enable_high_impedance: false,
|
||||
enable_reverse_drive: true,
|
||||
enable_resynchronization: true,
|
||||
enable_stationary_brake: true,
|
||||
stationary_detect_threshold: Threshold::MilliVolt25,
|
||||
brake_mode: BrakeMode::LowOn,
|
||||
brake_config: BrakeConfig::BrakeTimeExit,
|
||||
brake_current_threshold: Threshold::MilliVolt15,
|
||||
brake_time: IsdConfigTimeValue::Millisecond500,
|
||||
high_impedence_time: IsdConfigTimeValue::Millisecond10,
|
||||
startup_break_time: StartupBreakTime::Millisecond100,
|
||||
resync_minimum_threshold: ResyncMinimumThreshold::Computed,
|
||||
})?
|
||||
.set_motor_startup1(MotorStartup1 {
|
||||
motor_startup_method: MotorStartupMethod::DoubleAlign,
|
||||
align_ramp_rate: AlignRampRate::VoltsPerSecond250,
|
||||
align_time: AlignTime::Millisecond200,
|
||||
align_current_threshold: FullCurrentThreshold::Volts0_4,
|
||||
ipd_clock_frequency: IpdClockFrequency::Hertz500,
|
||||
ipd_current_threshold: IpdCurrentThreshold::Volts0_4,
|
||||
ipd_release_mode: IpdReleaseMode::Tristate,
|
||||
ipd_advance_angle: IpdAdvanceAngle::Degrees60,
|
||||
ipd_repeat: IpdRepeat::Average2,
|
||||
slow_first_cycle_frequency: SlowFirstCycleFrequency::Hertz5,
|
||||
})?
|
||||
.set_motor_startup2(MotorStartup2 {
|
||||
..MotorStartup2::default()
|
||||
})?
|
||||
.set_closed_loop1(ClosedLoop1 {
|
||||
..ClosedLoop1::default()
|
||||
})?
|
||||
.set_closed_loop2(ClosedLoop2 {
|
||||
..ClosedLoop2::default()
|
||||
})?
|
||||
.set_closed_loop3(ClosedLoop3 {
|
||||
..ClosedLoop3::default()
|
||||
})?
|
||||
.set_closed_loop4(ClosedLoop4 {
|
||||
..ClosedLoop4::default()
|
||||
})?
|
||||
.set_constant_speed(ConstantSpeed {
|
||||
..ConstantSpeed::default()
|
||||
})?
|
||||
.set_constant_power(ConstantPower {
|
||||
..ConstantPower::default()
|
||||
})?
|
||||
.set_two_phase_profile(TwoPhase150DegreeProfile {
|
||||
..TwoPhase150DegreeProfile::default()
|
||||
})?
|
||||
.set_three_phase_profile(ThreePhase150DegreeProfile {
|
||||
..ThreePhase150DegreeProfile::default()
|
||||
})?
|
||||
.set_trap_config1(TrapConfig1 {
|
||||
..TrapConfig1::default()
|
||||
})?
|
||||
.set_trap_config2(TrapConfig2 {
|
||||
..TrapConfig2::default()
|
||||
})?
|
||||
.commit()?;
|
||||
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
357
flight/src/hardware/mct8316a/eeprom.rs
Normal file
357
flight/src/hardware/mct8316a/eeprom.rs
Normal file
@@ -0,0 +1,357 @@
|
||||
use embedded_hal::i2c::I2c;
|
||||
use anyhow::{bail, Result};
|
||||
use log::trace;
|
||||
use crate::hardware::mct8316a::closed_loop::{ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4};
|
||||
use crate::hardware::mct8316a::constant_power::ConstantPower;
|
||||
use crate::hardware::mct8316a::constant_speed::ConstantSpeed;
|
||||
use crate::hardware::mct8316a::driver::Mct8316AVData;
|
||||
use crate::hardware::mct8316a::isd_config::IsdConfig;
|
||||
use crate::hardware::mct8316a::Mct8316AVDriver;
|
||||
use crate::hardware::mct8316a::motor_startup::{MotorStartup1, MotorStartup2};
|
||||
use crate::hardware::mct8316a::phase_profile::{ThreePhase150DegreeProfile, TwoPhase150DegreeProfile};
|
||||
use crate::hardware::mct8316a::trap_config::{TrapConfig1, TrapConfig2};
|
||||
|
||||
pub struct Mct8316AVEeprom<'a, I2C>
|
||||
where
|
||||
I2C: I2c + Send + Sync,
|
||||
I2C::Error: Send,
|
||||
I2C::Error: Sync,
|
||||
I2C::Error: 'static,
|
||||
{
|
||||
driver: &'a mut Mct8316AVDriver<I2C>,
|
||||
modified: bool,
|
||||
}
|
||||
|
||||
impl<'a, I2C> Mct8316AVEeprom<'a, I2C>
|
||||
where
|
||||
I2C: I2c + Send + Sync,
|
||||
I2C::Error: Send,
|
||||
I2C::Error: Sync,
|
||||
I2C::Error: 'static,
|
||||
{
|
||||
pub fn load(driver: &'a mut Mct8316AVDriver<I2C>) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::load()");
|
||||
|
||||
// driver.write(0x0000E6, Mct8316AVData::Four(0x40000000))?;
|
||||
|
||||
Ok(Self {
|
||||
driver,
|
||||
modified: false,
|
||||
})
|
||||
}
|
||||
|
||||
pub fn set_isd_config(self, isd_config: IsdConfig) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_isd_config(isd_config: {isd_config:?})");
|
||||
|
||||
let mut expected_value =
|
||||
if isd_config.enable_isd { 0x40000000 } else { 0 }
|
||||
| if isd_config.enable_brake { 0x20000000 } else { 0 }
|
||||
| if isd_config.enable_high_impedance { 0x10000000 } else { 0 }
|
||||
| if isd_config.enable_reverse_drive { 0x08000000 } else { 0 }
|
||||
| if isd_config.enable_resynchronization { 0x04000000 } else { 0 }
|
||||
| if isd_config.enable_stationary_brake { 0x02000000 } else { 0 }
|
||||
| (((isd_config.stationary_detect_threshold as u32) & 0x7) << 22)
|
||||
| ((isd_config.brake_mode as u32) & 0x1) << 21
|
||||
| ((isd_config.brake_config as u32) & 0x1) << 20
|
||||
| ((isd_config.brake_current_threshold as u32) & 0x7) << 17
|
||||
| ((isd_config.brake_time as u32) & 0xF) << 13
|
||||
| ((isd_config.high_impedence_time as u32) & 0xF) << 9
|
||||
| ((isd_config.startup_break_time as u32) & 0x7) << 6
|
||||
| ((isd_config.resync_minimum_threshold as u32) & 0x7) << 3;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x80, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_motor_startup1(self, motor_startup1: MotorStartup1) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_motor_startup1(motor_startup1: {motor_startup1:?})");
|
||||
|
||||
let mut expected_value =
|
||||
((motor_startup1.motor_startup_method as u32) & 0x3) << 29
|
||||
| ((motor_startup1.align_ramp_rate as u32) & 0xF) << 25
|
||||
| ((motor_startup1.align_time as u32) & 0xF) << 21
|
||||
| ((motor_startup1.align_current_threshold as u32) & 0xF) << 17
|
||||
| ((motor_startup1.ipd_clock_frequency as u32) & 0x7) << 14
|
||||
| ((motor_startup1.ipd_current_threshold as u32) & 0xF) << 10
|
||||
| ((motor_startup1.ipd_release_mode as u32) & 0x3) << 8
|
||||
| ((motor_startup1.ipd_advance_angle as u32) & 0x3) << 6
|
||||
| ((motor_startup1.ipd_repeat as u32) & 0x3) << 4
|
||||
| ((motor_startup1.slow_first_cycle_frequency as u32) & 0xF) << 0;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x82, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_motor_startup2(self, motor_startup2: MotorStartup2) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_motor_startup2(motor_startup2: {motor_startup2:?})");
|
||||
|
||||
let mut expected_value =
|
||||
((motor_startup2.open_loop_current_limit_mode as u32) & 0x1) << 30
|
||||
| ((motor_startup2.open_loop_duty_cycle as u32) & 0x7) << 27
|
||||
| ((motor_startup2.open_loop_current_limit as u32) & 0xF) << 23
|
||||
| ((motor_startup2.open_loop_acceleration1 as u32) & 0x1F) << 18
|
||||
| ((motor_startup2.open_loop_acceleration2 as u32) & 0x1F) << 13
|
||||
| ((motor_startup2.open_closed_handoff_threshold as u32) & 0x1F) << 8
|
||||
| ((motor_startup2.auto_handoff as u32) & 0x1) << 7
|
||||
| ((motor_startup2.first_cycle_frequency_select as u32) & 0x1) << 6
|
||||
| ((motor_startup2.minimum_duty_cycle as u32) & 0xF) << 2;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x84, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_closed_loop1(self, closed_loop1: ClosedLoop1) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_closed_loop1(closed_loop1: {closed_loop1:?})");
|
||||
|
||||
let lead_angle = (closed_loop1.lead_angle / 0.12f32).round().clamp(0.0f32, u8::MAX as f32) as u8;
|
||||
|
||||
let mut expected_value =
|
||||
((closed_loop1.commutation_mode as u32) & 0x2) << 29
|
||||
| ((closed_loop1.closed_loop_acceleration_rate as u32) & 0x1F) << 24
|
||||
| ((closed_loop1.closed_loop_deceleration_mode as u32) & 0x1) << 23
|
||||
| ((closed_loop1.closed_loop_deceleration_mode as u32) & 0x1F) << 18
|
||||
| ((closed_loop1.pwm_frequency as u32) & 0x1F) << 13
|
||||
| ((closed_loop1.pwm_modulation as u32) & 0x3) << 11
|
||||
| ((closed_loop1.pwm_mode as u32) & 0x1) << 10
|
||||
| ((closed_loop1.lead_angle_polarity as u32) & 0x1) << 9
|
||||
| (lead_angle as u32) << 1;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x86, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_closed_loop2(self, closed_loop2: ClosedLoop2) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_closed_loop2(closed_loop2: {closed_loop2:?})");
|
||||
|
||||
let mut expected_value =
|
||||
((closed_loop2.speed_feedback_mode as u32) & 0x2) << 29
|
||||
| ((closed_loop2.speed_feedback_division as u32) & 0xF) << 25
|
||||
| ((closed_loop2.speed_feedback_config as u32) & 0x1) << 24
|
||||
| ((closed_loop2.bemf_threshold as u32) & 0x7) << 21
|
||||
| ((closed_loop2.motor_stop_mode as u32) & 0x7) << 18
|
||||
| ((closed_loop2.motor_stop_brake_time as u32) & 0xF) << 14
|
||||
| ((closed_loop2.active_low_high_brake_threshold as u32) & 0x7) << 11
|
||||
| ((closed_loop2.brake_pin_threshold as u32) & 0x7) << 8
|
||||
| ((closed_loop2.avs_enable as u32) & 0x1) << 7
|
||||
| ((closed_loop2.cycle_current_limit as u32) & 0xF) << 3;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x88, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_closed_loop3(self, closed_loop3: ClosedLoop3) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_closed_loop3(closed_loop3: {closed_loop3:?})");
|
||||
|
||||
let mut expected_value =
|
||||
((closed_loop3.degauss_samples as u32) & 0x2) << 29
|
||||
| ((closed_loop3.degauss_upper_bound as u32) & 0x3) << 27
|
||||
| ((closed_loop3.degauss_lower_bound as u32) & 0x3) << 25
|
||||
| ((closed_loop3.integration_cycle_low_threshold as u32) & 0x3) << 23
|
||||
| ((closed_loop3.integration_cycle_high_threshold as u32) & 0x3) << 21
|
||||
| ((closed_loop3.integration_duty_cycle_low_threshold as u32) & 0x3) << 19
|
||||
| ((closed_loop3.integration_duty_cycle_high_threshold as u32) & 0x3) << 17
|
||||
| ((closed_loop3.bemf_threshold2 as u32) & 0x3F) << 11
|
||||
| ((closed_loop3.bemf_threshold1 as u32) & 0x3F) << 5
|
||||
| ((closed_loop3.commutation_method as u32) & 0x1) << 4
|
||||
| ((closed_loop3.degauss_window as u32) & 0x7) << 1
|
||||
| ((closed_loop3.degauss_enable as u32) & 0x1) << 0;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x8A, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_closed_loop4(self, closed_loop4: ClosedLoop4) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_closed_loop4(closed_loop4: {closed_loop4:?})");
|
||||
|
||||
let mut expected_value =
|
||||
((closed_loop4.wcomp_blanking as u32) & 0x1) << 19
|
||||
| ((closed_loop4.fast_deceleration_duty_window as u32) & 0x7) << 16
|
||||
| ((closed_loop4.fast_deceleration_duty_threshold as u32) & 0x7) << 13
|
||||
| ((closed_loop4.dynamic_brake_current_lower_threshold as u32) & 0x7) << 9
|
||||
| ((closed_loop4.dynamic_braking_current as u32) & 0x1) << 8
|
||||
| ((closed_loop4.fast_deceleration as u32) & 0x1) << 7
|
||||
| ((closed_loop4.fast_deceleration_current_theshold as u32) & 0xF) << 3
|
||||
| ((closed_loop4.fast_brake_delta as u32) & 0x7) << 0;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x8C, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_constant_speed(self, constant_speed: ConstantSpeed) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_constant_speed(constant_speed: {constant_speed:?})");
|
||||
|
||||
let speed_power_kp = (constant_speed.speed_power_kp * 10000f64).round().clamp(0.0f64, 0x3FF as f64) as u32;
|
||||
let speed_power_ki = (constant_speed.speed_power_ki * 1000000f64).round().clamp(0.0f64, 0xFFF as f64) as u32;
|
||||
|
||||
let mut expected_value =
|
||||
speed_power_kp << 20
|
||||
| speed_power_ki << 8
|
||||
| ((constant_speed.speed_power_upper_limit as u32) & 0x7) << 5
|
||||
| ((constant_speed.speed_power_lower_limit as u32) & 0x7) << 2
|
||||
| ((constant_speed.mode as u32) & 0x3) << 9;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x8E, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_constant_power(self, constant_power: ConstantPower) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_constant_power(constant_power: {constant_power:?})");
|
||||
|
||||
let max_speed = (constant_power.max_speed * 16f64).round().clamp(0.0f64, 0xFFFF as f64) as u32;
|
||||
let max_power = (constant_power.max_power * 4f64).round().clamp(0.0f64, 0x3FF as f64) as u32;
|
||||
|
||||
let mut expected_value =
|
||||
max_speed << 15
|
||||
| ((constant_power.dead_time_compensation as u32) & 0x1) << 14
|
||||
| max_power << 4
|
||||
| ((constant_power.power_hysteresis as u32) & 0x3) << 2
|
||||
| ((constant_power.mode as u32) & 0x3) << 0;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x90, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_two_phase_profile(self, profile: TwoPhase150DegreeProfile) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_two_phase_profile(profile: {profile:?})");
|
||||
|
||||
let mut expected_value =
|
||||
((profile.steps[0] as u32) & 0x7) << 28
|
||||
| ((profile.steps[1] as u32) & 0x7) << 25
|
||||
| ((profile.steps[2] as u32) & 0x7) << 22
|
||||
| ((profile.steps[3] as u32) & 0x7) << 19
|
||||
| ((profile.steps[4] as u32) & 0x7) << 16
|
||||
| ((profile.steps[5] as u32) & 0x7) << 13
|
||||
| ((profile.steps[6] as u32) & 0x7) << 10
|
||||
| ((profile.steps[7] as u32) & 0x7) << 7;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x96, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_three_phase_profile(self, profile: ThreePhase150DegreeProfile) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_three_phase_profile(profile: {profile:?})");
|
||||
|
||||
let mut expected_value =
|
||||
((profile.steps[0] as u32) & 0x7) << 28
|
||||
| ((profile.steps[1] as u32) & 0x7) << 25
|
||||
| ((profile.steps[2] as u32) & 0x7) << 22
|
||||
| ((profile.steps[3] as u32) & 0x7) << 19
|
||||
| ((profile.steps[4] as u32) & 0x7) << 16
|
||||
| ((profile.steps[5] as u32) & 0x7) << 13
|
||||
| ((profile.steps[6] as u32) & 0x7) << 10
|
||||
| ((profile.steps[7] as u32) & 0x7) << 7
|
||||
| ((profile.lead_angle as u32) & 0x3) << 5;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x98, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_trap_config1(self, trap_config1: TrapConfig1) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_trap_config1(trap_config1: {trap_config1:?})");
|
||||
|
||||
let mut expected_value =
|
||||
((trap_config1.open_loop_handoff_cycles as u32) & 0x3) << 22
|
||||
| ((trap_config1.avs_negative_current_limit as u32) & 0x7) << 16
|
||||
| ((trap_config1.avs_limit_hysteresis as u32) & 0x1) << 15
|
||||
| ((trap_config1.isd_bemf_threshold as u32) & 0x1F) << 10
|
||||
| ((trap_config1.isd_cycle_threshold as u32) & 0x7) << 7
|
||||
| ((trap_config1.open_loop_zc_detection_threshold as u32) & 0x7) << 2
|
||||
| ((trap_config1.fast_startup_div_factor as u32) & 0x3) << 0
|
||||
;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x9A, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
pub fn set_trap_config2(self, trap_config2: TrapConfig2) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::set_trap_config2(trap_config2: {trap_config2:?})");
|
||||
|
||||
let mut expected_value =
|
||||
((trap_config2.blanking_time_microseconds as u32) & 0xF) << 27
|
||||
| ((trap_config2.comparator_deglitch_time_microseconds as u32) & 0x7) << 24
|
||||
| 1u32 << 21
|
||||
| ((trap_config2.align_duty_cycle as u32) & 0x7) << 18;
|
||||
|
||||
// Set parity bit correctly
|
||||
if expected_value.count_ones() % 2 == 1 {
|
||||
expected_value |= 0x80000000;
|
||||
}
|
||||
|
||||
self.assert_register(0x9C, Mct8316AVData::Four(expected_value))
|
||||
}
|
||||
|
||||
fn assert_register(self, address: u32, value: Mct8316AVData) -> Result<Self> {
|
||||
trace!("Mct8316AVEeprom::assert_register(address: {address:06x}, value: {value})");
|
||||
|
||||
let mut read_value = value.clone();
|
||||
|
||||
self.driver.read(address, &mut read_value)?;
|
||||
|
||||
if read_value == value {
|
||||
Ok(self)
|
||||
} else {
|
||||
self.driver.write(address, value)?;
|
||||
|
||||
Ok(Self {
|
||||
modified: true,
|
||||
..self
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
pub fn commit(self) -> Result<()> {
|
||||
trace!("Mct8316AVEeprom::commit()");
|
||||
if self.modified {
|
||||
bail!("TODO");
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
108
flight/src/hardware/mct8316a/isd_config.rs
Normal file
108
flight/src/hardware/mct8316a/isd_config.rs
Normal file
@@ -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,
|
||||
}
|
||||
17
flight/src/hardware/mct8316a/mod.rs
Normal file
17
flight/src/hardware/mct8316a/mod.rs
Normal file
@@ -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;
|
||||
363
flight/src/hardware/mct8316a/motor_startup.rs
Normal file
363
flight/src/hardware/mct8316a/motor_startup.rs
Normal file
@@ -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,
|
||||
}
|
||||
|
||||
|
||||
|
||||
49
flight/src/hardware/mct8316a/phase_profile.rs
Normal file
49
flight/src/hardware/mct8316a/phase_profile.rs
Normal file
@@ -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,
|
||||
}
|
||||
140
flight/src/hardware/mct8316a/trap_config.rs
Normal file
140
flight/src/hardware/mct8316a/trap_config.rs
Normal file
@@ -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,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,10 +1,18 @@
|
||||
use crate::hardware::mcp23017::Mcp23017;
|
||||
use anyhow::Result;
|
||||
use embedded_hal::pwm::SetDutyCycle;
|
||||
use crate::hardware::mct8316a::Mct8316a;
|
||||
|
||||
pub trait Hardware {
|
||||
type Pwm: SetDutyCycle<Error: std::error::Error + Sync + Send> + Sync;
|
||||
|
||||
fn new_mcp23017_a(&self) -> Result<impl Mcp23017 + Sync>;
|
||||
fn new_mcp23017_b(&self) -> Result<impl Mcp23017 + Sync>;
|
||||
|
||||
fn new_pwm0(&self) -> Result<Self::Pwm>;
|
||||
|
||||
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync>;
|
||||
|
||||
fn get_battery_voltage(&self) -> Result<f64>;
|
||||
}
|
||||
|
||||
@@ -33,3 +41,4 @@ mod error;
|
||||
pub mod mcp23017;
|
||||
mod mcp3208;
|
||||
pub mod channelization;
|
||||
pub(crate) mod mct8316a;
|
||||
|
||||
@@ -1,20 +1,25 @@
|
||||
mod pwm;
|
||||
|
||||
use crate::hardware::mcp23017::{Mcp23017, Mcp23017Driver};
|
||||
use crate::hardware::mcp3208::Mcp3208;
|
||||
use crate::hardware::Hardware;
|
||||
use anyhow::Result;
|
||||
use embedded_hal_bus::i2c::MutexDevice;
|
||||
use log::{debug, info, trace};
|
||||
// use rpi_pal::gpio::Gpio;
|
||||
use rpi_pal::gpio::Gpio;
|
||||
use rpi_pal::i2c::I2c;
|
||||
use rpi_pal::spi::SimpleHalSpiDevice;
|
||||
use rpi_pal::spi::{Bus, Mode, SlaveSelect, Spi};
|
||||
use std::cell::RefCell;
|
||||
use std::sync::Mutex;
|
||||
use rpi_pal::pwm::Pwm;
|
||||
use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a};
|
||||
use crate::hardware::raspi::pwm::PwmWrapper;
|
||||
|
||||
const CLOCK_1MHZ: u32 = 1_000_000;
|
||||
|
||||
pub struct RaspiHardware {
|
||||
// gpio: Gpio,
|
||||
_gpio: Gpio,
|
||||
i2c_bus: Mutex<I2c>,
|
||||
mcp3208: RefCell<Mcp3208<SimpleHalSpiDevice>>,
|
||||
}
|
||||
@@ -28,7 +33,7 @@ impl RaspiHardware {
|
||||
debug!("SOC: {}", device.soc());
|
||||
|
||||
Ok(Self {
|
||||
// gpio: Gpio::new()?,
|
||||
_gpio: Gpio::new()?,
|
||||
i2c_bus: Mutex::new(I2c::with_bus(0u8)?),
|
||||
mcp3208: Mcp3208::new(SimpleHalSpiDevice::new(Spi::new(
|
||||
Bus::Spi1,
|
||||
@@ -41,15 +46,35 @@ impl RaspiHardware {
|
||||
}
|
||||
|
||||
impl Hardware for RaspiHardware {
|
||||
type Pwm = PwmWrapper;
|
||||
|
||||
fn new_mcp23017_a(&self) -> Result<impl Mcp23017> {
|
||||
trace!("RaspiHardware::new_mcp23017_a()");
|
||||
Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100000))
|
||||
}
|
||||
|
||||
fn new_mcp23017_b(&self) -> Result<impl Mcp23017> {
|
||||
trace!("RaspiHardware::new_mcp23017_b()");
|
||||
Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100001))
|
||||
}
|
||||
|
||||
fn new_pwm0(&self) -> Result<Self::Pwm> {
|
||||
trace!("RaspiHardware::new_pwm0()");
|
||||
// Unfortunately the current version of rpi_pal assumes an older version
|
||||
// of the kernel where pwmchip for RPi5 was 2
|
||||
const PWMCHIP: u8 = 0;
|
||||
const CHANNEL: u8 = 0;
|
||||
Ok(PwmWrapper::new(Pwm::with_pwmchip(PWMCHIP, CHANNEL)?)?)
|
||||
}
|
||||
|
||||
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
|
||||
trace!("RaspiHardware::new_mct8316a()");
|
||||
Ok(Mct8316AVDriver::new(MutexDevice::new(&self.i2c_bus), 0b0000000))
|
||||
}
|
||||
|
||||
|
||||
fn get_battery_voltage(&self) -> Result<f64> {
|
||||
trace!("RaspiHardware::get_battery_voltage()");
|
||||
self.mcp3208.borrow_mut().read_single(1)
|
||||
}
|
||||
}
|
||||
|
||||
55
flight/src/hardware/raspi/pwm.rs
Normal file
55
flight/src/hardware/raspi/pwm.rs
Normal file
@@ -0,0 +1,55 @@
|
||||
use std::fmt::{Display, Formatter};
|
||||
use std::time::Duration;
|
||||
use embedded_hal::pwm::{ErrorKind, ErrorType, SetDutyCycle};
|
||||
use log::trace;
|
||||
use rpi_pal::pwm::Pwm;
|
||||
|
||||
const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz
|
||||
|
||||
pub struct PwmWrapper {
|
||||
pwm: Pwm
|
||||
}
|
||||
|
||||
impl PwmWrapper {
|
||||
pub fn new(pwm: Pwm) -> anyhow::Result<Self> {
|
||||
trace!("PwmWrapper::new(pwm: {pwm:?})");
|
||||
pwm.set_period(PWM_PERIOD)?;
|
||||
pwm.enable()?;
|
||||
Ok(Self {
|
||||
pwm
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct ErrorWrapper(rpi_pal::pwm::Error);
|
||||
|
||||
impl embedded_hal::pwm::Error for ErrorWrapper {
|
||||
fn kind(&self) -> ErrorKind {
|
||||
ErrorKind::Other
|
||||
}
|
||||
}
|
||||
|
||||
impl Display for ErrorWrapper {
|
||||
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
|
||||
self.0.fmt(f)
|
||||
}
|
||||
}
|
||||
|
||||
impl std::error::Error for ErrorWrapper {}
|
||||
|
||||
impl ErrorType for PwmWrapper {
|
||||
type Error = ErrorWrapper;
|
||||
}
|
||||
|
||||
impl SetDutyCycle for PwmWrapper {
|
||||
fn max_duty_cycle(&self) -> u16 {
|
||||
trace!("PwmWrapper::max_duty_cycle()");
|
||||
u16::MAX
|
||||
}
|
||||
|
||||
fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
|
||||
trace!("PwmWrapper::set_duty_cycle(duty: {duty})");
|
||||
self.pwm.set_duty_cycle((duty as f64) / (u16::MAX as f64)).map_err(ErrorWrapper)
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -1,8 +1,9 @@
|
||||
use anyhow::Result;
|
||||
use log::debug;
|
||||
use log::{debug, LevelFilter};
|
||||
use std::fs::create_dir_all;
|
||||
use std::str::FromStr;
|
||||
use std::{env, thread};
|
||||
use fern::colors::{Color, ColoredLevelConfig};
|
||||
|
||||
pub fn setup_logger() -> Result<()> {
|
||||
let log_file = env::var("LOG_FILE").or_else(|_| {
|
||||
@@ -19,18 +20,23 @@ pub fn setup_logger() -> Result<()> {
|
||||
Err(_) => log::LevelFilter::Info,
|
||||
};
|
||||
|
||||
let colors = ColoredLevelConfig::new()
|
||||
.error(Color::Red)
|
||||
.warn(Color::Yellow)
|
||||
.info(Color::White)
|
||||
.debug(Color::White)
|
||||
.trace(Color::BrightBlack);
|
||||
|
||||
fern::Dispatch::new()
|
||||
.format(|out, message, record| {
|
||||
.format(move |out, message, record| {
|
||||
let binding = thread::current();
|
||||
let thread_name = binding.name().unwrap_or("<unnamed>");
|
||||
|
||||
out.finish(format_args!(
|
||||
"[{}][{}][{}][{}] {}",
|
||||
record.level(),
|
||||
chrono::Local::now().format("%Y-%m-%d %H:%M:%S%.9f"),
|
||||
thread_name,
|
||||
record.target(),
|
||||
message,
|
||||
"[{time} {level} {thread_name} {target}] {message}",
|
||||
level=colors.color(record.level()),
|
||||
time=chrono::Local::now().format("%Y-%m-%dT%H:%M:%S%.9f"),
|
||||
target=record.target(),
|
||||
))
|
||||
})
|
||||
.chain(
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user