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

lint

Browse Source
This commit is contained in:
Sergey Savelyev
2025-10-26 09:24:49 -07:00
parent 5455935f3a
commit eefc3293b4
36 changed files with 886 additions and 516 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Cargo.toml
View File

@@ -1,3 +1,3 @@
[workspace] [workspace]
resolver = "3" resolver = "3"
members = ["common", "flight", "ground"] members = ["common", "ground", "flight"]

12
common/src/lib.rs
View File

@@ -1,12 +1,20 @@
#![warn(
clippy::all,
clippy::pedantic,
)]
use log::info; use log::info;
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::Arc; use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
pub mod logger;
pub mod command; pub mod command;
pub mod logger;
pub mod telemetry; pub mod telemetry;
pub mod udp; pub mod udp;
/// Add a ctrl-c handler which will set an atomic flag to `false` when ctrl-c is detected
///
/// # Errors
/// If a system error occurred while trying to set the ctrl-c handler
pub fn add_ctrlc_handler(flag: Arc<AtomicBool>) -> anyhow::Result<()> { pub fn add_ctrlc_handler(flag: Arc<AtomicBool>) -> anyhow::Result<()> {
ctrlc::set_handler(move || { ctrlc::set_handler(move || {
info!("Shutdown Requested"); info!("Shutdown Requested");

8
common/src/logger.rs
View File

@@ -5,6 +5,10 @@ use std::fs::create_dir_all;
use std::str::FromStr; use std::str::FromStr;
use std::{env, thread}; use std::{env, thread};
/// Set up the logger with a given package name
///
/// # Errors
/// If an error occurred while trying to set up the logger
pub fn setup_logger(package_name: &'static str) -> Result<()> { pub fn setup_logger(package_name: &'static str) -> Result<()> {
let log_file = env::var("LOG_FILE").or_else(|_| { let log_file = env::var("LOG_FILE").or_else(|_| {
create_dir_all("logs/")?; create_dir_all("logs/")?;
@@ -37,7 +41,7 @@ pub fn setup_logger(package_name: &'static str) -> Result<()> {
level = colors.color(record.level()), level = colors.color(record.level()),
time = chrono::Local::now().format("%Y-%m-%dT%H:%M:%S%.9f"), time = chrono::Local::now().format("%Y-%m-%dT%H:%M:%S%.9f"),
target = record.target(), target = record.target(),
)) ));
}) })
.chain( .chain(
fern::Dispatch::new() fern::Dispatch::new()
@@ -47,6 +51,6 @@ pub fn setup_logger(package_name: &'static str) -> Result<()> {
.chain(fern::log_file(log_file.clone())?) .chain(fern::log_file(log_file.clone())?)
.apply()?; .apply()?;
debug!("Logging to {} at level {}", log_file, log_level); debug!("Logging to {log_file} at level {log_level}");
Ok(()) Ok(())
} }

2
common/src/telemetry/mod.rs
View File

@@ -20,5 +20,5 @@ pub enum TelemetryMessage {
SwitchState { SwitchState {
bank: SwitchBank, bank: SwitchBank,
switches: [bool; 16], switches: [bool; 16],
} },
} }

94
common/src/udp.rs
View File

@@ -1,7 +1,7 @@
use crate::udp::UdpSendCborError::LengthMismatch; use crate::udp::UdpSendCborError::LengthMismatch;
use log::error; use log::error;
use serde::de::DeserializeOwned;
use serde::Serialize; use serde::Serialize;
use serde::de::DeserializeOwned;
use std::io::{Cursor, ErrorKind}; use std::io::{Cursor, ErrorKind};
use std::net::{SocketAddr, ToSocketAddrs, UdpSocket}; use std::net::{SocketAddr, ToSocketAddrs, UdpSocket};
use thiserror::Error; use thiserror::Error;
@@ -23,58 +23,76 @@ pub enum UdpSendCborError {
#[error("Serialization Error")] #[error("Serialization Error")]
Serialization(#[from] ciborium::ser::Error<std::io::Error>), Serialization(#[from] ciborium::ser::Error<std::io::Error>),
#[error("Length Mismatch")] #[error("Length Mismatch")]
LengthMismatch { LengthMismatch { expected: usize, actual: usize },
expected: usize,
actual: usize,
},
} }
pub trait UdpSocketExt { pub trait UdpSocketExt {
fn recv_cbor<T: DeserializeOwned, const N: usize>(&self, buffer: &mut Cursor<[u8; N]>) -> Result<(T, SocketAddr), UdpRecvCborError>;
fn send_cbor<T: Serialize + ?Sized, A: ToSocketAddrs, const N: usize>(&self, data: &T, buffer: &mut Cursor<[u8; N]>, addr: A) -> Result<(), UdpSendCborError>; /// Receive a CBOR encoded message from this UDP Socket
///
/// # Errors
/// An error that could have occurred while trying to receive this message.
/// If no data was received a `UdpRecvCborError::NoData` error would be returned.
fn recv_cbor<T: DeserializeOwned, const N: usize>(
&self,
buffer: &mut Cursor<[u8; N]>,
) -> Result<(T, SocketAddr), UdpRecvCborError>;
/// Send a CBOR encoded message to an address using this socket
///
/// # Errors
/// An error that could have occurred while trying to send this message
fn send_cbor<T: Serialize + ?Sized, A: ToSocketAddrs, const N: usize>(
&self,
data: &T,
buffer: &mut Cursor<[u8; N]>,
addr: A,
) -> Result<(), UdpSendCborError>;
} }
impl UdpSocketExt for UdpSocket { impl UdpSocketExt for UdpSocket {
fn recv_cbor<T: DeserializeOwned, const N: usize>(&self, buffer: &mut Cursor<[u8; N]>) -> Result<(T, SocketAddr), UdpRecvCborError> { fn recv_cbor<T: DeserializeOwned, const N: usize>(
&self,
buffer: &mut Cursor<[u8; N]>,
) -> Result<(T, SocketAddr), UdpRecvCborError> {
buffer.set_position(0); buffer.set_position(0);
match self.recv_from(buffer.get_mut()) { match self.recv_from(buffer.get_mut()) {
Ok((size, addr)) => { Ok((size, addr)) => match ciborium::from_reader::<T, _>(&buffer.get_ref()[..size]) {
match ciborium::from_reader::<T, _>(&buffer.get_ref()[..size]) { Ok(res) => Ok((res, addr)),
Ok(res) => Ok((res, addr)), Err(err) => Err(err.into()),
Err(err) => Err(err.into()), },
} Err(err) => match err.kind() {
} ErrorKind::WouldBlock | ErrorKind::TimedOut => Err(UdpRecvCborError::NoData),
Err(err) => { _ => Err(err.into()),
match err.kind() { },
ErrorKind::WouldBlock | ErrorKind::TimedOut => {
Err(UdpRecvCborError::NoData)
}
_ => Err(err.into())
}
}
} }
} }
fn send_cbor<T: Serialize + ?Sized, A: ToSocketAddrs, const N: usize>(&self, data: &T, mut buffer: &mut Cursor<[u8; N]>, addr: A) -> Result<(), UdpSendCborError> { fn send_cbor<T: Serialize + ?Sized, A: ToSocketAddrs, const N: usize>(
&self,
data: &T,
mut buffer: &mut Cursor<[u8; N]>,
addr: A,
) -> Result<(), UdpSendCborError> {
buffer.set_position(0); buffer.set_position(0);
match ciborium::into_writer(data, &mut buffer) { match ciborium::into_writer(data, &mut buffer) {
Ok(_) => match self.send_to(&buffer.get_ref()[..buffer.position() as usize], addr) { Ok(()) => {
Ok(size) => { let size_encoded = usize::try_from(buffer.position())
if buffer.position() as usize != size { .expect("Values greater than u32 are not expected anyway");
return Err(LengthMismatch { match self.send_to(&buffer.get_ref()[..size_encoded], addr) {
expected: buffer.position() as usize, Ok(size_sent) => {
actual: size, if size_encoded != size_sent {
}); return Err(LengthMismatch {
expected: size_encoded,
actual: size_sent,
});
}
Ok(())
} }
Ok(()) Err(e) => Err(e.into()),
} }
Err(e) => { },
Err(e.into()) Err(e) => Err(e.into()),
}
}
Err(e) => {
Err(e.into())
}
} }
} }
} }

34
flight/src/comms/mod.rs
View File

@@ -8,9 +8,9 @@ use std::any::type_name;
use std::fmt::Debug; use std::fmt::Debug;
use std::io::Cursor; use std::io::Cursor;
use std::net::{IpAddr, Ipv4Addr, SocketAddr, ToSocketAddrs, UdpSocket}; use std::net::{IpAddr, Ipv4Addr, SocketAddr, ToSocketAddrs, UdpSocket};
use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::mpsc::Receiver; use std::sync::mpsc::Receiver;
use std::sync::Arc;
use std::time::Instant; use std::time::Instant;
pub type TelemetrySender = TaskHandle<Telemetry, ()>; pub type TelemetrySender = TaskHandle<Telemetry, ()>;
@@ -34,12 +34,11 @@ pub struct CommsTask<A: ToSocketAddrs + Debug> {
} }
impl<A: ToSocketAddrs + Debug> CommsTask<A> { impl<A: ToSocketAddrs + Debug> CommsTask<A> {
pub fn new( pub fn new(local_port: u16, ground_address: A, running: Arc<AtomicBool>) -> Result<Self> {
local_port: u16, trace!(
ground_address: A, "CommsTask::new<A={}>(local_port: {local_port}, ground_address: {ground_address:?})",
running: Arc<AtomicBool>, type_name::<A>()
) -> Result<Self> { );
trace!("CommsTask::new<A={}>(local_port: {local_port}, ground_address: {ground_address:?})", type_name::<A>());
let bind_addr = SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), local_port); let bind_addr = SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), local_port);
// let bind_addr = SocketAddr::new(IpAddr::V6(Ipv6Addr::UNSPECIFIED), local_port); // let bind_addr = SocketAddr::new(IpAddr::V6(Ipv6Addr::UNSPECIFIED), local_port);
let udp = UdpSocket::bind(bind_addr)?; let udp = UdpSocket::bind(bind_addr)?;
@@ -57,20 +56,23 @@ impl<A: ToSocketAddrs + Debug> CyclicTask for CommsTask<A> {
type Data = (); type Data = ();
fn get_data(&self) -> Self::Data { fn get_data(&self) -> Self::Data {
trace!("CommsTask<A={}>::get_data(self: {self:?})", type_name::<A>()); trace!(
() "CommsTask<A={}>::get_data(self: {self:?})",
type_name::<A>()
);
} }
fn step(&mut self, receiver: &Receiver<Self::Message>, _step_time: Instant) { fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant) {
trace!("CommsTask<A={}>::step(self: {self:?}, receiver: {receiver:?}, _step_time: {_step_time:?})", type_name::<A>()); trace!(
"CommsTask<A={}>::step(self: {self:?}, receiver: {receiver:?}, step_time: {step_time:?})",
type_name::<A>()
);
let mut buffer = Cursor::new([0u8; 512]); let mut buffer = Cursor::new([0u8; 512]);
match self.udp.recv_cbor::<Command, _>(&mut buffer) { match self.udp.recv_cbor::<Command, _>(&mut buffer) {
Ok((cmd, _)) => { Ok((cmd, _)) => match cmd {
match cmd { Command::Shutdown => self.running.store(false, Ordering::Relaxed),
Command::Shutdown => self.running.store(false, Ordering::Relaxed), },
}
}
Err(UdpRecvCborError::NoData) => {} Err(UdpRecvCborError::NoData) => {}
Err(err) => { Err(err) => {
error!("Rx error: {err}"); error!("Rx error: {err}");

28
flight/src/hardware/channelization.rs
View File

@@ -1,7 +1,6 @@
#![allow(dead_code)] #![allow(dead_code)]
use crate::hardware::pin::{Pin, PinDevice}; use crate::hardware::pin::{Pin, PinDevice};
use anyhow::Result;
use embedded_hal::digital::PinState; use embedded_hal::digital::PinState;
use log::trace; use log::trace;
use std::any::type_name; use std::any::type_name;
@@ -52,9 +51,12 @@ pub struct DevicePin<'a, Device: PinDevice> {
device: &'a Device, device: &'a Device,
} }
impl<'a, Device: PinDevice> Pin for DevicePin<'a, Device> { impl<Device: PinDevice> Pin for DevicePin<'_, Device> {
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) { fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
trace!("ChannelPin<Device={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})", type_name::<Device>()); trace!(
"ChannelPin<Device={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})",
type_name::<Device>()
);
self.device.set_pin(self.pin, value, valid_until, priority); self.device.set_pin(self.pin, value, valid_until, priority);
} }
} }
@@ -64,9 +66,13 @@ pub enum ChannelPin<'a, A: PinDevice, B: PinDevice> {
ExtB(DevicePin<'a, B>), ExtB(DevicePin<'a, B>),
} }
impl<'a, A: PinDevice, B: PinDevice> Pin for ChannelPin<'a, A, B> { impl<A: PinDevice, B: PinDevice> Pin for ChannelPin<'_, A, B> {
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) { fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
trace!("ChannelPin<A={}, B={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})", type_name::<A>(), type_name::<B>()); trace!(
"ChannelPin<A={}, B={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})",
type_name::<A>(),
type_name::<B>()
);
match self { match self {
ChannelPin::ExtA(pin) => pin.set(value, valid_until, priority), ChannelPin::ExtA(pin) => pin.set(value, valid_until, priority),
ChannelPin::ExtB(pin) => pin.set(value, valid_until, priority), ChannelPin::ExtB(pin) => pin.set(value, valid_until, priority),
@@ -75,11 +81,15 @@ impl<'a, A: PinDevice, B: PinDevice> Pin for ChannelPin<'a, A, B> {
} }
impl PinoutChannel { impl PinoutChannel {
pub fn new<'a>(self, ext_a: &'a (impl PinDevice + Debug), ext_b: &'a (impl PinDevice + Debug)) -> Result<impl Pin> { pub fn get_pin<'a>(
trace!("PinoutChannel::new(self: {self:?}, ext_a: {ext_a:?}, ext_b: {ext_b:?}"); self,
Ok(match self { ext_a: &'a (impl PinDevice + Debug),
ext_b: &'a (impl PinDevice + Debug),
) -> impl Pin {
trace!("PinoutChannel::get_pin(self: {self:?}, ext_a: {ext_a:?}, ext_b: {ext_b:?}");
match self {
PinoutChannel::ExtA(pin) => ChannelPin::ExtA(DevicePin { pin, device: ext_a }), PinoutChannel::ExtA(pin) => ChannelPin::ExtA(DevicePin { pin, device: ext_a }),
PinoutChannel::ExtB(pin) => ChannelPin::ExtB(DevicePin { pin, device: ext_b }), PinoutChannel::ExtB(pin) => ChannelPin::ExtB(DevicePin { pin, device: ext_b }),
}) }
} }
} }

14
flight/src/hardware/mcp23017/driver.rs
View File

@@ -29,7 +29,12 @@ where
I2C::Error: 'static, I2C::Error: 'static,
{ {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(f, "Mcp23017Driver<I2C={}> {{ address: {} }}", type_name::<I2C>(), self.address) write!(
f,
"Mcp23017Driver<I2C={}> {{ address: {} }}",
type_name::<I2C>(),
self.address
)
} }
} }
@@ -41,7 +46,10 @@ where
I2C::Error: 'static, I2C::Error: 'static,
{ {
pub fn new(i2c: I2C, address: u8) -> Self { pub fn new(i2c: I2C, address: u8) -> Self {
trace!("Mcp23017Driver<I2C={}>::new(i2c, address: 0x{address:02x})", type_name::<I2C>()); trace!(
"Mcp23017Driver<I2C={}>::new(i2c, address: 0x{address:02x})",
type_name::<I2C>()
);
Self { Self {
i2c: i2c.into(), i2c: i2c.into(),
address, address,
@@ -62,7 +70,7 @@ where
trace!("Mcp23017Driver::drop(self: {self:?})"); trace!("Mcp23017Driver::drop(self: {self:?})");
self.bank = 0; // We want all pins to be set back to 0 self.bank = 0; // We want all pins to be set back to 0
if let Err(e) = self.flush() { if let Err(e) = self.flush() {
error!("Mcp23017Driver: Failed to flush on drop. {self:?} Error: {e}") error!("Mcp23017Driver: Failed to flush on drop. {self:?} Error: {e}");
} }
} }
} }

2
flight/src/hardware/mcp23017/mod.rs
View File

@@ -1,5 +1,5 @@
mod task;
mod driver; mod driver;
mod task;
use anyhow::Result; use anyhow::Result;
use embedded_hal::digital::PinState; use embedded_hal::digital::PinState;

53
flight/src/hardware/mcp23017/task.rs
View File

@@ -107,37 +107,45 @@ impl PinData {
// Do this twice to check both the current and the current next // Do this twice to check both the current and the current next
// If the current is currently invalid, we'd upgrade the next to current // If the current is currently invalid, we'd upgrade the next to current
for _ in 0..2 { for _ in 0..2 {
let is_current_valid = self.valid_until.map(|current| current >= now).unwrap_or(false); let is_current_valid = self
.valid_until
.is_some_and(|current| current >= now);
if is_current_valid { if is_current_valid {
self.value = self.state; self.value = self.state;
return; return;
} else {
if self.valid_until.is_some() {
self.changed = true;
}
self.state = self.next_state;
self.valid_until = self.next_validity;
self.priority = self.next_priority;
self.next_validity = None;
self.next_priority = 0;
} }
if self.valid_until.is_some() {
self.changed = true;
}
self.state = self.next_state;
self.valid_until = self.next_validity;
self.priority = self.next_priority;
self.next_validity = None;
self.next_priority = 0;
} }
self.value = self.default; self.value = self.default;
} }
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) { fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
trace!("PinData::set(self: {self:?}, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})"); trace!(
let can_replace_current = self.valid_until.map(|current| current <= valid_until).unwrap_or(true); "PinData::set(self: {self:?}, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})"
let can_replace_next = self.next_validity.map(|next| next <= valid_until).unwrap_or(true); );
let can_replace_current = self
.valid_until
.is_none_or(|current| current <= valid_until);
let can_replace_next = self
.next_validity
.is_none_or(|next| next <= valid_until);
if priority >= self.priority { if priority >= self.priority {
// This is now the highest priority thing (or most recent of equal priority) // This is now the highest priority thing (or most recent of equal priority)
if can_replace_current { if can_replace_current {
if can_replace_next { if can_replace_next {
self.next_validity = None; self.next_validity = None;
self.next_priority = 0 self.next_priority = 0;
} }
} else { } else {
self.next_state = self.state; self.next_state = self.state;
@@ -182,15 +190,11 @@ impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
fn get_data(&self) -> Self::Data { fn get_data(&self) -> Self::Data {
trace!("Mcp23017Task::get_data(self: {self:?})"); trace!("Mcp23017Task::get_data(self: {self:?})");
Self::Data { Self::Data {
id: self.state.get_identifier() id: self.state.get_identifier(),
} }
} }
fn step( fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant) {
&mut self,
receiver: &Receiver<Self::Message>,
step_time: Instant,
) {
trace!("Mcp23017Task::step(self: {self:?}, receiver, step_time: {step_time:?})"); trace!("Mcp23017Task::step(self: {self:?}, receiver, step_time: {step_time:?})");
let mut changed = false; let mut changed = false;
@@ -200,7 +204,12 @@ impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
while let Ok(recv) = receiver.try_recv() { while let Ok(recv) = receiver.try_recv() {
match recv { match recv {
Mcp23017Message::SetPin { pin, value, valid_until, priority } => { Mcp23017Message::SetPin {
pin,
value,
valid_until,
priority,
} => {
if (0u8..16u8).contains(&pin) { if (0u8..16u8).contains(&pin) {
self.pins.pins[pin as usize].set(value, valid_until, priority); self.pins.pins[pin as usize].set(value, valid_until, priority);
} }

25
flight/src/hardware/mcp3208/mod.rs
View File

@@ -1,5 +1,5 @@
use crate::hardware::error::WrappingError; use crate::hardware::error::WrappingError;
use anyhow::{ensure, Result}; use anyhow::{Result, ensure};
use embedded_hal::spi::SpiDevice; use embedded_hal::spi::SpiDevice;
use log::trace; use log::trace;
use std::any::type_name; use std::any::type_name;
@@ -12,7 +12,12 @@ pub struct Mcp3208<SPI> {
impl<SPI> Debug for Mcp3208<SPI> { impl<SPI> Debug for Mcp3208<SPI> {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(f, "Mcp3208<SPI={}> {{ vref: {} }}", type_name::<SPI>(), self.vref) write!(
f,
"Mcp3208<SPI={}> {{ vref: {} }}",
type_name::<SPI>(),
self.vref
)
} }
} }
@@ -24,11 +29,11 @@ where
SPI::Error: 'static, SPI::Error: 'static,
{ {
pub fn new(spi: SPI, vref: f64) -> Self { pub fn new(spi: SPI, vref: f64) -> Self {
trace!("Mcp3208<SPI={}>::new(spi, vref: {vref})", type_name::<SPI>()); trace!(
Self { "Mcp3208<SPI={}>::new(spi, vref: {vref})",
spi, type_name::<SPI>()
vref, );
} Self { spi, vref }
} }
pub fn read_single(&mut self, channel: u8) -> Result<f64> { pub fn read_single(&mut self, channel: u8) -> Result<f64> {
@@ -41,10 +46,12 @@ where
write_bits[1] |= (channel.unbounded_shl(6)) & 0xFF; write_bits[1] |= (channel.unbounded_shl(6)) & 0xFF;
let mut read_bits = [0u8; 3]; let mut read_bits = [0u8; 3];
self.spi.transfer(&mut read_bits, &write_bits).map_err(WrappingError)?; self.spi
.transfer(&mut read_bits, &write_bits)
.map_err(WrappingError)?;
let value: u16 = u16::from_be_bytes([read_bits[1], read_bits[2]]) & 0x0FFF; let value: u16 = u16::from_be_bytes([read_bits[1], read_bits[2]]) & 0x0FFF;
Ok(((value as f64) / (0xFFF as f64)) * self.vref) Ok(((value as f64) / (0xFFF as f64)) * self.vref)
} }
} }

3
flight/src/hardware/mct8316a/closed_loop.rs
View File

@@ -249,7 +249,6 @@ pub enum DutyCycleThreshold {
Percent2_5 = 0x7, Percent2_5 = 0x7,
} }
#[derive(Debug, Clone)] #[derive(Debug, Clone)]
pub struct ClosedLoop3 { pub struct ClosedLoop3 {
pub degauss_samples: DegaussSamples, pub degauss_samples: DegaussSamples,
@@ -481,5 +480,3 @@ pub enum FastBrakeDelta {
Percent4 = 0x6, Percent4 = 0x6,
Percent5 = 0x7, Percent5 = 0x7,
} }

1
flight/src/hardware/mct8316a/constant_power.rs
View File

@@ -35,4 +35,3 @@ pub enum PowerMode {
ClosedLoop = 0x1, ClosedLoop = 0x1,
PowerLimit = 0x2, PowerLimit = 0x2,
} }

150
flight/src/hardware/mct8316a/driver.rs
View File

@@ -1,18 +1,57 @@
use crate::hardware::error::WrappingError; use crate::hardware::error::WrappingError;
use crate::hardware::mct8316a::closed_loop::{BemfThreshold, ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4, ClosedLoopDecelerationMode, ClosedLoopRate, CommutationMethod, CommutationMode, DegaussLowerBound, DegaussSamples, DegaussUpperBound, DegaussWindow, DutyCycleThreshold, FastBrakeDelta, IntegrationBemfThreshold, IntegrationCycleHighThreshold, IntegrationCycleLowThreshold, IntegrationDutyCycleThreshold, LeadAnglePolarity, LowerPercentLimit, MotorStopBrakeTime, MotorStopMode, PwmFrequency, PwmMode, PwmModulation, SpeedFeedbackConfig, SpeedFeedbackDivision, SpeedFeedbackMode, UpperPercentLimit}; use crate::hardware::mct8316a::Mct8316a;
use crate::hardware::mct8316a::closed_loop::{
BemfThreshold, ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4, ClosedLoopDecelerationMode,
ClosedLoopRate, CommutationMethod, CommutationMode, DegaussLowerBound, DegaussSamples,
DegaussUpperBound, DegaussWindow, DutyCycleThreshold, FastBrakeDelta, IntegrationBemfThreshold,
IntegrationCycleHighThreshold, IntegrationCycleLowThreshold, IntegrationDutyCycleThreshold,
LeadAnglePolarity, LowerPercentLimit, MotorStopBrakeTime, MotorStopMode, PwmFrequency, PwmMode,
PwmModulation, SpeedFeedbackConfig, SpeedFeedbackDivision, SpeedFeedbackMode,
UpperPercentLimit,
};
use crate::hardware::mct8316a::constant_power::{ConstantPower, PowerHysteresis, PowerMode}; use crate::hardware::mct8316a::constant_power::{ConstantPower, PowerHysteresis, PowerMode};
use crate::hardware::mct8316a::constant_speed::{ClosedLoopMode, ConstantSpeed}; use crate::hardware::mct8316a::constant_speed::{ClosedLoopMode, ConstantSpeed};
use crate::hardware::mct8316a::device_config::{ClockSource, DeviceConfig, DeviceMode, ExternalClockFrequency, PwmRangeSelect}; use crate::hardware::mct8316a::device_config::{
ClockSource, DeviceConfig, DeviceMode, ExternalClockFrequency, PwmRangeSelect,
};
use crate::hardware::mct8316a::eeprom::Mct8316AVEeprom; use crate::hardware::mct8316a::eeprom::Mct8316AVEeprom;
use crate::hardware::mct8316a::fault_config::{AbnormalSpeedLock, AbnormalSpeedLockThreshold, AutomaticRetries, CycleByCycleCurrentLimit, FaultConfig1, FaultConfig2, LockDetectionCurrentLimit, LockDetectionCurrentLimitDeglitchTime, LockMinSpeed, LockMode, LockRetryTime, LossSyncTimes, MaxMotorVoltage, MinMotorVoltage, MotorVoltageMode, NoMotorDetectDeglitchTime, NoMotorThreshold, ZeroDutyThreshold}; use crate::hardware::mct8316a::fault_config::{
use crate::hardware::mct8316a::gate_driver_config::{BuckCurrentLimit, BuckSlewRate, BuckVoltageSelection, CurrentSenseAmplifier, DemagComparatorThreshold, GateDriverConfig1, GateDriverConfig2, OvercurrentFaultMode, OvercurrentProtectionDeglitchTime, OvercurrentProtectionLevel, OvercurrentProtectionRetryTime, OvervoltageProtectionLevel, SlewRate, TargetDelay}; AbnormalSpeedLock, AbnormalSpeedLockThreshold, AutomaticRetries, CycleByCycleCurrentLimit,
use crate::hardware::mct8316a::isd_config::{BrakeConfig, BrakeMode, IsdConfig, IsdConfigTimeValue, ResyncMinimumThreshold, StartupBreakTime, Threshold}; FaultConfig1, FaultConfig2, LockDetectionCurrentLimit, LockDetectionCurrentLimitDeglitchTime,
use crate::hardware::mct8316a::motor_startup::{AlignRampRate, AlignTime, DutyCycle, FirstCycleFrequencySelect, FullCurrentThreshold, IpdAdvanceAngle, IpdClockFrequency, IpdCurrentThreshold, IpdReleaseMode, IpdRepeat, MinimumDutyCycle, MotorStartup1, MotorStartup2, MotorStartupMethod, OpenClosedHandoffThreshold, OpenLoopAcceleration1, OpenLoopAcceleration2, OpenLoopCurrentLimitMode, SlowFirstCycleFrequency}; LockMinSpeed, LockMode, LockRetryTime, LossSyncTimes, MaxMotorVoltage, MinMotorVoltage,
use crate::hardware::mct8316a::phase_profile::{ProfileSetting, ThreePhase150DegreeProfile, ThreePhaseLeadAngle, TwoPhase150DegreeProfile}; MotorVoltageMode, NoMotorDetectDeglitchTime, NoMotorThreshold, ZeroDutyThreshold,
use crate::hardware::mct8316a::pin_config::{BrakeInputConfig, DirectionInputConfig, ExternalWatchdogFaultMode, ExternalWatchdogFrequency, ExternalWatchdogSource, Pin36Config, Pin37_38Config, PinConfig1, PinConfig2, SleepTime, SpeedInputConfig}; };
use crate::hardware::mct8316a::trap_config::{AVSLimitHysteresis, AVSNegativeCurrentLimit, FastStartupDivFactor, IsdBemfThreshold, IsdCycleThreshold, OpenLoopHandoffCycles, OpenLoopZcDetectionThreshold, TrapConfig1, TrapConfig2}; use crate::hardware::mct8316a::gate_driver_config::{
use crate::hardware::mct8316a::Mct8316a; BuckCurrentLimit, BuckSlewRate, BuckVoltageSelection, CurrentSenseAmplifier,
use anyhow::{bail, ensure, Result}; DemagComparatorThreshold, GateDriverConfig1, GateDriverConfig2, OvercurrentFaultMode,
OvercurrentProtectionDeglitchTime, OvercurrentProtectionLevel, OvercurrentProtectionRetryTime,
OvervoltageProtectionLevel, SlewRate, TargetDelay,
};
use crate::hardware::mct8316a::isd_config::{
BrakeConfig, BrakeMode, IsdConfig, IsdConfigTimeValue, ResyncMinimumThreshold,
StartupBreakTime, Threshold,
};
use crate::hardware::mct8316a::motor_startup::{
AlignRampRate, AlignTime, DutyCycle, FirstCycleFrequencySelect, FullCurrentThreshold,
IpdAdvanceAngle, IpdClockFrequency, IpdCurrentThreshold, IpdReleaseMode, IpdRepeat,
MinimumDutyCycle, MotorStartup1, MotorStartup2, MotorStartupMethod, OpenClosedHandoffThreshold,
OpenLoopAcceleration1, OpenLoopAcceleration2, OpenLoopCurrentLimitMode,
SlowFirstCycleFrequency,
};
use crate::hardware::mct8316a::phase_profile::{
ProfileSetting, ThreePhase150DegreeProfile, ThreePhaseLeadAngle, TwoPhase150DegreeProfile,
};
use crate::hardware::mct8316a::pin_config::{
BrakeInputConfig, DirectionInputConfig, ExternalWatchdogFaultMode, ExternalWatchdogFrequency,
ExternalWatchdogSource, Pin36Config, Pin37_38Config, PinConfig1, PinConfig2, SleepTime,
SpeedInputConfig,
};
use crate::hardware::mct8316a::trap_config::{
AVSLimitHysteresis, AVSNegativeCurrentLimit, FastStartupDivFactor, IsdBemfThreshold,
IsdCycleThreshold, OpenLoopHandoffCycles, OpenLoopZcDetectionThreshold, TrapConfig1,
TrapConfig2,
};
use anyhow::{Result, bail, ensure};
use embedded_hal::i2c::{I2c, Operation}; use embedded_hal::i2c::{I2c, Operation};
use log::trace; use log::trace;
use std::any::type_name; use std::any::type_name;
@@ -39,7 +78,7 @@ impl Display for Mct8316AVData {
} }
} }
#[derive(Debug)] #[derive(Debug, Copy, Clone)]
enum OperationRW { enum OperationRW {
Read, Read,
Write, Write,
@@ -51,17 +90,16 @@ fn control_word(
data: Mct8316AVData, data: Mct8316AVData,
address: u32, address: u32,
) -> [u8; 3] { ) -> [u8; 3] {
trace!("control_word(operation_rw: {operation_rw:?}, crc: {crc}, data: {data}, address: {address:06x})"); trace!(
"control_word(operation_rw: {operation_rw:?}, crc: {crc}, data: {data}, address: {address:06x})"
);
let mut control_word = [0u8; _]; let mut control_word = [0u8; _];
control_word[0] |= match operation_rw { control_word[0] |= match operation_rw {
OperationRW::Read => 0x80, OperationRW::Read => 0x80,
OperationRW::Write => 0x00, OperationRW::Write => 0x00,
}; };
control_word[0] |= match crc { control_word[0] |= if crc { 0x40 } else { 0x00 };
true => 0x40,
false => 0x00,
};
control_word[0] |= match data { control_word[0] |= match data {
Mct8316AVData::Two(_) => 0x00, Mct8316AVData::Two(_) => 0x00,
Mct8316AVData::Four(_) => 0x10, Mct8316AVData::Four(_) => 0x10,
@@ -69,7 +107,7 @@ fn control_word(
}; };
control_word[0] |= ((address >> 16) & 0x0F) as u8; control_word[0] |= ((address >> 16) & 0x0F) as u8;
control_word[1] |= ((address >> 8) & 0xFF) as u8; control_word[1] |= ((address >> 8) & 0xFF) as u8;
control_word[2] |= ((address >> 0) & 0xFF) as u8; control_word[2] |= ((address) & 0xFF) as u8;
control_word control_word
} }
@@ -93,7 +131,12 @@ where
I2C::Error: 'static, I2C::Error: 'static,
{ {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(f, "Mct8316AVDriver<I2C={}> {{ address: {} }}", type_name::<I2C>(), self.address) write!(
f,
"Mct8316AVDriver<I2C={}> {{ address: {} }}",
type_name::<I2C>(),
self.address
)
} }
} }
@@ -105,13 +148,18 @@ where
I2C::Error: 'static, I2C::Error: 'static,
{ {
pub fn new(i2c: I2C, address: u8) -> Self { pub fn new(i2c: I2C, address: u8) -> Self {
trace!("Mct8316AVDriver<I2C={}>::new(i2c, address: 0x{address:02x})", type_name::<I2C>()); trace!(
"Mct8316AVDriver<I2C={}>::new(i2c, address: 0x{address:02x})",
type_name::<I2C>()
);
Self { Self {
i2c: i2c.into(), i2c: i2c.into(),
address, address,
} }
} }
#[allow(clippy::identity_op)]
pub(super) fn write(&self, address: u32, data: Mct8316AVData) -> Result<()> { pub(super) fn write(&self, address: u32, data: Mct8316AVData) -> Result<()> {
trace!("Mct8316AVDriver::write(self: {self:?}, address: {address:06x}, data: {data})"); trace!("Mct8316AVDriver::write(self: {self:?}, address: {address:06x}, data: {data})");
@@ -121,12 +169,7 @@ where
// 1 for crc // 1 for crc
let mut write_data = [0u8; 1 + 3 + 8 + 1]; let mut write_data = [0u8; 1 + 3 + 8 + 1];
write_data[0] = (self.address << 1) | 0b0; write_data[0] = (self.address << 1) | 0b0;
write_data[1..4].copy_from_slice(&control_word( write_data[1..4].copy_from_slice(&control_word(OperationRW::Write, true, data, address));
OperationRW::Write,
true,
data,
address,
));
let data_length = match data { let data_length = match data {
Mct8316AVData::Two(val) => { Mct8316AVData::Two(val) => {
write_data[4..6].copy_from_slice(&val.to_be_bytes()); write_data[4..6].copy_from_slice(&val.to_be_bytes());
@@ -145,13 +188,16 @@ where
write_data[4 + data_length] = crc_result; write_data[4 + data_length] = crc_result;
match self.i2c.lock() { match self.i2c.lock() {
Ok(mut lock) => lock.write(self.address, &write_data[1..(4 + data_length + 1)]).map_err(WrappingError)?, Ok(mut lock) => lock
.write(self.address, &write_data[1..=(4 + data_length)])
.map_err(WrappingError)?,
Err(_) => bail!("Lock was poisoned"), Err(_) => bail!("Lock was poisoned"),
} }
Ok(()) Ok(())
} }
#[allow(clippy::identity_op)]
pub(super) fn read(&self, address: u32, data: &mut Mct8316AVData) -> Result<()> { pub(super) fn read(&self, address: u32, data: &mut Mct8316AVData) -> Result<()> {
trace!("Mct8316AVDriver::read(self: {self:?}, address: {address:06x}, data: {data})"); trace!("Mct8316AVDriver::read(self: {self:?}, address: {address:06x}, data: {data})");
// 1 target id // 1 target id
@@ -160,13 +206,9 @@ where
// 8 data bytes // 8 data bytes
// 1 crc // 1 crc
let mut read_data = [0u8; 1 + 3 + 1 + 8 + 1]; let mut read_data = [0u8; 1 + 3 + 1 + 8 + 1];
// Least significant bit should be 0
read_data[0] = (self.address << 1) | 0b0; read_data[0] = (self.address << 1) | 0b0;
read_data[1..4].copy_from_slice(&control_word( read_data[1..4].copy_from_slice(&control_word(OperationRW::Read, true, *data, address));
OperationRW::Read,
true,
*data,
address,
));
read_data[4] = (self.address << 1) | 0b1; read_data[4] = (self.address << 1) | 0b1;
let data_length = match data { let data_length = match data {
Mct8316AVData::Two(_) => 2, Mct8316AVData::Two(_) => 2,
@@ -178,32 +220,42 @@ where
let mut i2c_ops = [ let mut i2c_ops = [
Operation::Write(&left[1..4]), Operation::Write(&left[1..4]),
Operation::Read(&mut right[..(data_length + 1)]) Operation::Read(&mut right[..=data_length]),
]; ];
match self.i2c.lock() { match self.i2c.lock() {
Ok(mut lock) => lock.transaction(self.address, &mut i2c_ops).map_err(WrappingError)?, Ok(mut lock) => lock
.transaction(self.address, &mut i2c_ops)
.map_err(WrappingError)?,
Err(_) => bail!("Lock was poisoned"), Err(_) => bail!("Lock was poisoned"),
} }
drop(i2c_ops);
let expected_crc = CRC.checksum(&read_data[0..(5 + data_length)]); let expected_crc = CRC.checksum(&read_data[0..(5 + data_length)]);
ensure!(expected_crc == read_data[5+data_length], "CRC Mismatch. {expected_crc} expected. {}", read_data[5+data_length]); ensure!(
expected_crc == read_data[5 + data_length],
"CRC Mismatch. {expected_crc} expected. {}",
read_data[5 + data_length]
);
match data { match data {
Mct8316AVData::Two(val) => { Mct8316AVData::Two(val) => {
*val = u16::from_be_bytes([read_data[5], read_data[6]]); *val = u16::from_be_bytes([read_data[5], read_data[6]]);
} }
Mct8316AVData::Four(val) => { Mct8316AVData::Four(val) => {
*val = u32::from_be_bytes([read_data[5], read_data[6], *val = u32::from_be_bytes([read_data[5], read_data[6], read_data[7], read_data[8]]);
read_data[7], read_data[8]]);
} }
Mct8316AVData::Eight(val) => { Mct8316AVData::Eight(val) => {
*val = u64::from_be_bytes([read_data[5], read_data[6], *val = u64::from_be_bytes([
read_data[7], read_data[8], read_data[5],
read_data[9], read_data[10], read_data[6],
read_data[11], read_data[12]]); read_data[7],
read_data[8],
read_data[9],
read_data[10],
read_data[11],
read_data[12],
]);
} }
} }
@@ -218,6 +270,7 @@ where
I2C::Error: Sync, I2C::Error: Sync,
I2C::Error: 'static, I2C::Error: 'static,
{ {
#[allow(clippy::too_many_lines)]
fn init(&mut self) -> Result<()> { fn init(&mut self) -> Result<()> {
trace!("Mct8316AVDriver::init(self: {self:?})"); trace!("Mct8316AVDriver::init(self: {self:?})");
@@ -312,7 +365,7 @@ where
})? })?
.set_constant_speed(ConstantSpeed { .set_constant_speed(ConstantSpeed {
speed_power_kp: 0.001f64, speed_power_kp: 0.001f64,
speed_power_ki: 0.000005f64, speed_power_ki: 0.000_005_f64,
speed_power_upper_limit: UpperPercentLimit::Percent100, speed_power_upper_limit: UpperPercentLimit::Percent100,
speed_power_lower_limit: LowerPercentLimit::Percent10, speed_power_lower_limit: LowerPercentLimit::Percent10,
mode: ClosedLoopMode::Disabled, mode: ClosedLoopMode::Disabled,
@@ -365,10 +418,12 @@ where
})? })?
.set_fault_config1(FaultConfig1 { .set_fault_config1(FaultConfig1 {
no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime::Milliseconds100, no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime::Milliseconds100,
cycle_by_cycle_current_limit: CycleByCycleCurrentLimit::RecoverNextPwmFaultInactiveRecirculation, cycle_by_cycle_current_limit:
CycleByCycleCurrentLimit::RecoverNextPwmFaultInactiveRecirculation,
lock_detection_current_limit: LockDetectionCurrentLimit::Volts1_4, lock_detection_current_limit: LockDetectionCurrentLimit::Volts1_4,
lock_detection_current_limit_mode: LockMode::ReportOnly, lock_detection_current_limit_mode: LockMode::ReportOnly,
lock_detection_current_limit_deglitch_time: LockDetectionCurrentLimitDeglitchTime::Milliseconds75, lock_detection_current_limit_deglitch_time:
LockDetectionCurrentLimitDeglitchTime::Milliseconds75,
cycle_by_cycle_pwm_limit: 0, cycle_by_cycle_pwm_limit: 0,
motor_lock_mode: LockMode::RecoverRetryTristated, motor_lock_mode: LockMode::RecoverRetryTristated,
lock_retry_time: LockRetryTime::Milliseconds5000, lock_retry_time: LockRetryTime::Milliseconds5000,
@@ -421,7 +476,8 @@ where
overvoltage_protection_level: OvervoltageProtectionLevel::Volts32, overvoltage_protection_level: OvervoltageProtectionLevel::Volts32,
overvoltage_protection_enable: true, overvoltage_protection_enable: true,
overtemperature_warning_enable: false, overtemperature_warning_enable: false,
overcurrent_protection_deglitch_time: OvercurrentProtectionDeglitchTime::Microsecond0_2, overcurrent_protection_deglitch_time:
OvercurrentProtectionDeglitchTime::Microsecond0_2,
overcurrent_protection_retry_time: OvercurrentProtectionRetryTime::Millisecond5, overcurrent_protection_retry_time: OvercurrentProtectionRetryTime::Millisecond5,
overcurrent_protection_level: OvercurrentProtectionLevel::Amps16, overcurrent_protection_level: OvercurrentProtectionLevel::Amps16,
overcurrent_fault_mode: OvercurrentFaultMode::Latch, overcurrent_fault_mode: OvercurrentFaultMode::Latch,

569
flight/src/hardware/mct8316a/eeprom.rs
View File

@@ -1,3 +1,4 @@
use crate::hardware::mct8316a::Mct8316AVDriver;
use crate::hardware::mct8316a::closed_loop::{ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4}; use crate::hardware::mct8316a::closed_loop::{ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4};
use crate::hardware::mct8316a::constant_power::ConstantPower; use crate::hardware::mct8316a::constant_power::ConstantPower;
use crate::hardware::mct8316a::constant_speed::ConstantSpeed; use crate::hardware::mct8316a::constant_speed::ConstantSpeed;
@@ -7,10 +8,11 @@ use crate::hardware::mct8316a::fault_config::{FaultConfig1, FaultConfig2};
use crate::hardware::mct8316a::gate_driver_config::{GateDriverConfig1, GateDriverConfig2}; use crate::hardware::mct8316a::gate_driver_config::{GateDriverConfig1, GateDriverConfig2};
use crate::hardware::mct8316a::isd_config::IsdConfig; use crate::hardware::mct8316a::isd_config::IsdConfig;
use crate::hardware::mct8316a::motor_startup::{MotorStartup1, MotorStartup2}; use crate::hardware::mct8316a::motor_startup::{MotorStartup1, MotorStartup2};
use crate::hardware::mct8316a::phase_profile::{ThreePhase150DegreeProfile, TwoPhase150DegreeProfile}; use crate::hardware::mct8316a::phase_profile::{
ThreePhase150DegreeProfile, TwoPhase150DegreeProfile,
};
use crate::hardware::mct8316a::pin_config::{PinConfig1, PinConfig2}; use crate::hardware::mct8316a::pin_config::{PinConfig1, PinConfig2};
use crate::hardware::mct8316a::trap_config::{TrapConfig1, TrapConfig2}; use crate::hardware::mct8316a::trap_config::{TrapConfig1, TrapConfig2};
use crate::hardware::mct8316a::Mct8316AVDriver;
use anyhow::Result; use anyhow::Result;
use embedded_hal::i2c::I2c; use embedded_hal::i2c::I2c;
use log::trace; use log::trace;
@@ -30,7 +32,7 @@ where
modified: bool, modified: bool,
} }
impl<'a, I2C> Debug for Mct8316AVEeprom<'a, I2C> impl<I2C> Debug for Mct8316AVEeprom<'_, I2C>
where where
I2C: I2c + Send + Sync, I2C: I2c + Send + Sync,
I2C::Error: Send, I2C::Error: Send,
@@ -38,10 +40,18 @@ where
I2C::Error: 'static, I2C::Error: 'static,
{ {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(f, "Mct8316AVEeprom<I2C={}> {{ driver: {:?}, modified: {} }}", type_name::<I2C>(), self.driver, self.modified) write!(
f,
"Mct8316AVEeprom<I2C={}> {{ driver: {:?}, modified: {} }}",
type_name::<I2C>(),
self.driver,
self.modified
)
} }
} }
// We intentionally pass by value in here
#[allow(clippy::needless_pass_by_value)]
impl<'a, I2C> Mct8316AVEeprom<'a, I2C> impl<'a, I2C> Mct8316AVEeprom<'a, I2C>
where where
I2C: I2c + Send + Sync, I2C: I2c + Send + Sync,
@@ -50,9 +60,12 @@ where
I2C::Error: 'static, I2C::Error: 'static,
{ {
pub fn load(driver: &'a mut Mct8316AVDriver<I2C>) -> Result<Self> { pub fn load(driver: &'a mut Mct8316AVDriver<I2C>) -> Result<Self> {
trace!("Mct8316AVEeprom<I2C={}>::load(driver: {driver:?})", type_name::<I2C>()); trace!(
"Mct8316AVEeprom<I2C={}>::load(driver: {driver:?})",
type_name::<I2C>()
);
driver.write(0x0000E6, Mct8316AVData::Four(0x40000000))?; driver.write(0x00_00E6, Mct8316AVData::Four(0x4000_0000))?;
// Wait 100ms for the EEPROM operation to complete // Wait 100ms for the EEPROM operation to complete
sleep(Duration::from_millis(100)); sleep(Duration::from_millis(100));
@@ -65,75 +78,105 @@ where
pub fn set_isd_config(self, isd_config: IsdConfig) -> Result<Self> { pub fn set_isd_config(self, isd_config: IsdConfig) -> Result<Self> {
trace!("Mct8316AVEeprom::set_isd_config(self: {self:?}, isd_config: {isd_config:?})"); trace!("Mct8316AVEeprom::set_isd_config(self: {self:?}, isd_config: {isd_config:?})");
let expected_value = let expected_value = if isd_config.enable_isd { 0x4000_0000 } else { 0 }
if isd_config.enable_isd { 0x40000000 } else { 0 } | if isd_config.enable_brake {
| if isd_config.enable_brake { 0x20000000 } else { 0 } 0x2000_0000
| if isd_config.enable_high_impedance { 0x10000000 } else { 0 } } else {
| if isd_config.enable_reverse_drive { 0x08000000 } else { 0 } 0
| if isd_config.enable_resynchronization { 0x04000000 } else { 0 } }
| if isd_config.enable_stationary_brake { 0x02000000 } else { 0 } | if isd_config.enable_high_impedance {
| (((isd_config.stationary_detect_threshold as u32) & 0x7) << 22) 0x1000_0000
| ((isd_config.brake_mode as u32) & 0x1) << 21 } else {
| ((isd_config.brake_config as u32) & 0x1) << 20 0
| ((isd_config.brake_current_threshold as u32) & 0x7) << 17 }
| ((isd_config.brake_time as u32) & 0xF) << 13 | if isd_config.enable_reverse_drive {
| ((isd_config.high_impedence_time as u32) & 0xF) << 9 0x0800_0000
| ((isd_config.startup_break_time as u32) & 0x7) << 6 } else {
| ((isd_config.resync_minimum_threshold as u32) & 0x7) << 3; 0
}
| if isd_config.enable_resynchronization {
0x0400_0000
} else {
0
}
| if isd_config.enable_stationary_brake {
0x0200_0000
} else {
0
}
| (((isd_config.stationary_detect_threshold as u32) & 0x7) << 22)
| ((isd_config.brake_mode as u32) & 0x1) << 21
| ((isd_config.brake_config as u32) & 0x1) << 20
| ((isd_config.brake_current_threshold as u32) & 0x7) << 17
| ((isd_config.brake_time as u32) & 0xF) << 13
| ((isd_config.high_impedence_time as u32) & 0xF) << 9
| ((isd_config.startup_break_time as u32) & 0x7) << 6
| ((isd_config.resync_minimum_threshold as u32) & 0x7) << 3;
self.assert_register(0x80, Mct8316AVData::Four(expected_value)) self.assert_register(0x80, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_motor_startup1(self, motor_startup1: MotorStartup1) -> Result<Self> { pub fn set_motor_startup1(self, motor_startup1: MotorStartup1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_motor_startup1(self: {self:?}, motor_startup1: {motor_startup1:?})"); trace!(
"Mct8316AVEeprom::set_motor_startup1(self: {self:?}, motor_startup1: {motor_startup1:?})"
);
let expected_value = let expected_value = ((motor_startup1.motor_startup_method as u32) & 0x3) << 29
((motor_startup1.motor_startup_method as u32) & 0x3) << 29 | ((motor_startup1.align_ramp_rate as u32) & 0xF) << 25
| ((motor_startup1.align_ramp_rate as u32) & 0xF) << 25 | ((motor_startup1.align_time as u32) & 0xF) << 21
| ((motor_startup1.align_time as u32) & 0xF) << 21 | ((motor_startup1.align_current_threshold as u32) & 0xF) << 17
| ((motor_startup1.align_current_threshold as u32) & 0xF) << 17 | ((motor_startup1.ipd_clock_frequency as u32) & 0x7) << 14
| ((motor_startup1.ipd_clock_frequency as u32) & 0x7) << 14 | ((motor_startup1.ipd_current_threshold as u32) & 0xF) << 10
| ((motor_startup1.ipd_current_threshold as u32) & 0xF) << 10 | ((motor_startup1.ipd_release_mode as u32) & 0x3) << 8
| ((motor_startup1.ipd_release_mode as u32) & 0x3) << 8 | ((motor_startup1.ipd_advance_angle as u32) & 0x3) << 6
| ((motor_startup1.ipd_advance_angle as u32) & 0x3) << 6 | ((motor_startup1.ipd_repeat as u32) & 0x3) << 4
| ((motor_startup1.ipd_repeat as u32) & 0x3) << 4 | ((motor_startup1.slow_first_cycle_frequency as u32) & 0xF) << 0;
| ((motor_startup1.slow_first_cycle_frequency as u32) & 0xF) << 0;
self.assert_register(0x82, Mct8316AVData::Four(expected_value)) self.assert_register(0x82, Mct8316AVData::Four(expected_value))
} }
pub fn set_motor_startup2(self, motor_startup2: MotorStartup2) -> Result<Self> { pub fn set_motor_startup2(self, motor_startup2: MotorStartup2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_motor_startup2(self: {self:?}, motor_startup2: {motor_startup2:?})"); trace!(
"Mct8316AVEeprom::set_motor_startup2(self: {self:?}, motor_startup2: {motor_startup2:?})"
);
let expected_value = let expected_value = ((motor_startup2.open_loop_current_limit_mode as u32) & 0x1) << 30
((motor_startup2.open_loop_current_limit_mode as u32) & 0x1) << 30 | ((motor_startup2.open_loop_duty_cycle as u32) & 0x7) << 27
| ((motor_startup2.open_loop_duty_cycle as u32) & 0x7) << 27 | ((motor_startup2.open_loop_current_limit as u32) & 0xF) << 23
| ((motor_startup2.open_loop_current_limit as u32) & 0xF) << 23 | ((motor_startup2.open_loop_acceleration1 as u32) & 0x1F) << 18
| ((motor_startup2.open_loop_acceleration1 as u32) & 0x1F) << 18 | ((motor_startup2.open_loop_acceleration2 as u32) & 0x1F) << 13
| ((motor_startup2.open_loop_acceleration2 as u32) & 0x1F) << 13 | ((motor_startup2.open_closed_handoff_threshold as u32) & 0x1F) << 8
| ((motor_startup2.open_closed_handoff_threshold as u32) & 0x1F) << 8 | if motor_startup2.auto_handoff_enable {
| if motor_startup2.auto_handoff_enable { 1u32 << 7 } else { 0u32 } 1u32 << 7
| ((motor_startup2.first_cycle_frequency_select as u32) & 0x1) << 6 } else {
| ((motor_startup2.minimum_duty_cycle as u32) & 0xF) << 2; 0u32
}
| ((motor_startup2.first_cycle_frequency_select as u32) & 0x1) << 6
| ((motor_startup2.minimum_duty_cycle as u32) & 0xF) << 2;
self.assert_register(0x84, Mct8316AVData::Four(expected_value)) self.assert_register(0x84, Mct8316AVData::Four(expected_value))
} }
// The casts in here are save due to clamps
#[allow(clippy::cast_possible_truncation)]
#[allow(clippy::cast_sign_loss)]
pub fn set_closed_loop1(self, closed_loop1: ClosedLoop1) -> Result<Self> { pub fn set_closed_loop1(self, closed_loop1: ClosedLoop1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_closed_loop1(self: {self:?}, closed_loop1: {closed_loop1:?})"); trace!("Mct8316AVEeprom::set_closed_loop1(self: {self:?}, closed_loop1: {closed_loop1:?})");
let lead_angle = (closed_loop1.lead_angle / 0.12f32).round().clamp(0.0f32, u8::MAX as f32) as u8; let lead_angle = (closed_loop1.lead_angle / 0.12f32)
.round()
.clamp(0.0f32, f32::from(u8::MAX)) as u8;
let expected_value = let expected_value = ((closed_loop1.commutation_mode as u32) & 0x2) << 29
((closed_loop1.commutation_mode as u32) & 0x2) << 29 | ((closed_loop1.closed_loop_acceleration_rate as u32) & 0x1F) << 24
| ((closed_loop1.closed_loop_acceleration_rate as u32) & 0x1F) << 24 | ((closed_loop1.closed_loop_deceleration_mode as u32) & 0x1) << 23
| ((closed_loop1.closed_loop_deceleration_mode as u32) & 0x1) << 23 | ((closed_loop1.closed_loop_deceleration_rate as u32) & 0x1F) << 18
| ((closed_loop1.closed_loop_deceleration_rate as u32) & 0x1F) << 18 | ((closed_loop1.pwm_frequency as u32) & 0x1F) << 13
| ((closed_loop1.pwm_frequency as u32) & 0x1F) << 13 | ((closed_loop1.pwm_modulation as u32) & 0x3) << 11
| ((closed_loop1.pwm_modulation as u32) & 0x3) << 11 | ((closed_loop1.pwm_mode as u32) & 0x1) << 10
| ((closed_loop1.pwm_mode as u32) & 0x1) << 10 | ((closed_loop1.lead_angle_polarity as u32) & 0x1) << 9
| ((closed_loop1.lead_angle_polarity as u32) & 0x1) << 9 | u32::from(lead_angle) << 1;
| (lead_angle as u32) << 1;
self.assert_register(0x86, Mct8316AVData::Four(expected_value)) self.assert_register(0x86, Mct8316AVData::Four(expected_value))
} }
@@ -141,17 +184,20 @@ where
pub fn set_closed_loop2(self, closed_loop2: ClosedLoop2) -> Result<Self> { pub fn set_closed_loop2(self, closed_loop2: ClosedLoop2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_closed_loop2(self: {self:?}, closed_loop2: {closed_loop2:?})"); trace!("Mct8316AVEeprom::set_closed_loop2(self: {self:?}, closed_loop2: {closed_loop2:?})");
let expected_value = let expected_value = ((closed_loop2.speed_feedback_mode as u32) & 0x2) << 29
((closed_loop2.speed_feedback_mode as u32) & 0x2) << 29 | ((closed_loop2.speed_feedback_division as u32) & 0xF) << 25
| ((closed_loop2.speed_feedback_division as u32) & 0xF) << 25 | ((closed_loop2.speed_feedback_config as u32) & 0x1) << 24
| ((closed_loop2.speed_feedback_config as u32) & 0x1) << 24 | ((closed_loop2.bemf_threshold as u32) & 0x7) << 21
| ((closed_loop2.bemf_threshold as u32) & 0x7) << 21 | ((closed_loop2.motor_stop_mode as u32) & 0x7) << 18
| ((closed_loop2.motor_stop_mode as u32) & 0x7) << 18 | ((closed_loop2.motor_stop_brake_time as u32) & 0xF) << 14
| ((closed_loop2.motor_stop_brake_time as u32) & 0xF) << 14 | ((closed_loop2.active_low_high_brake_threshold as u32) & 0x7) << 11
| ((closed_loop2.active_low_high_brake_threshold as u32) & 0x7) << 11 | ((closed_loop2.brake_pin_threshold as u32) & 0x7) << 8
| ((closed_loop2.brake_pin_threshold as u32) & 0x7) << 8 | if closed_loop2.avs_enable {
| if closed_loop2.avs_enable { 1u32 << 7 } else { 0u32 } 1u32 << 7
| ((closed_loop2.cycle_current_limit as u32) & 0xF) << 3; } else {
0u32
}
| ((closed_loop2.cycle_current_limit as u32) & 0xF) << 3;
self.assert_register(0x88, Mct8316AVData::Four(expected_value)) self.assert_register(0x88, Mct8316AVData::Four(expected_value))
} }
@@ -159,67 +205,101 @@ where
pub fn set_closed_loop3(self, closed_loop3: ClosedLoop3) -> Result<Self> { pub fn set_closed_loop3(self, closed_loop3: ClosedLoop3) -> Result<Self> {
trace!("Mct8316AVEeprom::set_closed_loop3(self: {self:?}, closed_loop3: {closed_loop3:?})"); trace!("Mct8316AVEeprom::set_closed_loop3(self: {self:?}, closed_loop3: {closed_loop3:?})");
let expected_value = let expected_value = ((closed_loop3.degauss_samples as u32) & 0x2) << 29
((closed_loop3.degauss_samples as u32) & 0x2) << 29 | ((closed_loop3.degauss_upper_bound as u32) & 0x3) << 27
| ((closed_loop3.degauss_upper_bound as u32) & 0x3) << 27 | ((closed_loop3.degauss_lower_bound as u32) & 0x3) << 25
| ((closed_loop3.degauss_lower_bound as u32) & 0x3) << 25 | ((closed_loop3.integration_cycle_low_threshold as u32) & 0x3) << 23
| ((closed_loop3.integration_cycle_low_threshold as u32) & 0x3) << 23 | ((closed_loop3.integration_cycle_high_threshold as u32) & 0x3) << 21
| ((closed_loop3.integration_cycle_high_threshold as u32) & 0x3) << 21 | ((closed_loop3.integration_duty_cycle_low_threshold as u32) & 0x3) << 19
| ((closed_loop3.integration_duty_cycle_low_threshold as u32) & 0x3) << 19 | ((closed_loop3.integration_duty_cycle_high_threshold as u32) & 0x3) << 17
| ((closed_loop3.integration_duty_cycle_high_threshold as u32) & 0x3) << 17 | ((closed_loop3.bemf_threshold2 as u32) & 0x3F) << 11
| ((closed_loop3.bemf_threshold2 as u32) & 0x3F) << 11 | ((closed_loop3.bemf_threshold1 as u32) & 0x3F) << 5
| ((closed_loop3.bemf_threshold1 as u32) & 0x3F) << 5 | ((closed_loop3.commutation_method as u32) & 0x1) << 4
| ((closed_loop3.commutation_method as u32) & 0x1) << 4 | ((closed_loop3.degauss_window as u32) & 0x7) << 1
| ((closed_loop3.degauss_window as u32) & 0x7) << 1 | u32::from(closed_loop3.degauss_enable);
| if closed_loop3.degauss_enable { 1 } else { 0u32 };
self.assert_register(0x8A, Mct8316AVData::Four(expected_value)) self.assert_register(0x8A, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_closed_loop4(self, closed_loop4: ClosedLoop4) -> Result<Self> { pub fn set_closed_loop4(self, closed_loop4: ClosedLoop4) -> Result<Self> {
trace!("Mct8316AVEeprom::set_closed_loop4(self: {self:?}, closed_loop4: {closed_loop4:?})"); trace!("Mct8316AVEeprom::set_closed_loop4(self: {self:?}, closed_loop4: {closed_loop4:?})");
let expected_value = let expected_value = if closed_loop4.wcomp_blanking_enable {
if closed_loop4.wcomp_blanking_enable { 1u32 << 19 } else { 0u32 } 1u32 << 19
| ((closed_loop4.fast_deceleration_duty_window as u32) & 0x7) << 16 } else {
| ((closed_loop4.fast_deceleration_duty_threshold as u32) & 0x7) << 13 0u32
| ((closed_loop4.dynamic_brake_current_lower_threshold as u32) & 0x7) << 9 } | ((closed_loop4.fast_deceleration_duty_window as u32) & 0x7) << 16
| if closed_loop4.dynamic_braking_current_enable { 1u32 << 8 } else { 0u32 } | ((closed_loop4.fast_deceleration_duty_threshold as u32) & 0x7) << 13
| if closed_loop4.fast_deceleration_enable { 1u32 << 7 } else { 0u32 } | ((closed_loop4.dynamic_brake_current_lower_threshold as u32) & 0x7) << 9
| ((closed_loop4.fast_deceleration_current_threshold as u32) & 0xF) << 3 | if closed_loop4.dynamic_braking_current_enable {
| ((closed_loop4.fast_brake_delta as u32) & 0x7) << 0; 1u32 << 8
} else {
0u32
}
| if closed_loop4.fast_deceleration_enable {
1u32 << 7
} else {
0u32
}
| ((closed_loop4.fast_deceleration_current_threshold as u32) & 0xF) << 3
| ((closed_loop4.fast_brake_delta as u32) & 0x7) << 0;
self.assert_register(0x8C, Mct8316AVData::Four(expected_value)) self.assert_register(0x8C, Mct8316AVData::Four(expected_value))
} }
// The casts in here are save due to clamps
#[allow(clippy::cast_possible_truncation)]
#[allow(clippy::cast_sign_loss)]
// These names come from the ICD
#[allow(clippy::similar_names)]
pub fn set_constant_speed(self, constant_speed: ConstantSpeed) -> Result<Self> { pub fn set_constant_speed(self, constant_speed: ConstantSpeed) -> Result<Self> {
trace!("Mct8316AVEeprom::set_constant_speed(self: {self:?}, constant_speed: {constant_speed:?})"); trace!(
"Mct8316AVEeprom::set_constant_speed(self: {self:?}, constant_speed: {constant_speed:?})"
);
let speed_power_kp = (constant_speed.speed_power_kp * 10000f64).round().clamp(0.0f64, 0x3FF as f64) as u32; let speed_power_kp = (constant_speed.speed_power_kp * 10000f64)
let speed_power_ki = (constant_speed.speed_power_ki * 1000000f64).round().clamp(0.0f64, 0xFFF as f64) as u32; .round()
.clamp(0.0f64, f64::from(0x3FF)) as u32;
let speed_power_ki = (constant_speed.speed_power_ki * 1_000_000_f64)
.round()
.clamp(0.0f64, f64::from(0xFFF)) as u32;
let expected_value = let expected_value = speed_power_kp << 20
speed_power_kp << 20 | speed_power_ki << 8
| speed_power_ki << 8 | ((constant_speed.speed_power_upper_limit as u32) & 0x7) << 5
| ((constant_speed.speed_power_upper_limit as u32) & 0x7) << 5 | ((constant_speed.speed_power_lower_limit as u32) & 0x7) << 2
| ((constant_speed.speed_power_lower_limit as u32) & 0x7) << 2 | ((constant_speed.mode as u32) & 0x3) << 9;
| ((constant_speed.mode as u32) & 0x3) << 9;
self.assert_register(0x8E, Mct8316AVData::Four(expected_value)) self.assert_register(0x8E, Mct8316AVData::Four(expected_value))
} }
// The casts in here are save due to clamps
#[allow(clippy::cast_possible_truncation)]
#[allow(clippy::cast_sign_loss)]
// To follow a pattern for the ICD
#[allow(clippy::identity_op)]
pub fn set_constant_power(self, constant_power: ConstantPower) -> Result<Self> { pub fn set_constant_power(self, constant_power: ConstantPower) -> Result<Self> {
trace!("Mct8316AVEeprom::set_constant_power(self: {self:?}, constant_power: {constant_power:?})"); trace!(
"Mct8316AVEeprom::set_constant_power(self: {self:?}, constant_power: {constant_power:?})"
);
let max_speed = (constant_power.max_speed * 16f64).round().clamp(0.0f64, 0xFFFF as f64) as u32; let max_speed = (constant_power.max_speed * 16f64)
let max_power = (constant_power.max_power * 4f64).round().clamp(0.0f64, 0x3FF as f64) as u32; .round()
.clamp(0.0f64, f64::from(0xFFFF)) as u32;
let max_power = (constant_power.max_power * 4f64)
.round()
.clamp(0.0f64, f64::from(0x3FF)) as u32;
let expected_value = let expected_value = max_speed << 15
max_speed << 15 | if constant_power.dead_time_compensation_enable {
| if constant_power.dead_time_compensation_enable { 1u32 << 14 } else { 0u32 } 1u32 << 14
| max_power << 4 } else {
| ((constant_power.power_hysteresis as u32) & 0x3) << 2 0u32
| ((constant_power.mode as u32) & 0x3) << 0; }
| max_power << 4
| ((constant_power.power_hysteresis as u32) & 0x3) << 2
| ((constant_power.mode as u32) & 0x3) << 0;
self.assert_register(0x90, Mct8316AVData::Four(expected_value)) self.assert_register(0x90, Mct8316AVData::Four(expected_value))
} }
@@ -227,15 +307,14 @@ where
pub fn set_two_phase_profile(self, profile: TwoPhase150DegreeProfile) -> Result<Self> { pub fn set_two_phase_profile(self, profile: TwoPhase150DegreeProfile) -> Result<Self> {
trace!("Mct8316AVEeprom::set_two_phase_profile(self: {self:?}, profile: {profile:?})"); trace!("Mct8316AVEeprom::set_two_phase_profile(self: {self:?}, profile: {profile:?})");
let expected_value = let expected_value = ((profile.steps[0] as u32) & 0x7) << 28
((profile.steps[0] as u32) & 0x7) << 28 | ((profile.steps[1] as u32) & 0x7) << 25
| ((profile.steps[1] as u32) & 0x7) << 25 | ((profile.steps[2] as u32) & 0x7) << 22
| ((profile.steps[2] as u32) & 0x7) << 22 | ((profile.steps[3] as u32) & 0x7) << 19
| ((profile.steps[3] as u32) & 0x7) << 19 | ((profile.steps[4] as u32) & 0x7) << 16
| ((profile.steps[4] as u32) & 0x7) << 16 | ((profile.steps[5] as u32) & 0x7) << 13
| ((profile.steps[5] as u32) & 0x7) << 13 | ((profile.steps[6] as u32) & 0x7) << 10
| ((profile.steps[6] as u32) & 0x7) << 10 | ((profile.steps[7] as u32) & 0x7) << 7;
| ((profile.steps[7] as u32) & 0x7) << 7;
self.assert_register(0x96, Mct8316AVData::Four(expected_value)) self.assert_register(0x96, Mct8316AVData::Four(expected_value))
} }
@@ -243,32 +322,30 @@ where
pub fn set_three_phase_profile(self, profile: ThreePhase150DegreeProfile) -> Result<Self> { pub fn set_three_phase_profile(self, profile: ThreePhase150DegreeProfile) -> Result<Self> {
trace!("Mct8316AVEeprom::set_three_phase_profile(self: {self:?}, profile: {profile:?})"); trace!("Mct8316AVEeprom::set_three_phase_profile(self: {self:?}, profile: {profile:?})");
let expected_value = let expected_value = ((profile.steps[0] as u32) & 0x7) << 28
((profile.steps[0] as u32) & 0x7) << 28 | ((profile.steps[1] as u32) & 0x7) << 25
| ((profile.steps[1] as u32) & 0x7) << 25 | ((profile.steps[2] as u32) & 0x7) << 22
| ((profile.steps[2] as u32) & 0x7) << 22 | ((profile.steps[3] as u32) & 0x7) << 19
| ((profile.steps[3] as u32) & 0x7) << 19 | ((profile.steps[4] as u32) & 0x7) << 16
| ((profile.steps[4] as u32) & 0x7) << 16 | ((profile.steps[5] as u32) & 0x7) << 13
| ((profile.steps[5] as u32) & 0x7) << 13 | ((profile.steps[6] as u32) & 0x7) << 10
| ((profile.steps[6] as u32) & 0x7) << 10 | ((profile.steps[7] as u32) & 0x7) << 7
| ((profile.steps[7] as u32) & 0x7) << 7 | ((profile.lead_angle as u32) & 0x3) << 5;
| ((profile.lead_angle as u32) & 0x3) << 5;
self.assert_register(0x98, Mct8316AVData::Four(expected_value)) self.assert_register(0x98, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_trap_config1(self, trap_config1: TrapConfig1) -> Result<Self> { pub fn set_trap_config1(self, trap_config1: TrapConfig1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_trap_config1(self: {self:?}, trap_config1: {trap_config1:?})"); trace!("Mct8316AVEeprom::set_trap_config1(self: {self:?}, trap_config1: {trap_config1:?})");
let expected_value = let expected_value = ((trap_config1.open_loop_handoff_cycles as u32) & 0x3) << 22
((trap_config1.open_loop_handoff_cycles as u32) & 0x3) << 22 | ((trap_config1.avs_negative_current_limit as u32) & 0x7) << 16
| ((trap_config1.avs_negative_current_limit as u32) & 0x7) << 16 | ((trap_config1.avs_limit_hysteresis as u32) & 0x1) << 15
| ((trap_config1.avs_limit_hysteresis as u32) & 0x1) << 15 | ((trap_config1.isd_bemf_threshold as u32) & 0x1F) << 10
| ((trap_config1.isd_bemf_threshold as u32) & 0x1F) << 10 | ((trap_config1.isd_cycle_threshold as u32) & 0x7) << 7
| ((trap_config1.isd_cycle_threshold as u32) & 0x7) << 7 | ((trap_config1.open_loop_zc_detection_threshold as u32) & 0x7) << 2
| ((trap_config1.open_loop_zc_detection_threshold as u32) & 0x7) << 2 | ((trap_config1.fast_startup_div_factor as u32) & 0x3) << 0;
| ((trap_config1.fast_startup_div_factor as u32) & 0x3) << 0
;
self.assert_register(0x9A, Mct8316AVData::Four(expected_value)) self.assert_register(0x9A, Mct8316AVData::Four(expected_value))
} }
@@ -276,48 +353,60 @@ where
pub fn set_trap_config2(self, trap_config2: TrapConfig2) -> Result<Self> { pub fn set_trap_config2(self, trap_config2: TrapConfig2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_trap_config2(self: {self:?}, trap_config2: {trap_config2:?})"); trace!("Mct8316AVEeprom::set_trap_config2(self: {self:?}, trap_config2: {trap_config2:?})");
let expected_value = let expected_value = (u32::from(trap_config2.blanking_time_microseconds) & 0xF) << 27
((trap_config2.blanking_time_microseconds as u32) & 0xF) << 27 | (u32::from(trap_config2.comparator_deglitch_time_microseconds) & 0x7) << 24
| ((trap_config2.comparator_deglitch_time_microseconds as u32) & 0x7) << 24 | ((trap_config2.align_duty_cycle as u32) & 0x7) << 18;
| ((trap_config2.align_duty_cycle as u32) & 0x7) << 18;
self.assert_register(0x9C, Mct8316AVData::Four(expected_value)) self.assert_register(0x9C, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_fault_config1(self, fault_config1: FaultConfig1) -> Result<Self> { pub fn set_fault_config1(self, fault_config1: FaultConfig1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_fault_config1(self: {self:?}, fault_config1: {fault_config1:?})"); trace!(
"Mct8316AVEeprom::set_fault_config1(self: {self:?}, fault_config1: {fault_config1:?})"
);
let expected_value = let expected_value = ((fault_config1.no_motor_detect_deglitch_time as u32) & 0x7) << 27
((fault_config1.no_motor_detect_deglitch_time as u32) & 0x7) << 27 | ((fault_config1.cycle_by_cycle_current_limit as u32) & 0xF) << 23
| ((fault_config1.cycle_by_cycle_current_limit as u32) & 0xF) << 23 | ((fault_config1.lock_detection_current_limit as u32) & 0xF) << 19
| ((fault_config1.lock_detection_current_limit as u32) & 0xF) << 19 | ((fault_config1.lock_detection_current_limit_mode as u32) & 0xF) << 15
| ((fault_config1.lock_detection_current_limit_mode as u32) & 0xF) << 15 | ((fault_config1.lock_detection_current_limit_deglitch_time as u32) & 0xF) << 11
| ((fault_config1.lock_detection_current_limit_deglitch_time as u32) & 0xF) << 11 | (u32::from(fault_config1.cycle_by_cycle_pwm_limit) & 0x7) << 8
| ((fault_config1.cycle_by_cycle_pwm_limit as u32) & 0x7) << 8 | ((fault_config1.motor_lock_mode as u32) & 0xF) << 3
| ((fault_config1.motor_lock_mode as u32) & 0xF) << 3 | ((fault_config1.lock_retry_time as u32) & 0x7) << 0;
| ((fault_config1.lock_retry_time as u32) & 0x7) << 0;
self.assert_register(0x92, Mct8316AVData::Four(expected_value)) self.assert_register(0x92, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_fault_config2(self, fault_config2: FaultConfig2) -> Result<Self> { pub fn set_fault_config2(self, fault_config2: FaultConfig2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_fault_config2(self: {self:?}, fault_config2: {fault_config2:?})"); trace!(
"Mct8316AVEeprom::set_fault_config2(self: {self:?}, fault_config2: {fault_config2:?})"
);
let expected_value = let expected_value = if fault_config2.lock_abnormal_speed_enable {
if fault_config2.lock_abnormal_speed_enable { 1u32 << 30 } else { 0u32 } 1u32 << 30
| if fault_config2.lock_loss_of_sync_enable { 1u32 << 29 } else { 0u32 } } else {
| if fault_config2.lock_no_motor_enable { 1u32 << 28 } else { 0u32 } 0u32
| ((fault_config2.abnormal_speed_lock_threshold as u32) & 0xF) << 24 } | if fault_config2.lock_loss_of_sync_enable {
| ((fault_config2.loss_sync_times as u32) & 0x7) << 21 1u32 << 29
| ((fault_config2.no_motor_threshold as u32) & 0x7) << 18 } else {
| ((fault_config2.max_motor_voltage_mode as u32) & 0x1) << 17 0u32
| ((fault_config2.max_motor_voltage as u32) & 0x7) << 14 } | if fault_config2.lock_no_motor_enable {
| ((fault_config2.min_motor_voltage_mode as u32) & 0x1) << 13 1u32 << 28
| ((fault_config2.min_motor_voltage as u32) & 0x7) << 10 } else {
| ((fault_config2.automatic_retries as u32) & 0x7) << 7 0u32
| ((fault_config2.lock_min_speed as u32) & 0x7) << 4 } | ((fault_config2.abnormal_speed_lock_threshold as u32) & 0xF) << 24
| ((fault_config2.abnormal_speed_lock as u32) & 0x3) << 2 | ((fault_config2.loss_sync_times as u32) & 0x7) << 21
| ((fault_config2.zero_duty_threshold as u32) & 0x3) << 0; | ((fault_config2.no_motor_threshold as u32) & 0x7) << 18
| ((fault_config2.max_motor_voltage_mode as u32) & 0x1) << 17
| ((fault_config2.max_motor_voltage as u32) & 0x7) << 14
| ((fault_config2.min_motor_voltage_mode as u32) & 0x1) << 13
| ((fault_config2.min_motor_voltage as u32) & 0x7) << 10
| ((fault_config2.automatic_retries as u32) & 0x7) << 7
| ((fault_config2.lock_min_speed as u32) & 0x7) << 4
| ((fault_config2.abnormal_speed_lock as u32) & 0x3) << 2
| ((fault_config2.zero_duty_threshold as u32) & 0x3) << 0;
self.assert_register(0x94, Mct8316AVData::Four(expected_value)) self.assert_register(0x94, Mct8316AVData::Four(expected_value))
} }
@@ -325,12 +414,11 @@ where
pub fn set_pin_config1(self, pin_config1: PinConfig1) -> Result<Self> { pub fn set_pin_config1(self, pin_config1: PinConfig1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_pin_config1(self: {self:?}, pin_config1: {pin_config1:?})"); trace!("Mct8316AVEeprom::set_pin_config1(self: {self:?}, pin_config1: {pin_config1:?})");
let expected_value = let expected_value = (u32::from(pin_config1.dacout1_address) & 0xFFF) << 19
((pin_config1.dacout1_address as u32) & 0xFFF) << 19 | (u32::from(pin_config1.dacout2_address) & 0xFFF) << 7
| ((pin_config1.dacout2_address as u32) & 0xFFF) << 7 | ((pin_config1.brake_input_config as u32) & 0x3) << 5
| ((pin_config1.brake_input_config as u32) & 0x3) << 5 | ((pin_config1.direction_input_config as u32) & 0x3) << 3
| ((pin_config1.direction_input_config as u32) & 0x3) << 3 | ((pin_config1.speed_input_config as u32) & 0x3) << 1;
| ((pin_config1.speed_input_config as u32) & 0x3) << 1;
self.assert_register(0xA4, Mct8316AVData::Four(expected_value)) self.assert_register(0xA4, Mct8316AVData::Four(expected_value))
} }
@@ -338,74 +426,122 @@ where
pub fn set_pin_config2(self, pin_config2: PinConfig2) -> Result<Self> { pub fn set_pin_config2(self, pin_config2: PinConfig2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_pin_config2(self: {self:?}, pin_config2: {pin_config2:?})"); trace!("Mct8316AVEeprom::set_pin_config2(self: {self:?}, pin_config2: {pin_config2:?})");
let expected_value = let expected_value = ((pin_config2.pin36config as u32) & 0x3) << 29
((pin_config2.pin36config as u32) & 0x3) << 29 | ((pin_config2.pin37_38config as u32) & 0x1) << 27
| ((pin_config2.pin37_38config as u32) & 0x1) << 27 | ((pin_config2.sleep_time as u32) & 0x3) << 18
| ((pin_config2.sleep_time as u32) & 0x3) << 18 | if pin_config2.enable_external_watchdog {
| if pin_config2.enable_external_watchdog { 1u32 << 17 } else { 0u32 } 1u32 << 17
| ((pin_config2.external_watchdog_source as u32) & 0x1) << 16 } else {
| ((pin_config2.external_watchdog_fault_mode as u32) & 0x1) << 15 0u32
| ((pin_config2.external_watchdog_frequency as u32) & 0x3) << 13; }
| ((pin_config2.external_watchdog_source as u32) & 0x1) << 16
| ((pin_config2.external_watchdog_fault_mode as u32) & 0x1) << 15
| ((pin_config2.external_watchdog_frequency as u32) & 0x3) << 13;
self.assert_register(0xA6, Mct8316AVData::Four(expected_value)) self.assert_register(0xA6, Mct8316AVData::Four(expected_value))
} }
pub fn set_device_config(self, device_config: DeviceConfig) -> Result<Self> { pub fn set_device_config(self, device_config: DeviceConfig) -> Result<Self> {
trace!("Mct8316AVEeprom::set_device_config(self: {self:?}, device_config: {device_config:?})"); trace!(
"Mct8316AVEeprom::set_device_config(self: {self:?}, device_config: {device_config:?})"
);
let expected_value = let expected_value = (u32::from(device_config.max_frequency) & 0x7FFF) << 16
((device_config.max_frequency as u32) & 0x7FFF) << 16 | if device_config.stl_enable {
| if device_config.stl_enable { 1u32 << 15 } else { 0u32 } 1u32 << 15
| if device_config.ssm_enable { 1u32 << 14 } else { 0u32 } } else {
| ((device_config.device_mode as u32) & 0x1) << 11 0u32
| ((device_config.pwm_range_select as u32) & 0x1) << 10 }
| ((device_config.clock_source as u32) & 0x3) << 8 | if device_config.ssm_enable {
| if device_config.external_clock_enable { 1u32 << 6 } else { 0u32 } 1u32 << 14
| ((device_config.external_clock_frequency as u32) & 0x7) << 3; } else {
0u32
}
| ((device_config.device_mode as u32) & 0x1) << 11
| ((device_config.pwm_range_select as u32) & 0x1) << 10
| ((device_config.clock_source as u32) & 0x3) << 8
| if device_config.external_clock_enable {
1u32 << 6
} else {
0u32
}
| ((device_config.external_clock_frequency as u32) & 0x7) << 3;
self.assert_register(0xA8, Mct8316AVData::Four(expected_value)) self.assert_register(0xA8, Mct8316AVData::Four(expected_value))
} }
#[allow(clippy::identity_op)]
pub fn set_gate_driver_config1(self, gate_driver_config1: GateDriverConfig1) -> Result<Self> { pub fn set_gate_driver_config1(self, gate_driver_config1: GateDriverConfig1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_gate_driver_config1(self: {self:?}, gate_driver_config1: {gate_driver_config1:?})"); trace!(
"Mct8316AVEeprom::set_gate_driver_config1(self: {self:?}, gate_driver_config1: {gate_driver_config1:?})"
);
let expected_value = let expected_value = ((gate_driver_config1.slew_rate as u32) & 0x3) << 26
((gate_driver_config1.slew_rate as u32) & 0x3) << 26 | ((gate_driver_config1.overvoltage_protection_level as u32) & 0x1) << 19
| ((gate_driver_config1.overvoltage_protection_level as u32) & 0x1) << 19 | if gate_driver_config1.overvoltage_protection_enable {
| if gate_driver_config1.overvoltage_protection_enable { 1u32 << 18 } else { 0u32 } 1u32 << 18
| if gate_driver_config1.overtemperature_warning_enable { 1u32 << 16 } else { 0u32 } } else {
| ((gate_driver_config1.overcurrent_protection_deglitch_time as u32) & 0x3) << 12 0u32
| ((gate_driver_config1.overcurrent_protection_retry_time as u32) & 0x1) << 11 }
| ((gate_driver_config1.overcurrent_protection_level as u32) & 0x1) << 10 | if gate_driver_config1.overtemperature_warning_enable {
| ((gate_driver_config1.overcurrent_fault_mode as u32) & 0x3) << 8 1u32 << 16
| ((gate_driver_config1.low_demag_comparator_threshold as u32) & 0x1) << 5 } else {
| ((gate_driver_config1.high_demag_comparator_threshold as u32) & 0x1) << 4 0u32
| if gate_driver_config1.synchronous_rectification_enable { 1u32 << 3 } else { 0u32 } }
| if gate_driver_config1.asynchronous_rectification_enable { 1u32 << 2 } else { 0u32 } | ((gate_driver_config1.overcurrent_protection_deglitch_time as u32) & 0x3) << 12
| ((gate_driver_config1.current_sense_amplifier as u32) & 0x3) << 0; | ((gate_driver_config1.overcurrent_protection_retry_time as u32) & 0x1) << 11
| ((gate_driver_config1.overcurrent_protection_level as u32) & 0x1) << 10
| ((gate_driver_config1.overcurrent_fault_mode as u32) & 0x3) << 8
| ((gate_driver_config1.low_demag_comparator_threshold as u32) & 0x1) << 5
| ((gate_driver_config1.high_demag_comparator_threshold as u32) & 0x1) << 4
| if gate_driver_config1.synchronous_rectification_enable {
1u32 << 3
} else {
0u32
}
| if gate_driver_config1.asynchronous_rectification_enable {
1u32 << 2
} else {
0u32
}
| ((gate_driver_config1.current_sense_amplifier as u32) & 0x3) << 0;
self.assert_register(0xAC, Mct8316AVData::Four(expected_value)) self.assert_register(0xAC, Mct8316AVData::Four(expected_value))
} }
pub fn set_gate_driver_config2(self, gate_driver_config2: GateDriverConfig2) -> Result<Self> { pub fn set_gate_driver_config2(self, gate_driver_config2: GateDriverConfig2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_gate_driver_config2(self: {self:?}, gate_driver_config2: {gate_driver_config2:?})"); trace!(
"Mct8316AVEeprom::set_gate_driver_config2(self: {self:?}, gate_driver_config2: {gate_driver_config2:?})"
);
let expected_value = let expected_value = if gate_driver_config2.driver_delay_compensation_enable {
if gate_driver_config2.driver_delay_compensation_enable { 1u32 << 30 } else { 0u32 } 1u32 << 30
| ((gate_driver_config2.target_delay as u32) & 0xF) << 26 } else {
| ((gate_driver_config2.buck_slew_rate as u32) & 0x1) << 25 0u32
| if gate_driver_config2.buck_power_sequencing_disable { 1u32 << 24 } else { 0u32 } } | ((gate_driver_config2.target_delay as u32) & 0xF) << 26
| ((gate_driver_config2.buck_current_limit as u32) & 0x1) << 23 | ((gate_driver_config2.buck_slew_rate as u32) & 0x1) << 25
| ((gate_driver_config2.buck_voltage_selection as u32) & 0x3) << 21 | if gate_driver_config2.buck_power_sequencing_disable {
| if gate_driver_config2.buck_disable { 1u32 << 20 } else { 0u32 }; 1u32 << 24
} else {
0u32
}
| ((gate_driver_config2.buck_current_limit as u32) & 0x1) << 23
| ((gate_driver_config2.buck_voltage_selection as u32) & 0x3) << 21
| if gate_driver_config2.buck_disable {
1u32 << 20
} else {
0u32
};
self.assert_register(0xAE, Mct8316AVData::Four(expected_value)) self.assert_register(0xAE, Mct8316AVData::Four(expected_value))
} }
fn assert_register(self, address: u32, value: Mct8316AVData) -> Result<Self> { fn assert_register(self, address: u32, value: Mct8316AVData) -> Result<Self> {
trace!("Mct8316AVEeprom::assert_register(self: {self:?}, address: {address:06x}, value: {value})"); trace!(
"Mct8316AVEeprom::assert_register(self: {self:?}, address: {address:06x}, value: {value})"
);
let mut read_value = value.clone(); let mut read_value = value;
self.driver.read(address, &mut read_value)?; self.driver.read(address, &mut read_value)?;
@@ -428,10 +564,11 @@ where
pub fn commit(self) -> Result<()> { pub fn commit(self) -> Result<()> {
trace!("Mct8316AVEeprom::commit(self: {self:?})"); trace!("Mct8316AVEeprom::commit(self: {self:?})");
if self.modified { if self.modified {
self.driver.write(0x0000E6, Mct8316AVData::Four(0x80000000))?; self.driver
.write(0x00_00E6, Mct8316AVData::Four(0x8000_0000))?;
// Wait for EEPROM operation to complete // Wait for EEPROM operation to complete
sleep(Duration::from_millis(100)); sleep(Duration::from_millis(100));
} }
Ok(()) Ok(())
} }
} }

6
flight/src/hardware/mct8316a/fault_config.rs
View File

@@ -17,10 +17,12 @@ impl Default for FaultConfig1 {
fn default() -> Self { fn default() -> Self {
Self { Self {
no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime::Milliseconds1, no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime::Milliseconds1,
cycle_by_cycle_current_limit: CycleByCycleCurrentLimit::RecoverNextPwmFaultActiveRecirculation, cycle_by_cycle_current_limit:
CycleByCycleCurrentLimit::RecoverNextPwmFaultActiveRecirculation,
lock_detection_current_limit: LockDetectionCurrentLimit::NotApplicable, lock_detection_current_limit: LockDetectionCurrentLimit::NotApplicable,
lock_detection_current_limit_mode: LockMode::LatchFaultTristated, lock_detection_current_limit_mode: LockMode::LatchFaultTristated,
lock_detection_current_limit_deglitch_time: LockDetectionCurrentLimitDeglitchTime::Milliseconds1, lock_detection_current_limit_deglitch_time:
LockDetectionCurrentLimitDeglitchTime::Milliseconds1,
cycle_by_cycle_pwm_limit: 0, cycle_by_cycle_pwm_limit: 0,
motor_lock_mode: LockMode::LatchFaultTristated, motor_lock_mode: LockMode::LatchFaultTristated,
lock_retry_time: LockRetryTime::Milliseconds100, lock_retry_time: LockRetryTime::Milliseconds100,

2
flight/src/hardware/mct8316a/gate_driver_config.rs
View File

@@ -1,5 +1,7 @@
#![allow(dead_code)] #![allow(dead_code)]
// This comes from the ICD
#[allow(clippy::struct_excessive_bools)]
#[derive(Debug, Clone)] #[derive(Debug, Clone)]
pub struct GateDriverConfig1 { pub struct GateDriverConfig1 {
pub slew_rate: SlewRate, pub slew_rate: SlewRate,

2
flight/src/hardware/mct8316a/isd_config.rs
View File

@@ -1,5 +1,7 @@
#![allow(dead_code)] #![allow(dead_code)]
// This comes from the ICD
#[allow(clippy::struct_excessive_bools)]
#[derive(Debug, Clone)] #[derive(Debug, Clone)]
pub struct IsdConfig { pub struct IsdConfig {
pub enable_isd: bool, pub enable_isd: bool,

14
flight/src/hardware/mct8316a/mod.rs
View File

@@ -1,16 +1,16 @@
mod closed_loop;
mod constant_power;
mod constant_speed;
mod device_config;
mod driver; mod driver;
mod eeprom; mod eeprom;
mod fault_config;
mod gate_driver_config;
mod isd_config; mod isd_config;
mod motor_startup; mod motor_startup;
mod closed_loop;
mod constant_speed;
mod constant_power;
mod phase_profile; mod phase_profile;
mod trap_config;
mod fault_config;
mod pin_config; mod pin_config;
mod device_config; mod trap_config;
mod gate_driver_config;
use anyhow::Result; use anyhow::Result;

6
flight/src/hardware/mct8316a/motor_startup.rs
View File

@@ -35,7 +35,7 @@ impl Default for MotorStartup1 {
pub enum MotorStartupMethod { pub enum MotorStartupMethod {
Align = 0x0, Align = 0x0,
DoubleAlign = 0x1, DoubleAlign = 0x1,
IPD = 0x2, Ipd = 0x2,
SlowFirstCycle = 0x3, SlowFirstCycle = 0x3,
} }
@@ -200,7 +200,6 @@ impl Default for MotorStartup2 {
} }
} }
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
pub enum OpenLoopCurrentLimitMode { pub enum OpenLoopCurrentLimitMode {
OpenLoopCurrentLimit = 0x0, OpenLoopCurrentLimit = 0x0,
@@ -358,6 +357,3 @@ pub enum MinimumDutyCycle {
Percent25 = 0xE, Percent25 = 0xE,
Percent30 = 0xF, Percent30 = 0xF,
} }

1
flight/src/hardware/mct8316a/trap_config.rs
View File

@@ -135,4 +135,3 @@ impl Default for TrapConfig2 {
} }
} }
} }

6
flight/src/hardware/mod.rs
View File

@@ -34,15 +34,15 @@ pub fn initialize() -> Result<impl Hardware> {
#[allow(unreachable_code)] #[allow(unreachable_code)]
pub fn initialize() -> Result<impl Hardware> { pub fn initialize() -> Result<impl Hardware> {
trace!("initialize()"); trace!("initialize()");
Ok(sim::SimHardware::new()) sim::SimHardware::new()
} }
pub mod error; pub mod error;
pub mod channelization;
pub mod mcp23017; pub mod mcp23017;
#[cfg(feature = "raspi")] #[cfg(feature = "raspi")]
mod mcp3208; mod mcp3208;
pub mod channelization;
pub mod mct8316a; pub mod mct8316a;
mod sim;
pub mod pin; pub mod pin;
mod sim;

36
flight/src/hardware/raspi/mod.rs
View File

@@ -1,11 +1,11 @@
mod pwm; mod pwm;
use crate::hardware::mcp23017::Mcp23017Driver; use crate::hardware::Hardware;
use crate::hardware::mcp3208::Mcp3208; use crate::hardware::mcp3208::Mcp3208;
use crate::hardware::mcp23017::Mcp23017Driver;
use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a}; use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a};
use crate::hardware::raspi::pwm::PwmWrapper; use crate::hardware::raspi::pwm::PwmWrapper;
use crate::hardware::sim::mct8316a::SimMct8316a; use crate::hardware::sim::mct8316a::SimMct8316a;
use crate::hardware::Hardware;
use anyhow::Result; use anyhow::Result;
use embedded_hal_bus::i2c::MutexDevice; use embedded_hal_bus::i2c::MutexDevice;
use log::{debug, info, trace}; use log::{debug, info, trace};
@@ -38,12 +38,16 @@ impl RaspiHardware {
Ok(Self { Ok(Self {
_gpio: Gpio::new()?, _gpio: Gpio::new()?,
i2c_bus: Mutex::new(I2c::with_bus(0u8)?), i2c_bus: Mutex::new(I2c::with_bus(0u8)?),
mcp3208: Mcp3208::new(SimpleHalSpiDevice::new(Spi::new( mcp3208: Mcp3208::new(
Bus::Spi1, SimpleHalSpiDevice::new(Spi::new(
SlaveSelect::Ss1, Bus::Spi1,
CLOCK_1MHZ, SlaveSelect::Ss1,
Mode::Mode0, CLOCK_1MHZ,
)?), 3.3f64).into(), Mode::Mode0,
)?),
3.3f64,
)
.into(),
mct8316a: SimMct8316a::new().into(), mct8316a: SimMct8316a::new().into(),
// mct8316a: SimMct8316a::new().into(), // mct8316a: SimMct8316a::new().into(),
}) })
@@ -56,12 +60,18 @@ impl Hardware for RaspiHardware {
fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> { fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> {
trace!("RaspiHardware::new_mcp23017_a(self: {self:?})"); trace!("RaspiHardware::new_mcp23017_a(self: {self:?})");
Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100000)) Ok(Mcp23017Driver::new(
MutexDevice::new(&self.i2c_bus),
0b0100000,
))
} }
fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>> { fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>> {
trace!("RaspiHardware::new_mcp23017_b(self: {self:?})"); trace!("RaspiHardware::new_mcp23017_b(self: {self:?})");
Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100001)) Ok(Mcp23017Driver::new(
MutexDevice::new(&self.i2c_bus),
0b0100001,
))
} }
fn new_pwm0(&self) -> Result<Self::Pwm> { fn new_pwm0(&self) -> Result<Self::Pwm> {
@@ -75,10 +85,12 @@ impl Hardware for RaspiHardware {
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> { fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
trace!("RaspiHardware::new_mct8316a(self: {self:?})"); trace!("RaspiHardware::new_mct8316a(self: {self:?})");
Ok(Mct8316AVDriver::new(MutexDevice::new(&self.mct8316a), 0b0000000)) Ok(Mct8316AVDriver::new(
MutexDevice::new(&self.mct8316a),
0b0000000,
))
} }
fn get_battery_voltage(&self) -> Result<f64> { fn get_battery_voltage(&self) -> Result<f64> {
trace!("RaspiHardware::get_battery_voltage(self: {self:?})"); trace!("RaspiHardware::get_battery_voltage(self: {self:?})");
self.mcp3208.borrow_mut().read_single(1) self.mcp3208.borrow_mut().read_single(1)

8
flight/src/hardware/raspi/pwm.rs
View File

@@ -17,9 +17,7 @@ impl PwmWrapper {
pwm.set_period(PWM_PERIOD)?; pwm.set_period(PWM_PERIOD)?;
pwm.enable()?; pwm.enable()?;
pwm.set_reset_on_drop(true); pwm.set_reset_on_drop(true);
Ok(Self { Ok(Self { pwm })
pwm
})
} }
} }
@@ -35,6 +33,8 @@ impl SetDutyCycle for PwmWrapper {
fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> { fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
trace!("PwmWrapper::set_duty_cycle(self: {self:?}, duty: {duty})"); trace!("PwmWrapper::set_duty_cycle(self: {self:?}, duty: {duty})");
self.pwm.set_duty_cycle((duty as f64) / (u16::MAX as f64)).map_err(WrappingError) self.pwm
.set_duty_cycle((duty as f64) / (u16::MAX as f64))
.map_err(WrappingError)
} }
} }

27
flight/src/hardware/sim/hardware.rs
View File

@@ -1,9 +1,9 @@
use crate::hardware::Hardware;
use crate::hardware::mcp23017::Mcp23017Driver; use crate::hardware::mcp23017::Mcp23017Driver;
use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a}; use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a};
use crate::hardware::sim::mcp23017::SimMcp23017; use crate::hardware::sim::mcp23017::SimMcp23017;
use crate::hardware::sim::mct8316a::SimMct8316a; use crate::hardware::sim::mct8316a::SimMct8316a;
use crate::hardware::sim::pwm::SimPwm; use crate::hardware::sim::pwm::SimPwm;
use crate::hardware::Hardware;
use anyhow::Result; use anyhow::Result;
use embedded_hal_bus::i2c::MutexDevice; use embedded_hal_bus::i2c::MutexDevice;
use log::trace; use log::trace;
@@ -18,14 +18,16 @@ pub struct SimHardware {
} }
impl SimHardware { impl SimHardware {
pub fn new() -> Self { // This returns a Result to match the raspi implementation
#[allow(clippy::unnecessary_wraps)]
pub fn new() -> Result<Self> {
trace!("SimHardware::new()"); trace!("SimHardware::new()");
Self { Ok(Self {
mcp23017a: SimMcp23017::new().into(), mcp23017a: SimMcp23017::new().into(),
mcp23017b: SimMcp23017::new().into(), mcp23017b: SimMcp23017::new().into(),
mct8316a: SimMct8316a::new().into(), mct8316a: SimMct8316a::new().into(),
battery_voltage: 12.4f64, battery_voltage: 12.4f64,
} })
} }
} }
@@ -35,12 +37,18 @@ impl Hardware for SimHardware {
fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> { fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> {
trace!("SimHardware::new_mcp23017_a(self: {self:?})"); trace!("SimHardware::new_mcp23017_a(self: {self:?})");
Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017a), 0b0100000)) Ok(Mcp23017Driver::new(
MutexDevice::new(&self.mcp23017a),
0b010_0000,
))
} }
fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>> { fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>> {
trace!("SimHardware::new_mcp23017_b(self: {self:?})"); trace!("SimHardware::new_mcp23017_b(self: {self:?})");
Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017b), 0b0100000)) Ok(Mcp23017Driver::new(
MutexDevice::new(&self.mcp23017b),
0b010_0001,
))
} }
fn new_pwm0(&self) -> Result<Self::Pwm> { fn new_pwm0(&self) -> Result<Self::Pwm> {
@@ -50,11 +58,14 @@ impl Hardware for SimHardware {
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> { fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
trace!("SimHardware::new_mct8316a(self: {self:?})"); trace!("SimHardware::new_mct8316a(self: {self:?})");
Ok(Mct8316AVDriver::new(MutexDevice::new(&self.mct8316a), 0b0000000)) Ok(Mct8316AVDriver::new(
MutexDevice::new(&self.mct8316a),
0b000_0000,
))
} }
fn get_battery_voltage(&self) -> Result<f64> { fn get_battery_voltage(&self) -> Result<f64> {
trace!("SimHardware::get_battery_voltage(self: {self:?})"); trace!("SimHardware::get_battery_voltage(self: {self:?})");
Ok(self.battery_voltage) Ok(self.battery_voltage)
} }
} }

10
flight/src/hardware/sim/mcp23017.rs
View File

@@ -34,8 +34,14 @@ impl ErrorType for SimMcp23017 {
} }
impl I2c for SimMcp23017 { impl I2c for SimMcp23017 {
fn transaction(&mut self, _address: SevenBitAddress, operations: &mut [Operation<'_>]) -> Result<(), Self::Error> { fn transaction(
trace!("SimMcp23017::transaction(self: {self:?}, _address: {_address}, operations: {operations:?})"); &mut self,
address: SevenBitAddress,
operations: &mut [Operation<'_>],
) -> Result<(), Self::Error> {
trace!(
"SimMcp23017::transaction(self: {self:?}, address: {address}, operations: {operations:?})"
);
for operation in operations { for operation in operations {
match operation { match operation {
Operation::Write(_write_buffer) => { Operation::Write(_write_buffer) => {

51
flight/src/hardware/sim/mct8316a.rs
View File

@@ -25,8 +25,14 @@ impl ErrorType for SimMct8316a {
} }
impl I2c for SimMct8316a { impl I2c for SimMct8316a {
fn transaction(&mut self, i2c_addr: SevenBitAddress, operations: &mut [Operation<'_>]) -> Result<(), Self::Error> { fn transaction(
trace!("SimMct8316a::transaction(self: {self:?}, i2c_addr: {i2c_addr}, operations: {operations:?})"); &mut self,
i2c_addr: SevenBitAddress,
operations: &mut [Operation<'_>],
) -> Result<(), Self::Error> {
trace!(
"SimMct8316a::transaction(self: {self:?}, i2c_addr: {i2c_addr}, operations: {operations:?})"
);
let mut do_read_operation = false; let mut do_read_operation = false;
let mut include_crc = false; let mut include_crc = false;
let mut data_length = 2; let mut data_length = 2;
@@ -47,38 +53,53 @@ impl I2c for SimMct8316a {
} else if write_buffer[0] & 0x20 > 0 { } else if write_buffer[0] & 0x20 > 0 {
data_length = 8; data_length = 8;
} }
address |= ((write_buffer[0] & 0xF) as u32) << 16; address |= u32::from(write_buffer[0] & 0xF) << 16;
address |= (write_buffer[1] as u32) << 8; address |= u32::from(write_buffer[1]) << 8;
address |= (write_buffer[2] as u32) << 0; address |= u32::from(write_buffer[2]);
if !do_read_operation { if !do_read_operation {
if write_buffer.len() != 3 + data_length + 1 { if write_buffer.len() != 3 + data_length + 1 {
return Err(WrappingError(anyhow!("Write for write has wrong length {}. {} expected", write_buffer.len(), 3 + data_length + 1))); return Err(WrappingError(anyhow!(
"Write for write has wrong length {}. {} expected",
write_buffer.len(),
3 + data_length + 1
)));
} }
if data_length == 2 { if data_length == 2 {
todo!("Unimplemented"); todo!("Unimplemented");
} else { } else {
let written_value = u32::from_be_bytes([write_buffer[3], write_buffer[4], write_buffer[5], write_buffer[6]]); let written_value = u32::from_be_bytes([
write_buffer[3],
write_buffer[4],
write_buffer[5],
write_buffer[6],
]);
self.data.insert(address, written_value); self.data.insert(address, written_value);
if data_length == 8 { if data_length == 8 {
todo!("Unimplemented"); todo!("Unimplemented");
} }
} }
} else { } else if write_buffer.len() != 3 {
if write_buffer.len() != 3 { return Err(WrappingError(anyhow!(
return Err(WrappingError(anyhow!("Write for read has wrong length {}. {} expected", write_buffer.len(), 3))); "Write for read has wrong length {}. {} expected",
} write_buffer.len(),
3
)));
} }
crc.update(&write_buffer); crc.update(write_buffer);
} }
Operation::Read(read_buffer) => { Operation::Read(read_buffer) => {
if !do_read_operation { if !do_read_operation {
return Err(WrappingError(anyhow!("Unexpected Read Operation"))); return Err(WrappingError(anyhow!("Unexpected Read Operation")));
} }
let expected_length = data_length + if include_crc { 1 } else { 0 }; let expected_length = data_length + usize::from(include_crc);
if read_buffer.len() != expected_length { if read_buffer.len() != expected_length {
return Err(WrappingError(anyhow!("Unexpected length in read buffer {}. {} expected", read_buffer.len(), expected_length))); return Err(WrappingError(anyhow!(
"Unexpected length in read buffer {}. {} expected",
read_buffer.len(),
expected_length
)));
} }
crc.update(&[((i2c_addr as u8) << 1) | 0b1]); crc.update(&[(i2c_addr << 1) | 0b1]);
if data_length == 2 { if data_length == 2 {
todo!("Unimplemented"); todo!("Unimplemented");
} else if data_length == 4 { } else if data_length == 4 {

4
flight/src/hardware/sim/pwm.rs
View File

@@ -10,9 +10,7 @@ pub struct SimPwm {
impl SimPwm { impl SimPwm {
pub fn new() -> Self { pub fn new() -> Self {
trace!("SimPwm::new()"); trace!("SimPwm::new()");
Self { Self { duty_cycle: 0 }
duty_cycle: 0,
}
} }
} }

90
flight/src/lib.rs
View File

@@ -1,10 +1,14 @@
#![warn(
clippy::all,
clippy::pedantic,
)]
use crate::comms::CommsTask; use crate::comms::CommsTask;
use crate::hardware::Hardware;
use crate::hardware::channelization::{LED_A, LED_B}; use crate::hardware::channelization::{LED_A, LED_B};
use crate::hardware::initialize; use crate::hardware::initialize;
use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task}; use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task};
use crate::hardware::mct8316a::Mct8316a; use crate::hardware::mct8316a::Mct8316a;
use crate::hardware::pin::Pin; use crate::hardware::pin::Pin;
use crate::hardware::Hardware;
use crate::scheduler::Scheduler; use crate::scheduler::Scheduler;
use crate::state_vector::StateVector; use crate::state_vector::StateVector;
use anyhow::Result; use anyhow::Result;
@@ -13,13 +17,17 @@ use embedded_hal::pwm::SetDutyCycle;
use log::{debug, info}; use log::{debug, info};
use nautilus_common::add_ctrlc_handler; use nautilus_common::add_ctrlc_handler;
use nautilus_common::telemetry::{SwitchBank, TelemetryMessage}; use nautilus_common::telemetry::{SwitchBank, TelemetryMessage};
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::Arc; use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
use std::thread::sleep; use std::thread::sleep;
use std::time::{Duration, Instant}; use std::time::{Duration, Instant};
mod hardware; mod hardware;
/// Run the flight software
///
/// # Errors
/// An unrecoverable error had occurred
pub fn run() -> Result<()> { pub fn run() -> Result<()> {
info!( info!(
"Project Nautilus Flight Software {}", "Project Nautilus Flight Software {}",
@@ -46,53 +54,77 @@ pub fn run() -> Result<()> {
mcp23017_b.init()?; mcp23017_b.init()?;
mct8316.init()?; mct8316.init()?;
Scheduler::new(running.clone(), |s| { Scheduler::scope(running.clone(), |s| {
let task_a = s.run_cyclic("mcp23017-a-task", Mcp23017Task::new(mcp23017_a, &state_vector), 10)?; let task_a = s.run_cyclic(
"mcp23017-a-task",
Mcp23017Task::new(mcp23017_a, &state_vector),
10,
)?;
let a_id = task_a.get_id(); let a_id = task_a.get_id();
let task_b = s.run_cyclic("mcp23017-b-task", Mcp23017Task::new(mcp23017_b, &state_vector), 10)?; let task_b = s.run_cyclic(
"mcp23017-b-task",
Mcp23017Task::new(mcp23017_b, &state_vector),
10,
)?;
let b_id = task_b.get_id(); let b_id = task_b.get_id();
let comms = s.run_cyclic("comms-task", CommsTask::new(15000, "nautilus-ground:14000", running.clone())?, 10)?; let comms = s.run_cyclic(
"comms-task",
CommsTask::new(15000, "nautilus-ground:14000", running.clone())?,
10,
)?;
let sv = &state_vector; let sv = &state_vector;
s.run_cyclic("telemetry-producer", move || { s.run_cyclic(
sv.access_section(&a_id, |state: &Mcp23017State| { "telemetry-producer",
comms.send(TelemetryMessage::SwitchState { move || {
bank: SwitchBank::A, sv.access_section(&a_id, |state: &Mcp23017State| {
switches: state.pins.clone(), comms.send(TelemetryMessage::SwitchState {
}) bank: SwitchBank::A,
}); switches: state.pins,
sv.access_section(&b_id, |state: &Mcp23017State| { });
comms.send(TelemetryMessage::SwitchState { });
bank: SwitchBank::B, sv.access_section(&b_id, |state: &Mcp23017State| {
switches: state.pins.clone(), comms.send(TelemetryMessage::SwitchState {
}) bank: SwitchBank::B,
}); switches: state.pins,
}, 1)?; });
});
},
1,
)?;
let mut led_pin_a = LED_A.new(&task_a, &task_b)?; let mut led_pin_a = LED_A.get_pin(&task_a, &task_b);
let mut led_pin_b = LED_B.new(&task_a, &task_b)?; let mut led_pin_b = LED_B.get_pin(&task_a, &task_b);
info!("Starting Main Loop"); info!("Starting Main Loop");
loop { loop {
debug!("A On"); debug!("A On");
led_pin_a.set(PinState::High, Instant::now() + Duration::from_secs(2), 0); led_pin_a.set(PinState::High, Instant::now() + Duration::from_secs(2), 0);
sleep(Duration::from_secs(1)); sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; }; if !running.load(Ordering::Relaxed) {
break;
}
debug!("B On"); debug!("B On");
led_pin_b.set(PinState::High, Instant::now() + Duration::from_secs(2), 0); led_pin_b.set(PinState::High, Instant::now() + Duration::from_secs(2), 0);
sleep(Duration::from_secs(1)); sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; }; if !running.load(Ordering::Relaxed) {
break;
}
debug!("A Off"); debug!("A Off");
sleep(Duration::from_secs(1)); sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; }; if !running.load(Ordering::Relaxed) {
break;
}
debug!("B Off"); debug!("B Off");
sleep(Duration::from_secs(1)); sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; }; if !running.load(Ordering::Relaxed) {
break;
}
} }
anyhow::Ok(()) anyhow::Ok(())
@@ -112,11 +144,11 @@ pub fn run() -> Result<()> {
Ok(()) Ok(())
} }
#[cfg(test)] mod comms;
mod test_utils;
mod data; mod data;
mod on_drop; mod on_drop;
mod rcs; mod rcs;
mod comms;
mod scheduler; mod scheduler;
mod state_vector; mod state_vector;
#[cfg(test)]
mod test_utils;

7
flight/src/main.rs
View File

@@ -2,13 +2,16 @@ use log::error;
use nautilus_common::logger::setup_logger; use nautilus_common::logger::setup_logger;
use nautilus_flight::run; use nautilus_flight::run;
fn main() { fn main() {
setup_logger(env!("CARGO_PKG_NAME")).expect("Failed to setup logger"); setup_logger(env!("CARGO_PKG_NAME")).expect("Failed to setup logger");
match run() { match run() {
Ok(_) => {} Ok(_) => {}
Err(err) => { Err(err) => {
error!("An unhandled error occurred: {}\n\n{}", err, err.backtrace()); error!(
"An unhandled error occurred: {}\n\n{}",
err,
err.backtrace()
);
} }
} }
} }

6
flight/src/on_drop.rs
View File

@@ -7,14 +7,12 @@ pub struct OnDrop<F: FnMut()> {
pub fn on_drop<F: FnMut()>(func: F) -> OnDrop<F> { pub fn on_drop<F: FnMut()>(func: F) -> OnDrop<F> {
trace!("on_drop<F={}>()", type_name::<F>()); trace!("on_drop<F={}>()", type_name::<F>());
OnDrop { OnDrop { func }
func
}
} }
impl<F: FnMut()> Drop for OnDrop<F> { impl<F: FnMut()> Drop for OnDrop<F> {
fn drop(&mut self) { fn drop(&mut self) {
trace!("OnDrop<F={}>::drop()", type_name::<F>()); trace!("OnDrop<F={}>::drop()", type_name::<F>());
(self.func)() (self.func)();
} }
} }

49
flight/src/scheduler/mod.rs
View File

@@ -4,11 +4,11 @@ use log::trace;
use std::any::type_name; use std::any::type_name;
use std::fmt::Debug; use std::fmt::Debug;
use std::ops::Deref; use std::ops::Deref;
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::mpsc::{channel, Receiver, Sender};
use std::sync::Arc; use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::mpsc::{Receiver, Sender, channel};
use std::thread; use std::thread;
use std::thread::{sleep, Scope}; use std::thread::{Scope, sleep};
use std::time::{Duration, Instant}; use std::time::{Duration, Instant};
#[derive(Clone, Debug)] #[derive(Clone, Debug)]
@@ -46,13 +46,12 @@ pub trait CyclicTask {
impl<F> CyclicTask for F impl<F> CyclicTask for F
where where
F: Fn() -> (), F: Fn(),
{ {
type Message = (); type Message = ();
type Data = (); type Data = ();
fn get_data(&self) -> Self::Data { fn get_data(&self) -> Self::Data {
()
} }
fn step(&mut self, _receiver: &Receiver<Self::Message>, _step_time: Instant) { fn step(&mut self, _receiver: &Receiver<Self::Message>, _step_time: Instant) {
@@ -60,27 +59,27 @@ where
} }
} }
pub struct Scheduler<'s, 'e> pub struct Scheduler<'s, 'e> {
{
scope: &'s Scope<'s, 'e>, scope: &'s Scope<'s, 'e>,
running: Arc<AtomicBool>, running: Arc<AtomicBool>,
} }
impl<'s, 'e> Scheduler<'s, 'e> { impl<'s> Scheduler<'s, '_> {
pub fn new<'env, F, R>(running: Arc<AtomicBool>, f: F) -> R pub fn scope<'env, F, R>(running: Arc<AtomicBool>, f: F) -> R
where where
F: FnOnce(Scheduler<'_, 'env>) -> R, F: FnOnce(Scheduler<'_, 'env>) -> R,
{ {
trace!("Scheduler::new(running: {running:?}, f)"); trace!("Scheduler::new(running: {running:?}, f)");
thread::scope(|scope: &Scope| { thread::scope(|scope: &Scope| {
let running_result = running.clone();
// This will automatically set running to false when it drops // This will automatically set running to false when it drops
// This means that if the function returns any side branches // This means that if the function returns any side branches
// checking running will shut down // checking running will shut down
let _shutdown_threads = on_drop(|| running.store(false, Ordering::Relaxed)); let _shutdown_threads = on_drop(move || running.store(false, Ordering::Relaxed));
f(Scheduler { f(Scheduler {
scope, scope,
running: running.clone(), running: running_result,
}) })
}) })
} }
@@ -90,12 +89,16 @@ impl<'s, 'e> Scheduler<'s, 'e> {
&self, &self,
name: impl Into<String>, name: impl Into<String>,
task: T, task: T,
) -> Result<TaskHandle<T::Message, T::Data>> where ) -> Result<TaskHandle<T::Message, T::Data>>
where
T: Task + Send + Debug + 's, T: Task + Send + Debug + 's,
T::Message: Send, T::Message: Send,
{ {
let name = name.into(); let name = name.into();
trace!("Scheduler::run<T={}>(name: {name}, task: {task:?})", type_name::<T>()); trace!(
"Scheduler::run<T={}>(name: {name}, task: {task:?})",
type_name::<T>()
);
let running = self.running.clone(); let running = self.running.clone();
let (sender, receiver) = channel::<T::Message>(); let (sender, receiver) = channel::<T::Message>();
let data = task.get_data(); let data = task.get_data();
@@ -104,11 +107,7 @@ impl<'s, 'e> Scheduler<'s, 'e> {
.spawn_scoped(self.scope, move || { .spawn_scoped(self.scope, move || {
task.run(receiver, running); task.run(receiver, running);
})?; })?;
Ok(TaskHandle { Ok(TaskHandle { name, sender, data })
name,
sender,
data,
})
} }
pub fn run_cyclic<T>( pub fn run_cyclic<T>(
@@ -116,12 +115,16 @@ impl<'s, 'e> Scheduler<'s, 'e> {
name: impl Into<String>, name: impl Into<String>,
mut task: T, mut task: T,
frequency: u64, frequency: u64,
) -> Result<TaskHandle<T::Message, T::Data>> where ) -> Result<TaskHandle<T::Message, T::Data>>
where
T: CyclicTask + Send + 's, T: CyclicTask + Send + 's,
T::Message: Send, T::Message: Send,
{ {
let name = name.into(); let name = name.into();
trace!("Scheduler::run_cyclic<T={}>(name: {name}, task, frequency: {frequency})", type_name::<T>()); trace!(
"Scheduler::run_cyclic<T={}>(name: {name}, task, frequency: {frequency})",
type_name::<T>()
);
let running = self.running.clone(); let running = self.running.clone();
let (sender, receiver) = channel::<T::Message>(); let (sender, receiver) = channel::<T::Message>();
let data = task.get_data(); let data = task.get_data();
@@ -140,10 +143,6 @@ impl<'s, 'e> Scheduler<'s, 'e> {
} }
} }
})?; })?;
Ok(TaskHandle { Ok(TaskHandle { name, sender, data })
name,
sender,
data,
})
} }
} }

52
flight/src/state_vector/mod.rs
View File

@@ -1,10 +1,10 @@
use log::trace; use log::trace;
use std::any::{type_name, Any}; use std::any::{Any, type_name};
use std::collections::HashMap; use std::collections::HashMap;
use std::fmt::{Debug, Formatter}; use std::fmt::{Debug, Formatter};
use std::marker::PhantomData; use std::marker::PhantomData;
use std::sync::atomic::{AtomicUsize, Ordering};
use std::sync::RwLock; use std::sync::RwLock;
use std::sync::atomic::{AtomicUsize, Ordering};
#[derive(Debug)] #[derive(Debug)]
pub struct StateVector { pub struct StateVector {
@@ -21,15 +21,24 @@ pub struct SectionWriter<'a, T> {
_phantom_data: PhantomData<T>, _phantom_data: PhantomData<T>,
} }
impl<'a, T> Debug for SectionWriter<'a, T> { impl<T> Debug for SectionWriter<'_, T> {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(f, "SectionWriter<T={}> {{ id: {:?}, state_vector: {:?} }}", type_name::<T>(), self.id, self.state_vector) write!(
f,
"SectionWriter<T={}> {{ id: {:?}, state_vector: {:?} }}",
type_name::<T>(),
self.id,
self.state_vector
)
} }
} }
impl<'a, T: 'static> SectionWriter<'a, T> { impl<T: 'static> SectionWriter<'_, T> {
pub fn get_identifier(&self) -> SectionIdentifier { pub fn get_identifier(&self) -> SectionIdentifier {
trace!("SectionWriter<T={}>::get_identifier(self: {self:?})", type_name::<T>()); trace!(
"SectionWriter<T={}>::get_identifier(self: {self:?})",
type_name::<T>()
);
self.id.clone() self.id.clone()
} }
@@ -37,7 +46,10 @@ impl<'a, T: 'static> SectionWriter<'a, T> {
where where
F: FnOnce(&mut T) -> R, F: FnOnce(&mut T) -> R,
{ {
trace!("SectionWriter<T={}>::update(self: {self:?}, f)", type_name::<T>()); trace!(
"SectionWriter<T={}>::update(self: {self:?}, f)",
type_name::<T>()
);
self.state_vector.sections.clear_poison(); self.state_vector.sections.clear_poison();
let sections = self.state_vector.sections.read().unwrap(); let sections = self.state_vector.sections.read().unwrap();
let section = sections.get(&self.id).unwrap(); let section = sections.get(&self.id).unwrap();
@@ -60,7 +72,10 @@ impl StateVector {
where where
T: Send + Sync + 'static, T: Send + Sync + 'static,
{ {
trace!("StateVector::create_section<T={}>(self: {self:?}, initial_value)", type_name::<T>()); trace!(
"StateVector::create_section<T={}>(self: {self:?}, initial_value)",
type_name::<T>()
);
let id = SectionIdentifier(self.next_section.fetch_add(1usize, Ordering::SeqCst)); let id = SectionIdentifier(self.next_section.fetch_add(1usize, Ordering::SeqCst));
let lock = Box::new(RwLock::new(initial_value)); let lock = Box::new(RwLock::new(initial_value));
@@ -74,7 +89,7 @@ impl StateVector {
SectionWriter { SectionWriter {
id, id,
state_vector: &self, state_vector: self,
_phantom_data: PhantomData, _phantom_data: PhantomData,
} }
} }
@@ -84,13 +99,22 @@ impl StateVector {
T: 'static, T: 'static,
F: FnOnce(&T) -> R, F: FnOnce(&T) -> R,
{ {
trace!("StateVector::access_section<T={}, F={}, R={}>(self: {self:?}, id: {id:?}, f)", type_name::<T>(), type_name::<F>(), type_name::<R>()); trace!(
"StateVector::access_section<T={}, F={}, R={}>(self: {self:?}, id: {id:?}, f)",
type_name::<T>(),
type_name::<F>(),
type_name::<R>()
);
self.sections.clear_poison(); self.sections.clear_poison();
let Ok(sections) = self.sections.read() else { return None; }; let Ok(sections) = self.sections.read() else {
let Some(section) = sections.get(id) else { return None; }; return None;
};
let section = sections.get(id)?;
section.clear_poison(); section.clear_poison();
let Ok(data) = section.read() else { return None; }; let Ok(data) = section.read() else {
let Some(inner) = data.downcast_ref::<T>() else { return None; }; return None;
};
let inner = data.downcast_ref::<T>()?;
Some(f(inner)) Some(f(inner))
} }
} }

8
flight/src/test_utils.rs
View File

@@ -10,7 +10,7 @@ pub struct SilentDrop<T: panic::UnwindSafe> {
impl<T: panic::UnwindSafe> SilentDrop<T> { impl<T: panic::UnwindSafe> SilentDrop<T> {
pub fn new(inner: T) -> Self { pub fn new(inner: T) -> Self {
Self { Self {
inner: ManuallyDrop::new(inner) inner: ManuallyDrop::new(inner),
} }
} }
} }
@@ -33,13 +33,11 @@ impl<T: panic::UnwindSafe> Drop for SilentDrop<T> {
fn drop(&mut self) { fn drop(&mut self) {
let prev_hook = panic::take_hook(); let prev_hook = panic::take_hook();
panic::set_hook(Box::new(|_| {})); panic::set_hook(Box::new(|_| {}));
let inner = unsafe { let inner = unsafe { ManuallyDrop::take(&mut self.inner) };
ManuallyDrop::take(&mut self.inner)
};
let destroy_closure = || { let destroy_closure = || {
drop(inner); drop(inner);
}; };
if panic::catch_unwind(destroy_closure).is_err() {}; if panic::catch_unwind(destroy_closure).is_err() {};
panic::set_hook(prev_hook); panic::set_hook(prev_hook);
} }
} }

14
ground/src/lib.rs
View File

@@ -1,3 +1,7 @@
#![warn(
clippy::all,
clippy::pedantic,
)]
use anyhow::Result; use anyhow::Result;
use log::{error, info}; use log::{error, info};
use nautilus_common::add_ctrlc_handler; use nautilus_common::add_ctrlc_handler;
@@ -6,10 +10,14 @@ use nautilus_common::telemetry::Telemetry;
use nautilus_common::udp::{UdpRecvCborError, UdpSocketExt}; use nautilus_common::udp::{UdpRecvCborError, UdpSocketExt};
use std::io::Cursor; use std::io::Cursor;
use std::net::{IpAddr, Ipv4Addr, SocketAddr, UdpSocket}; use std::net::{IpAddr, Ipv4Addr, SocketAddr, UdpSocket};
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::Arc; use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
use std::time::Duration; use std::time::Duration;
/// Run the Ground Software
///
/// # Errors
/// If any unhandled error occurred in the Ground Software
pub fn run() -> Result<()> { pub fn run() -> Result<()> {
info!( info!(
"Project Nautilus Ground Software {}", "Project Nautilus Ground Software {}",
@@ -43,8 +51,8 @@ pub fn run() -> Result<()> {
if let Some(flight_addr) = flight_addr { if let Some(flight_addr) = flight_addr {
let cmd = Command::Shutdown; let cmd = Command::Shutdown;
udp.send_cbor(&cmd, &mut buffer, &flight_addr)?; udp.send_cbor(&cmd, &mut buffer, flight_addr)?;
} }
Ok(()) Ok(())
} }

6
ground/src/main.rs
View File

@@ -7,7 +7,11 @@ fn main() {
match run() { match run() {
Ok(_) => {} Ok(_) => {}
Err(err) => { Err(err) => {
error!("An unhandled error occurred: {}\n\n{}", err, err.backtrace()); error!(
"An unhandled error occurred: {}\n\n{}",
err,
err.backtrace()
);
} }
} }
} }