sergeysav/ProjectNautilus
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

adds initial motor bravo implementation

Browse Source
This commit is contained in:
sergeysav
2026-08-19 19:24:28 -07:00
parent 47b1323af1
commit d9728b8680
25 changed files with 1081 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Cargo.lock
Generated
+27
View File
@@ -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"
Cargo.toml
+3
View File
@@ -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
avionics/avionics.kicad_pcb
+13
View File
@@ -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")
avionics/avionics.kicad_sch
+2 -2
View File
@@ -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)
avionics/eps.kicad_sch
+68 -28
View File
@@ -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)
common/src/command/mod.rs
+1
View File
@@ -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> {
common/src/command/set_pwm.rs
+9
View File
@@ -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 {}
config.txt
+2
View File
@@ -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
flight/Cargo.toml
+3
View File
@@ -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 }
flight/src/commanded_state.rs
+1 -1
View File
@@ -30,7 +30,7 @@ where
Self {
entries: [const { None }; N],
default,
changed: false,
changed: true,
}
}
flight/src/comms/mod.rs
+3 -3
View File
@@ -27,7 +27,7 @@ impl TelemetrySender {
}
}
type CommandCallback<'a> = Box<dyn Fn(&[u8]) -> Result<()> + Send + 'a>;
type CommandCallback<'a> = Box<dyn FnMut(&[u8]) -> Result<()> + Send + 'a>;
pub struct CommsTask<'a, A>
where
@@ -82,7 +82,7 @@ where
pub fn add_command_handler<T: Command>(
&mut self,
command: impl Into<String>,
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}");
flight/src/hardware/a4963/driver.rs
+145
View File
@@ -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: SPI,
mask_register: MaskRegister
}
impl<SPI> A4963Driver<SPI>
where
SPI: SpiDevice<u8>,
SPI::Error: Send,
SPI::Error: Sync,
SPI::Error: 'static, {
pub fn new(spi: SPI) -> Self {
trace!(
"A4963Driver<SPI={}>::new(spi)",
type_name::<SPI>()
);
Self {
spi,
mask_register: MaskRegister {
..Default::default()
},
}
}
fn update_mask_register(&mut self, mask_register: MaskRegister) -> Result<DiagnosticRegister> {
self.mask_register = mask_register;
self.write_verify(self.mask_register.clone())
}
fn read_diagnostic_register(&mut self) -> Result<DiagnosticRegister> {
self.write(self.mask_register.clone())
}
fn write<Register: A4963Register>(&mut self, register: Register) -> Result<DiagnosticRegister> {
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::<Register>());
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<Register: A4963Register>(&mut self) -> Result<Register> {
trace!("A4963Driver::read<{}>(self: {self:?})", type_name::<Register>());
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::<Register>());
DiagnosticRegister::decode(rx_data & 0b1110_0000_0000_0000)
.assert_healthy()?;
Ok(Register::decode(rx_data))
}
fn write_verify<Register: A4963Register>(&mut self, register: Register) -> Result<DiagnosticRegister> {
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<SPI> A4963 for A4963Driver<SPI>
where
SPI: SpiDevice<u8>,
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<SPI> Debug for A4963Driver<SPI> {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(
f,
"A4963Driver<SPI={}> {{ }}",
type_name::<SPI>(),
)
}
}
flight/src/hardware/a4963/mod.rs
+12
View File
@@ -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;
flight/src/hardware/a4963/register.rs
+587
View File
@@ -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(())
}
}
}
flight/src/hardware/channelization.rs
+44 -32
View File
@@ -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,
}
flight/src/hardware/mcp23017/driver.rs
+4 -4
View File
@@ -18,7 +18,7 @@ where
i2c: Mutex<I2C>,
address: u8,
bank: u16,
last_flushed: u16,
last_flushed: Option<u16>,
}
impl<I2C> Debug for Mcp23017Driver<I2C>
@@ -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(())
flight/src/hardware/mcp23017/task.rs
+4 -4
View File
@@ -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<PinState>;
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()),
}
}
flight/src/hardware/mct8316a/driver.rs
+15 -11
View File
@@ -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<u8> = crc::Crc::<u8>::new(&crc::CRC_8_SMBUS);
const CRC: crc::Crc<u8> = crc::Crc::<u8>::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],
]);
}
}
flight/src/hardware/mod.rs
+10 -4
View File
@@ -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<Error: std::error::Error + Sync + Send> + Sync;
type Pwm: SetDutyCycle<Error: std::error::Error + Sync + Send> + Sync + Send;
fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>>;
fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>>;
fn new_pwm0(&self) -> Result<Self::Pwm>;
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync>;
// fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync>;
fn new_a4963<'a, 'b>(&'a mut self) -> Result<impl A4963 + Send + 'b> ;
fn get_battery_voltage(&self) -> Result<f64>;
// fn get_fault(&self) -> Result<bool>;
// fn get_fg(&self) -> Result<bool>;
}
#[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;
flight/src/hardware/raspi/mod.rs
+55 -17
View File
@@ -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<I2c>,
mcp3208: RefCell<Mcp3208<SimpleHalSpiDevice>>,
mct8316a: Mutex<SimMct8316a>,
// a4963: RefCell<A4963Driver<SimpleHalSpiDevice>>,
a4963: Option<A4963Driver<SimpleHalSpiDevice>>,
// mct8316a: Mutex<SimMct8316a>,
}
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<impl Mct8316a + Sync> {
trace!("RaspiHardware::new_mct8316a(self: {self:?})");
Ok(Mct8316AVDriver::new(
MutexDevice::new(&self.mct8316a),
0b0000000,
))
// fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
// 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<impl A4963 + Send + 'b> {
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<f64> {
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<bool> {
// trace!("RaspiHardware::get_fault(self: {self:?})");
// Ok(self.fault.is_high())
// }
// fn get_fg(&self) -> Result<bool> {
// trace!("RaspiHardware::get_fg(self: {self:?})");
// Ok(self.fg.is_high())
// }
}
flight/src/hardware/sim/hardware.rs
+8
View File
@@ -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<bool> {
// Ok(true)
// }
//
// fn get_fg(&self) -> anyhow::Result<bool> {
// Ok(true)
// }
}
flight/src/hardware/sim/mct8316a.rs
+6 -3
View File
@@ -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<u8> = crc::Crc::<u8>::new(&crc::CRC_8_SMBUS);
const CRC: crc::Crc<u8> = crc::Crc::<u8>::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");
}
flight/src/hardware/sim/mod.rs
+2 -1
View File
@@ -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;
flight/src/lib.rs
+28 -11
View File
@@ -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<u8> = Algorithm { width: 8, poly: 0x07, init: 0xFF, refin: false, refout: false, xorout: 0x00, check: 0xfb, residue: 0xFF };
fn new_shutdown_handler(running: &Arc<AtomicBool>) -> 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);
ground/src/command.rs
+29 -2
View File
@@ -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<Utc> = 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<RcsCommand> for SetRcs {
}
}
#[derive(IntoCommandDefinition)]
struct PwmCommand {
duty_cycle: u8
}
impl From<PwmCommand> for SetPwm {
fn from(value: PwmCommand) -> Self {
Self {
duty_cycle: value.duty_cycle
}
}
}
impl<'a> CommandHandler<'a> {
pub fn new(
client: Arc<Client>,
@@ -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);