From d9728b8680695ff5b3044f5817a6a37e27a55404 Mon Sep 17 00:00:00 2001 From: Sergey Savelyev Date: Wed, 19 Aug 2026 19:24:28 -0700 Subject: [PATCH] adds initial motor bravo implementation --- Cargo.lock | 27 ++ Cargo.toml | 3 + avionics/avionics.kicad_pcb | 13 + avionics/avionics.kicad_sch | 4 +- avionics/eps.kicad_sch | 96 ++-- common/src/command/mod.rs | 1 + common/src/command/set_pwm.rs | 9 + config.txt | 2 + flight/Cargo.toml | 3 + flight/src/commanded_state.rs | 2 +- flight/src/comms/mod.rs | 6 +- flight/src/hardware/a4963/driver.rs | 145 ++++++ flight/src/hardware/a4963/mod.rs | 12 + flight/src/hardware/a4963/register.rs | 587 +++++++++++++++++++++++++ flight/src/hardware/channelization.rs | 76 ++-- flight/src/hardware/mcp23017/driver.rs | 8 +- flight/src/hardware/mcp23017/task.rs | 8 +- flight/src/hardware/mct8316a/driver.rs | 26 +- flight/src/hardware/mod.rs | 14 +- flight/src/hardware/raspi/mod.rs | 72 ++- flight/src/hardware/sim/hardware.rs | 8 + flight/src/hardware/sim/mct8316a.rs | 9 +- flight/src/hardware/sim/mod.rs | 3 +- flight/src/lib.rs | 39 +- ground/src/command.rs | 31 +- 25 files changed, 1081 insertions(+), 123 deletions(-) create mode 100644 common/src/command/set_pwm.rs create mode 100644 flight/src/hardware/a4963/driver.rs create mode 100644 flight/src/hardware/a4963/mod.rs create mode 100644 flight/src/hardware/a4963/register.rs diff --git a/Cargo.lock b/Cargo.lock index 01e3db3..aa773d5 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -595,6 +595,12 @@ dependencies = [ "microlp", ] +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" + [[package]] name = "http" version = "1.4.0" @@ -778,6 +784,9 @@ dependencies = [ "nautilus_common", "postcard", "rpi-pal", + "strum", + "strum_macros", + "thiserror", ] [[package]] @@ -1332,6 +1341,24 @@ dependencies = [ "smallvec", ] +[[package]] +name = "strum" +version = "0.28.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9628de9b8791db39ceda2b119bbe13134770b56c138ec1d3af810d045c04f9bd" + +[[package]] +name = "strum_macros" +version = "0.28.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ab85eea0270ee17587ed4156089e10b9e6880ee688791d45a905f5b1ca36f664" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "syn 2.0.112", +] + [[package]] name = "subtle" version = "2.6.1" diff --git a/Cargo.toml b/Cargo.toml index e4aeca3..05e2388 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -25,6 +25,9 @@ tokio = { version = "1.48.0" } tokio-util = "0.7.17" env_logger = "0.11.8" good_lp = { version = "1.14.2", default-features = false } +hex = "0.4.3" +strum = "0.28.0" +strum_macros = "0.28.0" [profile.dev.package.good_lp] opt-level = 3 diff --git a/avionics/avionics.kicad_pcb b/avionics/avionics.kicad_pcb index a58ff52..3d89b39 100644 --- a/avionics/avionics.kicad_pcb +++ b/avionics/avionics.kicad_pcb @@ -36912,6 +36912,19 @@ (justify left bottom) ) ) + (gr_text "REALIGN PADS" + (at 126.75 57.25 90) + (layer "User.1") + (uuid "8ef41091-99f8-4363-a916-34cf5745d944") + (effects + (font + (size 1.5 1.5) + (thickness 0.3) + (bold yes) + ) + (justify left bottom) + ) + ) (dimension (type orthogonal) (layer "Cmts.User") diff --git a/avionics/avionics.kicad_sch b/avionics/avionics.kicad_sch index b28a447..f4298bc 100644 --- a/avionics/avionics.kicad_sch +++ b/avionics/avionics.kicad_sch @@ -9621,7 +9621,7 @@ ) ) ) - (property "Datasheet" "" + (property "Datasheet" "https://optoelectronics.liteon.com/upload/download/DS22-2000-109/LTST-C171KRKT.pdf" (at 213.36 30.48 90) (hide yes) (show_name no) @@ -11853,7 +11853,7 @@ ) ) ) - (property "Datasheet" "" + (property "Datasheet" "https://optoelectronics.liteon.com/upload/download/DS22-2000-109/LTST-C171KRKT.pdf" (at 213.36 27.94 90) (hide yes) (show_name no) diff --git a/avionics/eps.kicad_sch b/avionics/eps.kicad_sch index b0cb7b6..46a2627 100644 --- a/avionics/eps.kicad_sch +++ b/avionics/eps.kicad_sch @@ -1885,12 +1885,6 @@ (color 0 0 0 0) (uuid "58737559-4af0-4025-a15b-0be67b948e40") ) - (junction - (at 78.74 25.4) - (diameter 0) - (color 0 0 0 0) - (uuid "638f1580-1417-43d0-b150-028593283c47") - ) (junction (at 43.18 93.98) (diameter 0) @@ -1927,6 +1921,12 @@ (color 0 0 0 0) (uuid "858b5d28-91e1-4c3e-bf95-5e809dd5d3ba") ) + (junction + (at 64.77 24.13) + (diameter 0) + (color 0 0 0 0) + (uuid "8757994f-eb6b-451a-a95f-23ea687e6c5e") + ) (junction (at 40.64 137.16) (diameter 0) @@ -1975,12 +1975,6 @@ (color 0 0 0 0) (uuid "b924a36d-88a6-4cf0-88f5-211ab223142d") ) - (junction - (at 64.77 25.4) - (diameter 0) - (color 0 0 0 0) - (uuid "c980e8fa-d354-48ba-8e1b-4e8c5032a88c") - ) (junction (at 38.1 113.03) (diameter 0) @@ -2059,6 +2053,12 @@ (color 0 0 0 0) (uuid "fd8276ae-87af-4a04-801b-77ae578f6c83") ) + (junction + (at 78.74 22.86) + (diameter 0) + (color 0 0 0 0) + (uuid "ff809a6f-44a7-414d-89e0-54e4c492c583") + ) (wire (pts (xy 139.7 36.83) (xy 139.7 35.56) @@ -2081,7 +2081,7 @@ ) (wire (pts - (xy 62.23 33.02) (xy 62.23 25.4) + (xy 62.23 33.02) (xy 62.23 24.13) ) (stroke (width 0) @@ -2161,7 +2161,7 @@ ) (wire (pts - (xy 62.23 25.4) (xy 64.77 25.4) + (xy 62.23 24.13) (xy 64.77 24.13) ) (stroke (width 0) @@ -2219,6 +2219,16 @@ ) (uuid "2688f221-dfad-4bbb-926b-0f359b25b508") ) + (wire + (pts + (xy 64.77 24.13) (xy 87.63 24.13) + ) + (stroke + (width 0) + (type default) + ) + (uuid "275307e6-93c8-471f-a651-8bbfc3cfbd1a") + ) (wire (pts (xy 186.69 25.4) (xy 184.15 25.4) @@ -2269,6 +2279,16 @@ ) (uuid "427a5cce-66db-4adb-a9f8-ef0864bf88fc") ) + (wire + (pts + (xy 87.63 25.4) (xy 90.17 25.4) + ) + (stroke + (width 0) + (type default) + ) + (uuid "492e28eb-03a1-49fe-a2b5-e6ea309272d7") + ) (wire (pts (xy 118.11 35.56) (xy 120.65 35.56) @@ -2309,6 +2329,16 @@ ) (uuid "53eb8329-0ac7-4e17-9be3-84176607c2f4") ) + (wire + (pts + (xy 78.74 22.86) (xy 78.74 25.4) + ) + (stroke + (width 0) + (type default) + ) + (uuid "54c42731-c9a9-44b0-b081-920babf85581") + ) (wire (pts (xy 139.7 34.29) (xy 139.7 35.56) @@ -2329,6 +2359,16 @@ ) (uuid "5751b0a6-69cd-4e52-9692-c42e5559aa79") ) + (wire + (pts + (xy 64.77 24.13) (xy 64.77 25.4) + ) + (stroke + (width 0) + (type default) + ) + (uuid "58c44f9f-9018-406e-b3fe-6e8f10997877") + ) (wire (pts (xy 78.74 34.29) (xy 87.63 34.29) @@ -2479,16 +2519,6 @@ ) (uuid "8239b1ab-2d60-484e-957f-3c08014438e9") ) - (wire - (pts - (xy 64.77 25.4) (xy 78.74 25.4) - ) - (stroke - (width 0) - (type default) - ) - (uuid "82779f97-be5f-4a84-9346-3af6de0eacee") - ) (wire (pts (xy 87.63 27.94) (xy 87.63 33.02) @@ -2759,6 +2789,16 @@ ) (uuid "c56aaf00-d142-41e0-888b-162694ec5357") ) + (wire + (pts + (xy 87.63 24.13) (xy 87.63 25.4) + ) + (stroke + (width 0) + (type default) + ) + (uuid "c5f9aec5-b051-431f-84da-edd43d77c375") + ) (wire (pts (xy 139.7 55.88) (xy 99.06 55.88) @@ -2841,7 +2881,7 @@ ) (wire (pts - (xy 52.07 22.86) (xy 90.17 22.86) + (xy 52.07 22.86) (xy 78.74 22.86) ) (stroke (width 0) @@ -2931,13 +2971,13 @@ ) (wire (pts - (xy 78.74 25.4) (xy 90.17 25.4) + (xy 78.74 22.86) (xy 90.17 22.86) ) (stroke (width 0) (type default) ) - (uuid "f06f6fbc-c2c6-4b90-91a1-2c62b78fe3a7") + (uuid "f119ed64-4dec-4ca6-8ae8-2509054b948a") ) (wire (pts @@ -3281,7 +3321,7 @@ ) ) ) - (property "Datasheet" "" + (property "Datasheet" "https://www.ti.com/lit/ds/symlink/tps51388.pdf?ts=1786658782522" (at 100.33 22.86 0) (hide yes) (show_name no) diff --git a/common/src/command/mod.rs b/common/src/command/mod.rs index 3c7ac45..8394f06 100644 --- a/common/src/command/mod.rs +++ b/common/src/command/mod.rs @@ -5,6 +5,7 @@ use std::fmt::Debug; pub mod set_pin; pub mod set_rcs; pub mod valid_priority_command; +pub mod set_pwm; #[derive(Clone, Debug, Serialize, Deserialize)] pub struct CommandHeader<'a> { diff --git a/common/src/command/set_pwm.rs b/common/src/command/set_pwm.rs new file mode 100644 index 0000000..cc8d46d --- /dev/null +++ b/common/src/command/set_pwm.rs @@ -0,0 +1,9 @@ +use crate::command::Command; +use serde::{Deserialize, Serialize}; + +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct SetPwm { + pub duty_cycle: u8 +} + +impl Command for SetPwm {} diff --git a/config.txt b/config.txt index 34797f7..90a273b 100644 --- a/config.txt +++ b/config.txt @@ -3,8 +3,10 @@ # Some settings may impact device functionality. See link above for details +# Enable SPI0 with 1 Chip Select Pin # Enable SPI1 with 3 Chip Select Pins dtparam=spi=on +dtoverlay=spi0-1cs dtoverlay=spi1-3cs # Enable I2C0 diff --git a/flight/Cargo.toml b/flight/Cargo.toml index 9c2f711..18a4dd1 100644 --- a/flight/Cargo.toml +++ b/flight/Cargo.toml @@ -15,6 +15,9 @@ nautilus_common = { workspace = true } postcard = { workspace = true } rpi-pal = { workspace = true, features = ["hal"], optional = true } good_lp = { workspace = true, features = ["microlp"] } +strum = {workspace = true} +strum_macros = {workspace = true} +thiserror = {workspace = true} [dev-dependencies] embedded-hal-mock = { workspace = true } diff --git a/flight/src/commanded_state.rs b/flight/src/commanded_state.rs index 0466b91..ef12609 100644 --- a/flight/src/commanded_state.rs +++ b/flight/src/commanded_state.rs @@ -30,7 +30,7 @@ where Self { entries: [const { None }; N], default, - changed: false, + changed: true, } } diff --git a/flight/src/comms/mod.rs b/flight/src/comms/mod.rs index 462449c..e0bf046 100644 --- a/flight/src/comms/mod.rs +++ b/flight/src/comms/mod.rs @@ -27,7 +27,7 @@ impl TelemetrySender { } } -type CommandCallback<'a> = Box Result<()> + Send + 'a>; +type CommandCallback<'a> = Box Result<()> + Send + 'a>; pub struct CommsTask<'a, A> where @@ -82,7 +82,7 @@ where pub fn add_command_handler( &mut self, command: impl Into, - handler: impl Fn(T) + Send + 'a, + mut handler: impl FnMut(T) + Send + 'a, ) -> Result<()> { let command = command.into(); ensure!( @@ -137,7 +137,7 @@ where .rx_bytes .wrapping_add(u32::try_from(size % (u32::MAX as usize)).unwrap_or(0)); }); - match self.command_callbacks.get(cmd.name) { + match self.command_callbacks.get_mut(cmd.name) { Some(handler) => { if let Err(e) = handler(cmd.data) { error!("Command Error: {e}"); diff --git a/flight/src/hardware/a4963/driver.rs b/flight/src/hardware/a4963/driver.rs new file mode 100644 index 0000000..1d287cb --- /dev/null +++ b/flight/src/hardware/a4963/driver.rs @@ -0,0 +1,145 @@ +use std::any::type_name; +use std::fmt::{Debug, Formatter}; +use std::thread::sleep; +use std::time::Duration; +use crate::hardware::a4963::A4963; +use anyhow::{ensure, Result}; +use embedded_hal::spi::SpiDevice; +use log::{trace, warn}; +use crate::hardware::a4963::register::{A4963Register, Configuration0Register, Configuration1Register, Configuration2Register, Configuration3Register, Configuration4Register, Configuration5Register, DiagnosticRegister, Direction, MaskRegister, MotorControlMode, RunRegister, SpeedOutputSelection, WRITE_BIT}; +use crate::hardware::error::WrappingError; + +pub struct A4963Driver { + spi: SPI, + mask_register: MaskRegister +} + +impl A4963Driver +where + SPI: SpiDevice, + SPI::Error: Send, + SPI::Error: Sync, + SPI::Error: 'static, { + pub fn new(spi: SPI) -> Self { + trace!( + "A4963Driver::new(spi)", + type_name::() + ); + Self { + spi, + mask_register: MaskRegister { + ..Default::default() + }, + } + } + + fn update_mask_register(&mut self, mask_register: MaskRegister) -> Result { + self.mask_register = mask_register; + self.write_verify(self.mask_register.clone()) + } + + fn read_diagnostic_register(&mut self) -> Result { + self.write(self.mask_register.clone()) + } + + fn write(&mut self, register: Register) -> Result { + trace!("A4963Driver::write(self: {self:?}, register: {register:?})"); + let mut rx_buffer = [0u8; 2]; + let tx_data = register.encode() | Register::ADDRESS | WRITE_BIT; + trace!("A4963Driver::write - tx {} = {tx_data:016b}", type_name::()); + let tx_buffer = tx_data.to_be_bytes(); + self.spi.transfer(&mut rx_buffer, &tx_buffer).map_err(WrappingError)?; + let rx_data = u16::from_be_bytes(rx_buffer); + trace!("A4963Driver::write - rx = {rx_data:016b}"); + Ok(DiagnosticRegister::decode(rx_data)) + } + + fn read(&mut self) -> Result { + trace!("A4963Driver::read<{}>(self: {self:?})", type_name::()); + let mut rx_buffer = [0u8; 2]; + let tx_buffer = Register::ADDRESS.to_be_bytes(); + trace!("A4963Driver::read - tx = {:016b}", Register::ADDRESS); + self.spi.transfer(&mut rx_buffer, &tx_buffer).map_err(WrappingError)?; + let rx_data = u16::from_be_bytes(rx_buffer); + trace!("A4963Driver::read - rx {} = {rx_data:016b}", type_name::()); + DiagnosticRegister::decode(rx_data & 0b1110_0000_0000_0000) + .assert_healthy()?; + Ok(Register::decode(rx_data)) + } + + fn write_verify(&mut self, register: Register) -> Result { + let result = self.write(register.clone())?; + let readback = self.read()?; + + ensure!(register == readback, "Register did not match readback.\nregister = {register:?},\nreadback = {readback:?}"); + + Ok(result) + } +} + +impl A4963 for A4963Driver +where + SPI: SpiDevice, + SPI::Error: Send, + SPI::Error: Sync, + SPI::Error: 'static, { + + fn init(&mut self) -> Result<()> { + trace!("A4963Driver::init(self: {self:?})"); + + // Write the fault mask first + self.update_mask_register(MaskRegister { + enable_loss_of_synchronization: false, + ..Default::default() + })?.assert_healthy()?; + // Write our configurations + self.write_verify(Configuration0Register { + ..Default::default() + })?.assert_healthy()?; + self.write_verify(Configuration1Register { + // I_lim = (n + 1) * 0.0125 / 0.007 = 8.9285714286A + current_sense_threshold: 4, + ..Default::default() + })?.assert_healthy()?; + self.write_verify(Configuration2Register { + ..Default::default() + })?.assert_healthy()?; + self.write_verify(Configuration3Register { + ..Default::default() + })?.assert_healthy()?; + self.write_verify(Configuration4Register { + forced_startup_torque_duty_cycle: 0b0111, + start_speed: 0b1111, + ..Default::default() + })?.assert_healthy()?; + self.write_verify(Configuration5Register { + speed_output_selection: SpeedOutputSelection::CommutationFrequency, + // 819.1Hz = 49146 rpm max speed (divide by number of motor pole pairs) + // Measured maximum of the motor is ~840Hz + maximum_electrical_cycle_frequency: 0b101, + ..Default::default() + })?.assert_healthy()?; + // Set up the run mode for the motor (this enables the motor) + self.write_verify(RunRegister { + motor_control_mode: MotorControlMode::ClosedLoopSpeed, + direction: Direction::Forward, // A = Black, B = Yellow, C = Red + ..Default::default() + })?.assert_healthy()?; + + Ok(()) + } + + fn check_health(&mut self) -> Result<()> { + self.read_diagnostic_register()?.assert_healthy() + } +} + +impl Debug for A4963Driver { + fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { + write!( + f, + "A4963Driver {{ }}", + type_name::(), + ) + } +} diff --git a/flight/src/hardware/a4963/mod.rs b/flight/src/hardware/a4963/mod.rs new file mode 100644 index 0000000..054368e --- /dev/null +++ b/flight/src/hardware/a4963/mod.rs @@ -0,0 +1,12 @@ +mod driver; +mod register; + +use anyhow::Result; + +pub trait A4963 { + fn init(&mut self) -> Result<()>; + + fn check_health(&mut self) -> Result<()>; +} + +pub use driver::A4963Driver; diff --git a/flight/src/hardware/a4963/register.rs b/flight/src/hardware/a4963/register.rs new file mode 100644 index 0000000..c8d81fd --- /dev/null +++ b/flight/src/hardware/a4963/register.rs @@ -0,0 +1,587 @@ +use std::fmt::Debug; +use std::hint::unreachable_unchecked; +use strum_macros::FromRepr; +use anyhow::Result; +use thiserror::Error; + +pub(super) const WRITE_BIT: u16 = 0b0001_0000_0000_0000; + +pub(super) trait A4963Register: Clone + PartialEq + Debug { + const ADDRESS: u16; + + fn encode(self) -> u16; + fn decode(value: u16) -> Self; +} + +#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)] +#[repr(u8)] +pub(super) enum RecirculationMode { + Auto = 0b00, + High = 0b01, + Low = 0b10, + Off = 0b11, +} + +#[derive(Clone, Debug, PartialEq, Eq)] +pub(super) struct Configuration0Register { + pub recirculation_mode: RecirculationMode, + /// 4 bits long. Blank time is this * 400ns + pub blank_time: u8, + /// 6 bits long. Dead time is this * 50ns + pub dead_time: u8, +} + +impl Default for Configuration0Register { + fn default() -> Self { + Self { + recirculation_mode: RecirculationMode::Auto, + blank_time: 0b1000, + dead_time: 0b01_0100, + } + } +} + +impl A4963Register for Configuration0Register { + const ADDRESS: u16 = 0b0000_0000_0000_0000; + + fn encode(self) -> u16 { + assert!(self.blank_time <= 0b1111); + assert!(self.dead_time <= 0b11_1111); + + let mut result = 0; + + result |= (self.recirculation_mode as u16) << 10; + result |= (self.blank_time as u16) << 6; + result |= self.dead_time as u16; + + result + } + + fn decode(value: u16) -> Self { + Self { + recirculation_mode: RecirculationMode::from_repr(((value >> 10) & 0b11) as u8) + // Safety: the enum covers all permutations + .unwrap_or_else(|| unsafe { unreachable_unchecked() }), + blank_time: ((value >> 6) & 0b1111) as u8, + dead_time: (value & 0b11_1111) as u8, + } + } +} + +#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)] +#[repr(u8)] +pub(super) enum BemfTimeQualifier { + Debounce = 0b0, + Window = 0b1, +} + +#[derive(Clone, Debug, PartialEq, Eq)] +pub(super) struct Configuration1Register { + /// 12.5% if false, 25% if true + pub enable_fast_decay: bool, + pub invert_pwm: bool, + /// 4 bit value. V_ilm = (n + 1) * 12.5mV + pub current_sense_threshold: u8, + pub bemf_time_qualifier: BemfTimeQualifier, + /// 5 bit value. V_dst = n * 50mV + pub short_detection_threshold: u8, +} + +impl Default for Configuration1Register { + fn default() -> Self { + Self { + enable_fast_decay: false, + invert_pwm: false, + current_sense_threshold: 0b1111, + bemf_time_qualifier: BemfTimeQualifier::Debounce, + short_detection_threshold: 0b1_1111, + } + } +} + +impl A4963Register for Configuration1Register { + const ADDRESS: u16 = 0b0010_0000_0000_0000; + + fn encode(self) -> u16 { + assert!(self.current_sense_threshold <= 0b1111); + assert!(self.short_detection_threshold <= 0b1_1111); + + let mut result = 0; + + result |= if self.enable_fast_decay { 0b0000_1000_0000_0000 } else {0}; + result |= if self.invert_pwm { 0b0000_0100_0000_0000 } else {0}; + result |= (self.current_sense_threshold as u16) << 6; + result |= (self.bemf_time_qualifier as u16) << 5; + result |= self.short_detection_threshold as u16; + + result + } + + fn decode(value: u16) -> Self { + Self { + enable_fast_decay: (value & 0b0000_1000_0000_0000) != 0, + invert_pwm: (value & 0b0000_0100_0000_0000) != 0, + current_sense_threshold: ((value >> 6) & 0b1111) as u8, + bemf_time_qualifier: BemfTimeQualifier::from_repr(((value >> 5) & 0b1) as u8) + // Safety: the enum covers all permutations + .unwrap_or_else(|| unsafe { unreachable_unchecked() }), + short_detection_threshold: (value & 0b1_1111) as u8, + } + } +} + +#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)] +#[repr(u8)] +pub(super) enum OverspeedLimit { + Percent100 = 0b00, + Percent125 = 0b01, + Percent150 = 0b10, + Percent200 = 0b11, +} + +#[derive(Clone, Debug, PartialEq, Eq)] +pub(super) struct Configuration2Register { + /// K_CP = 2^(n-7) + pub position_proportional_gain: u8, + pub overspeed_limit: OverspeedLimit, + pub degauss_compensation: bool, + /// t_PW = 20us + (n * 1.6us) + pub fixed_period: u8, +} + +impl Default for Configuration2Register { + fn default() -> Self { + Self { + position_proportional_gain: 0b1000, + overspeed_limit: OverspeedLimit::Percent150, + degauss_compensation: false, + fixed_period: 0b1_0011, + } + } +} + +impl A4963Register for Configuration2Register { + const ADDRESS: u16 = 0b0100_0000_0000_0000; + + fn encode(self) -> u16 { + assert!(self.position_proportional_gain <= 0b1111); + assert!(self.fixed_period <= 0b1_1111); + + let mut result = 0; + + result |= (self.position_proportional_gain as u16) << 8; + result |= (self.overspeed_limit as u16) << 6; + result |= if self.degauss_compensation {0b0000_0000_0010_0000} else {0}; + result |= self.fixed_period as u16; + + result + } + + fn decode(value: u16) -> Self { + Self { + position_proportional_gain: ((value >> 8) & 0b1111) as u8, + overspeed_limit: OverspeedLimit::from_repr(((value >> 6) & 0b11) as u8) + // Safety: the enum covers all permutations + .unwrap_or_else(|| unsafe { unreachable_unchecked() }), + degauss_compensation: (value & 0b0000_0000_0010_0000) != 0, + fixed_period: (value & 0b1_1111) as u8, + } + } +} + +#[derive(Clone, Debug, PartialEq, Eq)] +pub(super) struct Configuration3Register { + /// K_CI = 2^(n-7) + pub position_integral_gain: u8, + /// D_H = (n + 1) * 6.25% + pub hold_torque_duty_cycle: u8, + /// t_HOLD = n * 8ms + pub hold_time: u8, +} + +impl Default for Configuration3Register { + fn default() -> Self { + Self { + position_integral_gain: 0b1000, + hold_torque_duty_cycle: 0b0101, + hold_time: 0b0010, + } + } +} + +impl A4963Register for Configuration3Register { + const ADDRESS: u16 = 0b0110_0000_0000_0000; + + fn encode(self) -> u16 { + assert!(self.position_integral_gain <= 0b1111); + assert!(self.hold_torque_duty_cycle <= 0b1111); + assert!(self.hold_time <= 0b1111); + + let mut result = 0; + + result |= (self.position_integral_gain as u16) << 8; + result |= (self.hold_torque_duty_cycle as u16) << 4; + result |= self.hold_time as u16; + + result + } + + fn decode(value: u16) -> Self { + Self { + position_integral_gain: ((value >> 8) & 0b1111) as u8, + hold_torque_duty_cycle: ((value >> 4) & 0b1111) as u8, + hold_time: (value & 0b1111) as u8, + } + } +} + +#[derive(Clone, Debug, PartialEq, Eq)] +pub(super) struct Configuration4Register { + /// K_SP = 2^(n-7) * K_NSP + pub speed_proportional_gain: u8, + /// D_S = (n + 1) * 6.25% + pub forced_startup_torque_duty_cycle: u8, + /// f_ST = (n + 1) * 2Hz + pub start_speed: u8, +} + +impl Default for Configuration4Register { + fn default() -> Self { + Self { + speed_proportional_gain: 0b1000, + forced_startup_torque_duty_cycle: 0b0111, + start_speed: 0b0011, + } + } +} + +impl A4963Register for Configuration4Register { + const ADDRESS: u16 = 0b1000_0000_0000_0000; + + fn encode(self) -> u16 { + assert!(self.speed_proportional_gain <= 0b1111); + assert!(self.forced_startup_torque_duty_cycle <= 0b1111); + assert!(self.start_speed <= 0b1111); + + let mut result = 0; + + result |= (self.speed_proportional_gain as u16) << 8; + result |= (self.forced_startup_torque_duty_cycle as u16) << 4; + result |= self.start_speed as u16; + + result + } + + fn decode(value: u16) -> Self { + Self { + speed_proportional_gain: ((value >> 8) & 0b1111) as u8, + forced_startup_torque_duty_cycle: ((value >> 4) & 0b1111) as u8, + start_speed: (value & 0b1111) as u8, + } + } +} + +#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)] +#[repr(u8)] +pub(super) enum SpeedOutputSelection { + ElectricalFrequency = 0b0, + CommutationFrequency = 0b1, +} + +#[derive(Clone, Debug, PartialEq, Eq)] +pub(super) struct Configuration5Register { + /// K_SI = 2^(n-7) * K_NSI + pub speed_integral_gain: u8, + pub speed_output_selection: SpeedOutputSelection, + /// f_MS = (2^(8 + n) - 1) * 0.1Hz + pub maximum_electrical_cycle_frequency: u8, + /// theta_ADV = n * 1.875˚ (electrical) + pub phase_advance: u8, +} + +impl Default for Configuration5Register { + fn default() -> Self { + Self { + speed_integral_gain: 0b1000, + speed_output_selection: SpeedOutputSelection::ElectricalFrequency, + maximum_electrical_cycle_frequency: 0b101, + phase_advance: 0b1000, + } + } +} + +impl A4963Register for Configuration5Register { + const ADDRESS: u16 = 0b1010_0000_0000_0000; + + fn encode(self) -> u16 { + assert!(self.speed_integral_gain <= 0b1111); + assert!(self.maximum_electrical_cycle_frequency <= 0b111); + assert!(self.phase_advance <= 0b1111); + + let mut result = 0; + + result |= (self.speed_integral_gain as u16) << 8; + result |= (self.speed_output_selection as u16) << 7; + result |= (self.maximum_electrical_cycle_frequency as u16) << 4; + result |= self.phase_advance as u16; + + result + } + + fn decode(value: u16) -> Self { + Self { + speed_integral_gain: ((value >> 8) & 0b1111) as u8, + speed_output_selection: SpeedOutputSelection::from_repr(((value >> 7) & 0b1) as u8) + // Safety: the enum covers all permutations + .unwrap_or_else(|| unsafe { unreachable_unchecked() }), + maximum_electrical_cycle_frequency: ((value >> 4) & 0b111) as u8, + phase_advance: (value & 0b1111) as u8, + } + } +} + +#[derive(Clone, Debug, PartialEq, Eq)] +pub(super) struct MaskRegister { + pub enable_temperature_warning: bool, + pub enable_overtemperature: bool, + pub enable_loss_of_synchronization: bool, + pub enable_undervoltage: bool, + pub enable_phase_a_high_side: bool, + pub enable_phase_a_low_side: bool, + pub enable_phase_b_high_side: bool, + pub enable_phase_b_low_side: bool, + pub enable_phase_c_high_side: bool, + pub enable_phase_c_low_side: bool, +} + +impl Default for MaskRegister { + fn default() -> Self { + Self { + enable_temperature_warning: true, + enable_overtemperature: true, + enable_loss_of_synchronization: true, + enable_undervoltage: true, + enable_phase_a_high_side: true, + enable_phase_a_low_side: true, + enable_phase_b_high_side: true, + enable_phase_b_low_side: true, + enable_phase_c_high_side: true, + enable_phase_c_low_side: true, + } + } +} + +impl A4963Register for MaskRegister { + const ADDRESS: u16 = 0b1100_0000_0000_0000; + + fn encode(self) -> u16 { + let mut result = 0; + + result |= if !self.enable_temperature_warning { 0b0000_1000_0000_0000 } else { 0 }; + result |= if !self.enable_overtemperature { 0b0000_0100_0000_0000 } else { 0 }; + result |= if !self.enable_loss_of_synchronization { 0b0000_0010_0000_0000 } else { 0 }; + result |= if !self.enable_undervoltage { 0b0000_0000_1000_0000 } else { 0 }; + result |= if !self.enable_phase_a_high_side { 0b0000_0000_0010_0000 } else { 0 }; + result |= if !self.enable_phase_a_low_side { 0b0000_0000_0001_0000 } else { 0 }; + result |= if !self.enable_phase_b_high_side { 0b0000_0000_0000_1000 } else { 0 }; + result |= if !self.enable_phase_b_low_side { 0b0000_0000_0000_0100 } else { 0 }; + result |= if !self.enable_phase_c_high_side { 0b0000_0000_0000_0010 } else { 0 }; + result |= if !self.enable_phase_c_low_side { 0b0000_0000_0000_0001 } else { 0 }; + + result + } + + fn decode(value: u16) -> Self { + Self { + enable_temperature_warning: (value & 0b0000_1000_0000_0000) == 0, + enable_overtemperature: (value & 0b0000_0100_0000_0000) == 0, + enable_loss_of_synchronization: (value & 0b0000_0010_0000_0000) == 0, + enable_undervoltage: (value & 0b0000_0000_1000_0000) == 0, + enable_phase_a_high_side: (value & 0b0000_0000_0010_0000) == 0, + enable_phase_a_low_side: (value & 0b0000_0000_0001_0000) == 0, + enable_phase_b_high_side: (value & 0b0000_0000_0000_1000) == 0, + enable_phase_b_low_side: (value & 0b0000_0000_0000_0100) == 0, + enable_phase_c_high_side: (value & 0b0000_0000_0000_0010) == 0, + enable_phase_c_low_side: (value & 0b0000_0000_0000_0001) == 0, + } + } +} + +#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)] +#[repr(u8)] +pub(super) enum MotorControlMode { + IndirectDutyCycle = 0b00, + DirectDutyCycle = 0b01, + ClosedLoopCurrent = 0b10, + ClosedLoopSpeed = 0b11, +} + +#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)] +#[repr(u8)] +pub(super) enum Direction { + Forward = 0b0, + Reverse = 0b1, +} + +#[derive(Clone, Debug, PartialEq, Eq)] +pub(super) struct RunRegister { + pub motor_control_mode: MotorControlMode, + pub stop_on_fail: bool, + /// 0 enables PWM input; otherwise duty cycle D_C = 7 + (n * 3)% + pub duty_cycle_control: u8, + pub restart_after_loss_of_sync: bool, + pub brake_when_inactive: bool, + pub direction: Direction, + pub enable: bool, +} + +impl Default for RunRegister { + fn default() -> Self { + Self { + motor_control_mode: MotorControlMode::IndirectDutyCycle, + stop_on_fail: false, + duty_cycle_control: 0, + restart_after_loss_of_sync: true, + brake_when_inactive: false, + direction: Direction::Forward, + enable: true, + } + } +} + +impl A4963Register for RunRegister { + const ADDRESS: u16 = 0b1110_0000_0000_0000; + + fn encode(self) -> u16 { + assert!(self.duty_cycle_control <= 0b11111); + + let mut result = 0; + + result |= (self.motor_control_mode as u16) << 10; + result |= if self.stop_on_fail { 0b0000_0010_0000_0000 } else { 0 }; + result |= (self.duty_cycle_control as u16) << 4; + result |= if self.restart_after_loss_of_sync { 0b0000_0000_0000_1000 } else { 0 }; + result |= if self.brake_when_inactive { 0b0000_0000_0000_0100 } else { 0 }; + result |= (self.direction as u16) << 1; + result |= if self.enable { 0b0000_0000_0000_0001 } else { 0 }; + + result + } + + fn decode(value: u16) -> Self { + Self { + motor_control_mode: MotorControlMode::from_repr(((value >> 10) & 0b11) as u8) + // Safety: the enum covers all permutations + .unwrap_or_else(|| unsafe { unreachable_unchecked() }), + stop_on_fail: (value & 0b0000_0010_0000_0000) != 0, + duty_cycle_control: ((value >> 4) & 0b11111) as u8, + restart_after_loss_of_sync: (value & 0b0000_0000_0000_1000) != 0, + brake_when_inactive: (value & 0b0000_0000_0000_0100) != 0, + direction: Direction::from_repr(((value >> 1) & 0b1) as u8) + // Safety: the enum covers all permutations + .unwrap_or_else(|| unsafe { unreachable_unchecked() }), + enable: (value & 0b0000_0000_0000_0001) != 0, + } + } +} + +#[derive(Clone, Debug, PartialEq, Eq)] +pub(super) struct DiagnosticRegister { + pub fault_flag: bool, + pub power_on_reset: bool, + pub serial_transfer_error: bool, + pub high_temperature_warning: bool, + pub overtemperature_shutdown: bool, + pub loss_of_synchronization: bool, + pub undervoltage: bool, + pub phase_a_high_side_fault: bool, + pub phase_a_low_side_fault: bool, + pub phase_b_high_side_fault: bool, + pub phase_b_low_side_fault: bool, + pub phase_c_high_side_fault: bool, + pub phase_c_low_side_fault: bool, +} + +#[derive(Clone, Debug, Error, PartialEq, Eq)] +enum DiagnosticRegisterError { + #[error("Unknown Fault")] + UnknownFault, + #[error("Power On Reset")] + PowerOnReset, + #[error("Serial Transfer Error")] + SerialTransferError, + #[error("High Temperature Warning")] + HighTemperatureWarning, + #[error("Overtemperature Fault")] + Overtemperature, + #[error("Loss of Synchronization Fault")] + LossOfSynchronization, + #[error("Undervoltage Fault")] + Undervoltage, + #[error("Phase A High Side Fault")] + PhaseAHighSide, + #[error("Phase A Low Side Fault")] + PhaseALowSide, + #[error("Phase B High Side Fault")] + PhaseBHighSide, + #[error("Phase B Low Side Fault")] + PhaseBLowSide, + #[error("Phase C High Side Fault")] + PhaseCHighSide, + #[error("Phase C Low Side Fault")] + PhaseCLowSide, +} + +impl DiagnosticRegister { + pub(super) const fn decode(value: u16) -> Self { + Self { + fault_flag: (value & 0b1000_0000_0000_0000) != 0, + power_on_reset: (value & 0b0100_0000_0000_0000) != 0, + serial_transfer_error: (value & 0b0010_0000_0000_0000) != 0, + + high_temperature_warning: (value & 0b0000_1000_0000_0000) != 0, + overtemperature_shutdown: (value & 0b0000_0100_0000_0000) != 0, + loss_of_synchronization: (value & 0b0000_0010_0000_0000) != 0, + undervoltage: (value & 0b0000_0000_1000_0000) != 0, + phase_a_high_side_fault: (value & 0b0000_0000_0010_0000) != 0, + phase_a_low_side_fault: (value & 0b0000_0000_0001_0000) != 0, + phase_b_high_side_fault: (value & 0b0000_0000_0000_1000) != 0, + phase_b_low_side_fault: (value & 0b0000_0000_0000_0100) != 0, + phase_c_high_side_fault: (value & 0b0000_0000_0000_0010) != 0, + phase_c_low_side_fault: (value & 0b0000_0000_0000_0001) != 0, + } + } + + pub fn assert_healthy(self) -> Result<()> { + if self.power_on_reset { + Err(DiagnosticRegisterError::PowerOnReset.into()) + } else if self.serial_transfer_error { + Err(DiagnosticRegisterError::SerialTransferError.into()) + } else if self.high_temperature_warning { + Err(DiagnosticRegisterError::HighTemperatureWarning.into()) + } else if self.overtemperature_shutdown { + Err(DiagnosticRegisterError::Overtemperature.into()) + } else if self.loss_of_synchronization { + Err(DiagnosticRegisterError::LossOfSynchronization.into()) + } else if self.undervoltage { + Err(DiagnosticRegisterError::Undervoltage.into()) + } else if self.phase_a_high_side_fault { + Err(DiagnosticRegisterError::PhaseAHighSide.into()) + } else if self.phase_a_low_side_fault { + Err(DiagnosticRegisterError::PhaseALowSide.into()) + } else if self.phase_b_high_side_fault { + Err(DiagnosticRegisterError::PhaseBHighSide.into()) + } else if self.phase_b_low_side_fault { + Err(DiagnosticRegisterError::PhaseBLowSide.into()) + } else if self.phase_c_high_side_fault { + Err(DiagnosticRegisterError::PhaseCHighSide.into()) + } else if self.phase_c_low_side_fault { + Err(DiagnosticRegisterError::PhaseCLowSide.into()) + } else if self.fault_flag { + // This shouldn't be possible + Err(DiagnosticRegisterError::UnknownFault.into()) + } else { + Ok(()) + } + } +} diff --git a/flight/src/hardware/channelization.rs b/flight/src/hardware/channelization.rs index f7950d5..bcc091b 100644 --- a/flight/src/hardware/channelization.rs +++ b/flight/src/hardware/channelization.rs @@ -7,38 +7,38 @@ use std::any::type_name; use std::fmt::Debug; use std::time::Instant; -pub const RCS5: PinoutChannel = PinoutChannel::ExtA(0); -pub const RCS6: PinoutChannel = PinoutChannel::ExtA(1); -pub const RCS7: PinoutChannel = PinoutChannel::ExtA(2); -pub const RCS8: PinoutChannel = PinoutChannel::ExtA(3); -pub const RCS9: PinoutChannel = PinoutChannel::ExtA(4); -pub const MOTOR0_A: PinoutChannel = PinoutChannel::ExtA(5); -pub const MOTOR0_B: PinoutChannel = PinoutChannel::ExtA(6); -pub const LED_A: PinoutChannel = PinoutChannel::ExtA(7); -pub const RCS0: PinoutChannel = PinoutChannel::ExtA(8); -pub const RCS1: PinoutChannel = PinoutChannel::ExtA(9); -pub const DRIVE0_BRAKE: PinoutChannel = PinoutChannel::ExtA(10); -pub const DRIVE0_DIR: PinoutChannel = PinoutChannel::ExtA(11); -pub const DRIVE0_DRIVEOFF: PinoutChannel = PinoutChannel::ExtA(12); -pub const RCS2: PinoutChannel = PinoutChannel::ExtA(13); -pub const RCS3: PinoutChannel = PinoutChannel::ExtA(14); -pub const RCS4: PinoutChannel = PinoutChannel::ExtA(15); -pub const MOTOR1_A: PinoutChannel = PinoutChannel::ExtB(8); -pub const MOTOR1_B: PinoutChannel = PinoutChannel::ExtB(9); -pub const MOTOR2_A: PinoutChannel = PinoutChannel::ExtB(10); -pub const MOTOR2_B: PinoutChannel = PinoutChannel::ExtB(11); -pub const MOTOR3_A: PinoutChannel = PinoutChannel::ExtB(12); -pub const MOTOR3_B: PinoutChannel = PinoutChannel::ExtB(13); -pub const OUT1: PinoutChannel = PinoutChannel::ExtB(14); -pub const OUT2: PinoutChannel = PinoutChannel::ExtB(15); -pub const OUT3: PinoutChannel = PinoutChannel::ExtB(0); -pub const OUT4: PinoutChannel = PinoutChannel::ExtB(1); -pub const OUT5: PinoutChannel = PinoutChannel::ExtB(2); -pub const OUT6: PinoutChannel = PinoutChannel::ExtB(3); -pub const OUT7: PinoutChannel = PinoutChannel::ExtB(4); -pub const OUT8: PinoutChannel = PinoutChannel::ExtB(5); -pub const OUT9: PinoutChannel = PinoutChannel::ExtB(6); -pub const LED_B: PinoutChannel = PinoutChannel::ExtB(7); +pub const OUT1: PinoutChannel = PinoutChannel::ExtA(0); +pub const OUT2: PinoutChannel = PinoutChannel::ExtA(1); +pub const OUT3: PinoutChannel = PinoutChannel::ExtA(2); +pub const OUT4: PinoutChannel = PinoutChannel::ExtA(3); +pub const OUT5: PinoutChannel = PinoutChannel::ExtA(4); +pub const OUT6: PinoutChannel = PinoutChannel::ExtA(5); +pub const OUT7: PinoutChannel = PinoutChannel::ExtA(6); +pub const OUT8: PinoutChannel = PinoutChannel::ExtA(7); +pub const LED_A: PinoutChannel = PinoutChannel::ExtA(8); +pub const MOTOR1_B: PinoutChannel = PinoutChannel::ExtA(9); +pub const MOTOR1_A: PinoutChannel = PinoutChannel::ExtA(10); +pub const MOTOR0_B: PinoutChannel = PinoutChannel::ExtA(11); +pub const MOTOR0_A: PinoutChannel = PinoutChannel::ExtA(12); +pub const DRIVE0_BRAKE: PinoutChannel = PinoutChannel::ExtA(13); +pub const DRIVE0_DIR: PinoutChannel = PinoutChannel::ExtA(14); +pub const DRIVE0_DRIVEOFF: PinoutChannel = PinoutChannel::ExtA(15); +pub const RCS1: PinoutChannel = PinoutChannel::ExtB(0); +pub const RCS0: PinoutChannel = PinoutChannel::ExtB(1); +pub const OUT9: PinoutChannel = PinoutChannel::ExtB(2); +pub const MOTOR3_B: PinoutChannel = PinoutChannel::ExtB(3); +pub const MOTOR3_A: PinoutChannel = PinoutChannel::ExtB(4); +pub const MOTOR2_B: PinoutChannel = PinoutChannel::ExtB(5); +pub const MOTOR2_A: PinoutChannel = PinoutChannel::ExtB(6); +pub const RCS9: PinoutChannel = PinoutChannel::ExtB(7); +pub const LED_B: PinoutChannel = PinoutChannel::ExtB(8); +pub const RCS8: PinoutChannel = PinoutChannel::ExtB(9); +pub const RCS6: PinoutChannel = PinoutChannel::ExtB(10); +pub const RCS7: PinoutChannel = PinoutChannel::ExtB(11); +pub const RCS4: PinoutChannel = PinoutChannel::ExtB(12); +pub const RCS5: PinoutChannel = PinoutChannel::ExtB(13); +pub const RCS2: PinoutChannel = PinoutChannel::ExtB(14); +pub const RCS3: PinoutChannel = PinoutChannel::ExtB(15); #[derive(Copy, Clone, Debug)] pub enum PinoutChannel { @@ -93,3 +93,15 @@ impl PinoutChannel { } } } + +#[derive(Copy, Clone, Debug)] +pub enum PininChannel { + In1 = 0, + In2 = 1, + In3 = 2, + In4 = 3, + In5 = 4, + BatterySense = 5, + In6 = 6, + In7 = 7, +} diff --git a/flight/src/hardware/mcp23017/driver.rs b/flight/src/hardware/mcp23017/driver.rs index d316031..8dcf14e 100644 --- a/flight/src/hardware/mcp23017/driver.rs +++ b/flight/src/hardware/mcp23017/driver.rs @@ -18,7 +18,7 @@ where i2c: Mutex, address: u8, bank: u16, - last_flushed: u16, + last_flushed: Option, } impl Debug for Mcp23017Driver @@ -54,7 +54,7 @@ where i2c: i2c.into(), address, bank: 0, - last_flushed: 0, + last_flushed: None, } } } @@ -112,14 +112,14 @@ where fn flush(&mut self) -> anyhow::Result<()> { trace!("Mcp23017Driver::flush(self: {self:?})"); - if self.bank != self.last_flushed { + if Some(self.bank) != self.last_flushed { let bytes = self.bank.to_le_bytes(); let data: [u8; _] = [0x12, bytes[0], bytes[1]]; // This blocks while writing if let Ok(mut lock) = self.i2c.lock() { lock.write(self.address, &data).map_err(WrappingError)?; self.i2c.clear_poison(); - self.last_flushed = self.bank; + self.last_flushed = Some(self.bank); } } Ok(()) diff --git a/flight/src/hardware/mcp23017/task.rs b/flight/src/hardware/mcp23017/task.rs index 82a9a85..9a51f28 100644 --- a/flight/src/hardware/mcp23017/task.rs +++ b/flight/src/hardware/mcp23017/task.rs @@ -69,10 +69,10 @@ struct AllPins { } impl AllPins { - fn new() -> Self { + fn new(default_states: [PinState; 16]) -> Self { trace!("AllPins::new()"); Self { - pins: array::repeat(PinData::new(PinState::Low)), + pins: array::from_fn(|i| PinData::new(default_states[i])), } } } @@ -80,11 +80,11 @@ impl AllPins { type PinData = CommandedState; impl<'a, M: Mcp23017 + Debug> Mcp23017Task<'a, M> { - pub fn new(mcp23017: M, state_vector: &'a StateVector) -> Self { + pub fn new(mcp23017: M, state_vector: &'a StateVector, _is_a: bool) -> Self { trace!("Mcp23017Task::new(mcp23017: {mcp23017:?})"); Self { mcp23017, - pins: AllPins::new(), + pins: AllPins::new(array::repeat(PinState::Low)), state: state_vector.create_section(Mcp23017State::default()), } } diff --git a/flight/src/hardware/mct8316a/driver.rs b/flight/src/hardware/mct8316a/driver.rs index 98e7873..b871310 100644 --- a/flight/src/hardware/mct8316a/driver.rs +++ b/flight/src/hardware/mct8316a/driver.rs @@ -57,8 +57,9 @@ use log::trace; use std::any::type_name; use std::fmt::{Debug, Display, Formatter}; use std::sync::Mutex; +use crate::CRC_8_CCITT; -const CRC: crc::Crc = crc::Crc::::new(&crc::CRC_8_SMBUS); +const CRC: crc::Crc = crc::Crc::::new(&CRC_8_CCITT); #[allow(dead_code)] #[derive(Copy, Clone, Eq, PartialEq, Debug)] @@ -171,15 +172,18 @@ where 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()); + write_data[4..6].copy_from_slice(&val.to_le_bytes()); 2 } Mct8316AVData::Four(val) => { - write_data[4..8].copy_from_slice(&val.to_be_bytes()); + write_data[4..8].copy_from_slice(&val.to_le_bytes()); 4 } Mct8316AVData::Eight(val) => { - write_data[4..12].copy_from_slice(&val.to_be_bytes()); + let first_half = ((val >> 32) & u32::MAX as u64) as u32; + let second_half = (val & u32::MAX as u64) as u32; + write_data[4..8].copy_from_slice(&first_half.to_le_bytes()); + write_data[8..12].copy_from_slice(&second_half.to_le_bytes()); 8 } }; @@ -239,21 +243,21 @@ where match data { Mct8316AVData::Two(val) => { - *val = u16::from_be_bytes([read_data[5], read_data[6]]); + *val = u16::from_le_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]]); + *val = u32::from_le_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], + *val = u64::from_le_bytes([ read_data[9], read_data[10], read_data[11], read_data[12], + read_data[5], + read_data[6], + read_data[7], + read_data[8], ]); } } diff --git a/flight/src/hardware/mod.rs b/flight/src/hardware/mod.rs index 3bef55b..a26e906 100644 --- a/flight/src/hardware/mod.rs +++ b/flight/src/hardware/mod.rs @@ -1,24 +1,29 @@ use crate::hardware::mcp23017::Mcp23017; -use crate::hardware::mct8316a::Mct8316a; use anyhow::Result; use embedded_hal::pwm::SetDutyCycle; use log::trace; use std::fmt::Debug; +use crate::hardware::a4963::A4963; pub trait Hardware { type Mcp23017<'a>: Mcp23017 + Send + Debug where Self: 'a; - type Pwm: SetDutyCycle + Sync; + type Pwm: SetDutyCycle + Sync + Send; fn new_mcp23017_a(&self) -> Result>; fn new_mcp23017_b(&self) -> Result>; fn new_pwm0(&self) -> Result; - fn new_mct8316a(&self) -> Result; + // fn new_mct8316a(&self) -> Result; + fn new_a4963<'a, 'b>(&'a mut self) -> Result ; fn get_battery_voltage(&self) -> Result; + + // fn get_fault(&self) -> Result; + + // fn get_fg(&self) -> Result; } #[cfg(feature = "raspi")] @@ -43,6 +48,7 @@ pub mod channelization; pub mod mcp23017; #[cfg(feature = "raspi")] mod mcp3208; -pub mod mct8316a; +// pub mod mct8316a; pub mod pin; mod sim; +pub mod a4963; diff --git a/flight/src/hardware/raspi/mod.rs b/flight/src/hardware/raspi/mod.rs index 08e3df0..99be530 100644 --- a/flight/src/hardware/raspi/mod.rs +++ b/flight/src/hardware/raspi/mod.rs @@ -3,28 +3,32 @@ mod pwm; use crate::hardware::Hardware; use crate::hardware::mcp3208::Mcp3208; use crate::hardware::mcp23017::Mcp23017Driver; -use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a}; use crate::hardware::raspi::pwm::PwmWrapper; -use crate::hardware::sim::mct8316a::SimMct8316a; -use anyhow::Result; +use anyhow::{anyhow, Result}; use embedded_hal_bus::i2c::MutexDevice; use log::{debug, info, trace}; -use rpi_pal::gpio::Gpio; use rpi_pal::i2c::I2c; use rpi_pal::pwm::Pwm; use rpi_pal::spi::SimpleHalSpiDevice; use rpi_pal::spi::{Bus, Mode, SlaveSelect, Spi}; use std::cell::RefCell; use std::sync::Mutex; +use rpi_pal::gpio::{Gpio, InputPin}; +use crate::hardware::a4963::{A4963Driver, A4963}; +use crate::hardware::channelization::PininChannel; const CLOCK_1MHZ: u32 = 1_000_000; #[derive(Debug)] pub struct RaspiHardware { - _gpio: Gpio, + // gpio: Gpio, + // fault: InputPin, + // fg: InputPin, i2c_bus: Mutex, mcp3208: RefCell>, - mct8316a: Mutex, + // a4963: RefCell>, + a4963: Option>, + // mct8316a: Mutex, } impl RaspiHardware { @@ -35,8 +39,17 @@ impl RaspiHardware { info!("Running on {}", device.model()); debug!("SOC: {}", device.soc()); + // let gpio = Gpio::new()?; + + // let mut fault = gpio.get(25)?.into_input(); + // fault.set_reset_on_drop(false); + // let mut fg = gpio.get(24)?.into_input(); + // fg.set_reset_on_drop(false); + Ok(Self { - _gpio: Gpio::new()?, + // gpio, + // fault, + // fg, i2c_bus: Mutex::new(I2c::with_bus(0u8)?), mcp3208: Mcp3208::new( SimpleHalSpiDevice::new(Spi::new( @@ -46,9 +59,14 @@ impl RaspiHardware { Mode::Mode0, )?), 3.3f64, - ) - .into(), - mct8316a: SimMct8316a::new().into(), + ).into(), + a4963: Some(A4963Driver::new(SimpleHalSpiDevice::new(Spi::new( + Bus::Spi0, + SlaveSelect::Ss0, + CLOCK_1MHZ, + Mode::Mode3, + )?))), + // mct8316a: Mct8316AVDriver::new().into(), // mct8316a: SimMct8316a::new().into(), }) } @@ -83,16 +101,36 @@ impl Hardware for RaspiHardware { Ok(PwmWrapper::new(Pwm::with_pwmchip(PWMCHIP, CHANNEL)?)?) } - fn new_mct8316a(&self) -> Result { - trace!("RaspiHardware::new_mct8316a(self: {self:?})"); - Ok(Mct8316AVDriver::new( - MutexDevice::new(&self.mct8316a), - 0b0000000, - )) + // fn new_mct8316a(&self) -> Result { + // trace!("RaspiHardware::new_mct8316a(self: {self:?})"); + // Ok(Mct8316AVDriver::new( + // MutexDevice::new(&self.mct8316a), + // // MutexDevice::new(&self.i2c_bus), + // 0b0000000, + // )) + // } + + fn new_a4963<'a, 'b>(&'a mut self) -> Result { + trace!("RaspiHardware::new_a4963(self: {self:?})"); + Ok(self.a4963.take().ok_or_else(|| anyhow!("Cannot construct two A4963 instances"))?) } fn get_battery_voltage(&self) -> Result { + const LOW_RESISTOR: f64 = 100.0; + const HIGH_RESISTOR: f64 = 374.0; trace!("RaspiHardware::get_battery_voltage(self: {self:?})"); - self.mcp3208.borrow_mut().read_single(1) + let sensed = self.mcp3208.borrow_mut().read_single(PininChannel::BatterySense as u8)?; + let battery = sensed * (HIGH_RESISTOR + LOW_RESISTOR) / LOW_RESISTOR; + Ok(battery) } + + // fn get_fault(&self) -> Result { + // trace!("RaspiHardware::get_fault(self: {self:?})"); + // Ok(self.fault.is_high()) + // } + + // fn get_fg(&self) -> Result { + // trace!("RaspiHardware::get_fg(self: {self:?})"); + // Ok(self.fg.is_high()) + // } } diff --git a/flight/src/hardware/sim/hardware.rs b/flight/src/hardware/sim/hardware.rs index 367e949..f819f9f 100644 --- a/flight/src/hardware/sim/hardware.rs +++ b/flight/src/hardware/sim/hardware.rs @@ -68,4 +68,12 @@ impl Hardware for SimHardware { trace!("SimHardware::get_battery_voltage(self: {self:?})"); Ok(self.battery_voltage) } + + // fn get_nfault(&self) -> anyhow::Result { + // Ok(true) + // } + // + // fn get_fg(&self) -> anyhow::Result { + // Ok(true) + // } } diff --git a/flight/src/hardware/sim/mct8316a.rs b/flight/src/hardware/sim/mct8316a.rs index 405deed..fb1a19c 100644 --- a/flight/src/hardware/sim/mct8316a.rs +++ b/flight/src/hardware/sim/mct8316a.rs @@ -3,8 +3,10 @@ use anyhow::anyhow; use embedded_hal::i2c::{ErrorType, I2c, Operation, SevenBitAddress}; use log::trace; use std::collections::HashMap; +use crate::CRC_8_CCITT; +use log::warn; -const CRC: crc::Crc = crc::Crc::::new(&crc::CRC_8_SMBUS); +const CRC: crc::Crc = crc::Crc::::new(&CRC_8_CCITT); #[derive(Debug)] pub struct SimMct8316a { @@ -67,7 +69,7 @@ impl I2c for SimMct8316a { if data_length == 2 { todo!("Unimplemented"); } else { - let written_value = u32::from_be_bytes([ + let written_value = u32::from_le_bytes([ write_buffer[3], write_buffer[4], write_buffer[5], @@ -85,6 +87,7 @@ impl I2c for SimMct8316a { 3 ))); } + crc.update(&[(i2c_addr << 1) | 0b0]); crc.update(write_buffer); } Operation::Read(read_buffer) => { @@ -104,7 +107,7 @@ impl I2c for SimMct8316a { todo!("Unimplemented"); } else if data_length == 4 { let value = *self.data.get(&address).unwrap_or(&0); - read_buffer[0..4].copy_from_slice(&value.to_be_bytes()); + read_buffer[0..4].copy_from_slice(&value.to_le_bytes()); } else { todo!("Unimplemented"); } diff --git a/flight/src/hardware/sim/mod.rs b/flight/src/hardware/sim/mod.rs index 339975a..26e9397 100644 --- a/flight/src/hardware/sim/mod.rs +++ b/flight/src/hardware/sim/mod.rs @@ -3,7 +3,8 @@ mod mcp23017; #[cfg(not(feature = "raspi"))] mod pwm; -pub(super) mod mct8316a; +// #[cfg(not(feature = "raspi"))] +// pub mod mct8316a; #[cfg(not(feature = "raspi"))] pub mod hardware; diff --git a/flight/src/lib.rs b/flight/src/lib.rs index 8119840..43a6f3f 100644 --- a/flight/src/lib.rs +++ b/flight/src/lib.rs @@ -1,26 +1,31 @@ #![warn(clippy::all, clippy::pedantic)] +use std::f64::consts::{PI, TAU}; use crate::comms::{CommsState, CommsTask}; use crate::hardware::Hardware; use crate::hardware::initialize; use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task}; -use crate::hardware::mct8316a::Mct8316a; use crate::hardware::pin::PinDevice; use crate::rcs::RcsTask; use crate::scheduler::Scheduler; use crate::state_vector::StateVector; use anyhow::Result; -use embedded_hal::pwm::SetDutyCycle; -use log::info; +use embedded_hal::pwm::{ErrorType, SetDutyCycle}; +use log::{error, info}; use nautilus_common::add_ctrlc_handler_arc; use nautilus_common::telemetry::{SwitchBank, TelemetryMessage}; use std::sync::Arc; use std::sync::atomic::{AtomicBool, Ordering}; use std::thread::sleep; use std::time::Duration; +use crc::Algorithm; +use nautilus_common::command::set_pwm::SetPwm; +use crate::hardware::a4963::A4963; mod hardware; +pub const CRC_8_CCITT: Algorithm = Algorithm { width: 8, poly: 0x07, init: 0xFF, refin: false, refout: false, xorout: 0x00, check: 0xfb, residue: 0xFF }; + fn new_shutdown_handler(running: &Arc) -> impl Fn(()) { let running = running.clone(); move |()| running.store(false, Ordering::Relaxed) @@ -41,32 +46,34 @@ pub fn run() -> Result<()> { let state_vector = StateVector::new(); - let hal = initialize()?; + let mut hal = initialize()?; + let mut a4963 = hal.new_a4963()?; 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)?; + pwm0.set_duty_cycle_percent(0)?; mcp23017_a.init()?; mcp23017_b.init()?; - mct8316.init()?; + a4963.init()?; + + pwm0.set_duty_cycle_percent(100)?; Scheduler::scope(running.clone(), |s| { let task_a = s.run_cyclic( "mcp23017-a-task", - Mcp23017Task::new(mcp23017_a, &state_vector), + Mcp23017Task::new(mcp23017_a, &state_vector, true), 10, )?; let a_id = task_a.get_id(); let task_b = s.run_cyclic( "mcp23017-b-task", - Mcp23017Task::new(mcp23017_b, &state_vector), + Mcp23017Task::new(mcp23017_b, &state_vector, false), 10, )?; let b_id = task_b.get_id(); @@ -78,6 +85,14 @@ pub fn run() -> Result<()> { comms.add_command_handler("/mcp23017a/set", task_a.new_set_pin_callback())?; comms.add_command_handler("/mcp23017b/set", task_b.new_set_pin_callback())?; comms.add_command_handler("/rcs/set", rcs.new_set_rcs_callback())?; + comms.add_command_handler("/pwm/set", move |pwm: SetPwm| { + match pwm0.set_duty_cycle_percent(pwm.duty_cycle) { + Ok(_) => {} + Err(err) => { + error!("Failed to set PWM duty cycle: {err}"); + } + } + })?; let comms = s.run_cyclic("comms-task", comms, 10)?; let comms_id = *comms; @@ -105,8 +120,10 @@ pub fn run() -> Result<()> { )?; info!("Starting Main Loop"); + while running.load(Ordering::Relaxed) { sleep(Duration::from_millis(100)); + a4963.check_health()?; } anyhow::Ok(()) @@ -116,8 +133,8 @@ pub fn run() -> Result<()> { // Explicitly drop these to allow the borrow checker to be sure that // dropping the hal is safe - drop(pwm0); - drop(mct8316); + // drop(pwm0); + // drop(mct8316); drop(hal); diff --git a/ground/src/command.rs b/ground/src/command.rs index 9e34001..432a829 100644 --- a/ground/src/command.rs +++ b/ground/src/command.rs @@ -18,8 +18,12 @@ use tokio::select; use tokio::sync::RwLock; use tokio::sync::mpsc::{Sender, channel}; use tokio_util::sync::CancellationToken; +use nautilus_common::command::set_pwm::SetPwm; const MAX_DATETIME: DateTime = DateTime::from_timestamp_nanos(i64::MAX); +const PIN_PRIORITY: u8 = 1; +const RCS_PRIORITY: u8 = 1; + pub struct CommandHandler<'a> { cmd: CommandRegistry, @@ -60,6 +64,18 @@ impl From for SetRcs { } } +#[derive(IntoCommandDefinition)] +struct PwmCommand { + duty_cycle: u8 +} +impl From for SetPwm { + fn from(value: PwmCommand) -> Self { + Self { + duty_cycle: value.duty_cycle + } + } +} + impl<'a> CommandHandler<'a> { pub fn new( client: Arc, @@ -101,7 +117,7 @@ impl<'a> CommandHandler<'a> { &ValidPriorityCommand { inner: SetPin::from(cmd), valid_until: MAX_DATETIME, - priority: 0, + priority: PIN_PRIORITY, } )?; @@ -132,7 +148,7 @@ impl<'a> CommandHandler<'a> { &ValidPriorityCommand { inner: SetRcs::from(cmd), valid_until: header.timestamp + TimeDelta::seconds(5), - priority: 0, + priority: RCS_PRIORITY, } )?; @@ -140,6 +156,17 @@ impl<'a> CommandHandler<'a> { })); } + { + let outgoing_commands_tx = outgoing_commands_tx.clone(); + commands.push(self.cmd.register_handler("pwm.set", move |_, cmd: PwmCommand| -> anyhow::Result<_> { + trace!("Sending Pwm Set Command"); + + outgoing_commands_tx.try_send_command("/pwm/set", &SetPwm::from(cmd))?; + + Ok("Command Executed Successfully".to_string()) + })); + } + // We no longer need this drop(outgoing_commands_tx);