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]
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 std::sync::atomic::{AtomicBool, Ordering};
use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
pub mod logger;
pub mod command;
pub mod logger;
pub mod telemetry;
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<()> {
ctrlc::set_handler(move || {
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::{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<()> {
let log_file = env::var("LOG_FILE").or_else(|_| {
create_dir_all("logs/")?;
@@ -37,7 +41,7 @@ pub fn setup_logger(package_name: &'static str) -> Result<()> {
level = colors.color(record.level()),
time = chrono::Local::now().format("%Y-%m-%dT%H:%M:%S%.9f"),
target = record.target(),
))
));
})
.chain(
fern::Dispatch::new()
@@ -47,6 +51,6 @@ pub fn setup_logger(package_name: &'static str) -> Result<()> {
.chain(fern::log_file(log_file.clone())?)
.apply()?;
debug!("Logging to {} at level {}", log_file, log_level);
debug!("Logging to {log_file} at level {log_level}");
Ok(())
}

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

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

94
common/src/udp.rs
View File

@@ -1,7 +1,7 @@
use crate::udp::UdpSendCborError::LengthMismatch;
use log::error;
use serde::de::DeserializeOwned;
use serde::Serialize;
use serde::de::DeserializeOwned;
use std::io::{Cursor, ErrorKind};
use std::net::{SocketAddr, ToSocketAddrs, UdpSocket};
use thiserror::Error;
@@ -23,58 +23,76 @@ pub enum UdpSendCborError {
#[error("Serialization Error")]
Serialization(#[from] ciborium::ser::Error<std::io::Error>),
#[error("Length Mismatch")]
LengthMismatch {
expected: usize,
actual: usize,
},
LengthMismatch { expected: usize, actual: usize },
}
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 {
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);
match self.recv_from(buffer.get_mut()) {
Ok((size, addr)) => {
match ciborium::from_reader::<T, _>(&buffer.get_ref()[..size]) {
Ok(res) => Ok((res, addr)),
Err(err) => Err(err.into()),
}
}
Err(err) => {
match err.kind() {
ErrorKind::WouldBlock | ErrorKind::TimedOut => {
Err(UdpRecvCborError::NoData)
}
_ => Err(err.into())
}
}
Ok((size, addr)) => match ciborium::from_reader::<T, _>(&buffer.get_ref()[..size]) {
Ok(res) => Ok((res, addr)),
Err(err) => Err(err.into()),
},
Err(err) => 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);
match ciborium::into_writer(data, &mut buffer) {
Ok(_) => match self.send_to(&buffer.get_ref()[..buffer.position() as usize], addr) {
Ok(size) => {
if buffer.position() as usize != size {
return Err(LengthMismatch {
expected: buffer.position() as usize,
actual: size,
});
Ok(()) => {
let size_encoded = usize::try_from(buffer.position())
.expect("Values greater than u32 are not expected anyway");
match self.send_to(&buffer.get_ref()[..size_encoded], addr) {
Ok(size_sent) => {
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::io::Cursor;
use std::net::{IpAddr, Ipv4Addr, SocketAddr, ToSocketAddrs, UdpSocket};
use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::mpsc::Receiver;
use std::sync::Arc;
use std::time::Instant;
pub type TelemetrySender = TaskHandle<Telemetry, ()>;
@@ -34,12 +34,11 @@ pub struct CommsTask<A: ToSocketAddrs + Debug> {
}
impl<A: ToSocketAddrs + Debug> CommsTask<A> {
pub fn new(
local_port: u16,
ground_address: A,
running: Arc<AtomicBool>,
) -> Result<Self> {
trace!("CommsTask::new<A={}>(local_port: {local_port}, ground_address: {ground_address:?})", type_name::<A>());
pub fn new(local_port: u16, ground_address: A, running: Arc<AtomicBool>) -> 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::V6(Ipv6Addr::UNSPECIFIED), local_port);
let udp = UdpSocket::bind(bind_addr)?;
@@ -57,20 +56,23 @@ impl<A: ToSocketAddrs + Debug> CyclicTask for CommsTask<A> {
type 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) {
trace!("CommsTask<A={}>::step(self: {self:?}, receiver: {receiver:?}, _step_time: {_step_time:?})", type_name::<A>());
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>()
);
let mut buffer = Cursor::new([0u8; 512]);
match self.udp.recv_cbor::<Command, _>(&mut buffer) {
Ok((cmd, _)) => {
match cmd {
Command::Shutdown => self.running.store(false, Ordering::Relaxed),
}
}
Ok((cmd, _)) => match cmd {
Command::Shutdown => self.running.store(false, Ordering::Relaxed),
},
Err(UdpRecvCborError::NoData) => {}
Err(err) => {
error!("Rx error: {err}");

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

@@ -1,7 +1,6 @@
#![allow(dead_code)]
use crate::hardware::pin::{Pin, PinDevice};
use anyhow::Result;
use embedded_hal::digital::PinState;
use log::trace;
use std::any::type_name;
@@ -52,9 +51,12 @@ pub struct DevicePin<'a, Device: PinDevice> {
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) {
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);
}
}
@@ -64,9 +66,13 @@ pub enum ChannelPin<'a, A: PinDevice, B: PinDevice> {
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) {
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 {
ChannelPin::ExtA(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 {
pub fn new<'a>(self, ext_a: &'a (impl PinDevice + Debug), ext_b: &'a (impl PinDevice + Debug)) -> Result<impl Pin> {
trace!("PinoutChannel::new(self: {self:?}, ext_a: {ext_a:?}, ext_b: {ext_b:?}");
Ok(match self {
pub fn get_pin<'a>(
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::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,
{
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,
{
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 {
i2c: i2c.into(),
address,
@@ -62,7 +70,7 @@ where
trace!("Mcp23017Driver::drop(self: {self:?})");
self.bank = 0; // We want all pins to be set back to 0
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 task;
use anyhow::Result;
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
// If the current is currently invalid, we'd upgrade the next to current
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 {
self.value = self.state;
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;
}
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
trace!("PinData::set(self: {self:?}, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})");
let can_replace_current = self.valid_until.map(|current| current <= valid_until).unwrap_or(true);
let can_replace_next = self.next_validity.map(|next| next <= valid_until).unwrap_or(true);
trace!(
"PinData::set(self: {self:?}, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})"
);
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 {
// This is now the highest priority thing (or most recent of equal priority)
if can_replace_current {
if can_replace_next {
self.next_validity = None;
self.next_priority = 0
self.next_priority = 0;
}
} else {
self.next_state = self.state;
@@ -182,15 +190,11 @@ impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
fn get_data(&self) -> Self::Data {
trace!("Mcp23017Task::get_data(self: {self:?})");
Self::Data {
id: self.state.get_identifier()
id: self.state.get_identifier(),
}
}
fn step(
&mut self,
receiver: &Receiver<Self::Message>,
step_time: Instant,
) {
fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant) {
trace!("Mcp23017Task::step(self: {self:?}, receiver, step_time: {step_time:?})");
let mut changed = false;
@@ -200,7 +204,12 @@ impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
while let Ok(recv) = receiver.try_recv() {
match recv {
Mcp23017Message::SetPin { pin, value, valid_until, priority } => {
Mcp23017Message::SetPin {
pin,
value,
valid_until,
priority,
} => {
if (0u8..16u8).contains(&pin) {
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 anyhow::{ensure, Result};
use anyhow::{Result, ensure};
use embedded_hal::spi::SpiDevice;
use log::trace;
use std::any::type_name;
@@ -12,7 +12,12 @@ pub struct Mcp3208<SPI> {
impl<SPI> Debug for Mcp3208<SPI> {
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,
{
pub fn new(spi: SPI, vref: f64) -> Self {
trace!("Mcp3208<SPI={}>::new(spi, vref: {vref})", type_name::<SPI>());
Self {
spi,
vref,
}
trace!(
"Mcp3208<SPI={}>::new(spi, vref: {vref})",
type_name::<SPI>()
);
Self { spi, vref }
}
pub fn read_single(&mut self, channel: u8) -> Result<f64> {
@@ -41,10 +46,12 @@ where
write_bits[1] |= (channel.unbounded_shl(6)) & 0xFF;
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;
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,
}
#[derive(Debug, Clone)]
pub struct ClosedLoop3 {
pub degauss_samples: DegaussSamples,
@@ -481,5 +480,3 @@ pub enum FastBrakeDelta {
Percent4 = 0x6,
Percent5 = 0x7,
}

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

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

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

@@ -1,18 +1,57 @@
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_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::fault_config::{AbnormalSpeedLock, AbnormalSpeedLockThreshold, AutomaticRetries, CycleByCycleCurrentLimit, FaultConfig1, FaultConfig2, LockDetectionCurrentLimit, LockDetectionCurrentLimitDeglitchTime, LockMinSpeed, LockMode, LockRetryTime, LossSyncTimes, MaxMotorVoltage, MinMotorVoltage, MotorVoltageMode, NoMotorDetectDeglitchTime, NoMotorThreshold, ZeroDutyThreshold};
use crate::hardware::mct8316a::gate_driver_config::{BuckCurrentLimit, BuckSlewRate, BuckVoltageSelection, CurrentSenseAmplifier, 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 crate::hardware::mct8316a::Mct8316a;
use anyhow::{bail, ensure, Result};
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::gate_driver_config::{
BuckCurrentLimit, BuckSlewRate, BuckVoltageSelection, CurrentSenseAmplifier,
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 log::trace;
use std::any::type_name;
@@ -39,7 +78,7 @@ impl Display for Mct8316AVData {
}
}
#[derive(Debug)]
#[derive(Debug, Copy, Clone)]
enum OperationRW {
Read,
Write,
@@ -51,17 +90,16 @@ fn control_word(
data: Mct8316AVData,
address: u32,
) -> [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; _];
control_word[0] |= match operation_rw {
OperationRW::Read => 0x80,
OperationRW::Write => 0x00,
};
control_word[0] |= match crc {
true => 0x40,
false => 0x00,
};
control_word[0] |= if crc { 0x40 } else { 0x00 };
control_word[0] |= match data {
Mct8316AVData::Two(_) => 0x00,
Mct8316AVData::Four(_) => 0x10,
@@ -69,7 +107,7 @@ fn control_word(
};
control_word[0] |= ((address >> 16) & 0x0F) as u8;
control_word[1] |= ((address >> 8) & 0xFF) as u8;
control_word[2] |= ((address >> 0) & 0xFF) as u8;
control_word[2] |= ((address) & 0xFF) as u8;
control_word
}
@@ -93,7 +131,12 @@ where
I2C::Error: 'static,
{
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,
{
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 {
i2c: i2c.into(),
address,
}
}
#[allow(clippy::identity_op)]
pub(super) fn write(&self, address: u32, data: Mct8316AVData) -> Result<()> {
trace!("Mct8316AVDriver::write(self: {self:?}, address: {address:06x}, data: {data})");
@@ -121,12 +169,7 @@ where
// 1 for crc
let mut write_data = [0u8; 1 + 3 + 8 + 1];
write_data[0] = (self.address << 1) | 0b0;
write_data[1..4].copy_from_slice(&control_word(
OperationRW::Write,
true,
data,
address,
));
write_data[1..4].copy_from_slice(&control_word(OperationRW::Write, true, data, address));
let data_length = match data {
Mct8316AVData::Two(val) => {
write_data[4..6].copy_from_slice(&val.to_be_bytes());
@@ -145,13 +188,16 @@ where
write_data[4 + data_length] = crc_result;
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"),
}
Ok(())
}
#[allow(clippy::identity_op)]
pub(super) fn read(&self, address: u32, data: &mut Mct8316AVData) -> Result<()> {
trace!("Mct8316AVDriver::read(self: {self:?}, address: {address:06x}, data: {data})");
// 1 target id
@@ -160,13 +206,9 @@ where
// 8 data bytes
// 1 crc
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[1..4].copy_from_slice(&control_word(
OperationRW::Read,
true,
*data,
address,
));
read_data[1..4].copy_from_slice(&control_word(OperationRW::Read, true, *data, address));
read_data[4] = (self.address << 1) | 0b1;
let data_length = match data {
Mct8316AVData::Two(_) => 2,
@@ -178,32 +220,42 @@ where
let mut i2c_ops = [
Operation::Write(&left[1..4]),
Operation::Read(&mut right[..(data_length + 1)])
Operation::Read(&mut right[..=data_length]),
];
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"),
}
drop(i2c_ops);
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 {
Mct8316AVData::Two(val) => {
*val = u16::from_be_bytes([read_data[5], read_data[6]]);
}
Mct8316AVData::Four(val) => {
*val = u32::from_be_bytes([read_data[5], read_data[6],
read_data[7], read_data[8]]);
*val = u32::from_be_bytes([read_data[5], read_data[6], read_data[7], read_data[8]]);
}
Mct8316AVData::Eight(val) => {
*val = u64::from_be_bytes([read_data[5], read_data[6],
read_data[7], read_data[8],
read_data[9], read_data[10],
read_data[11], read_data[12]]);
*val = u64::from_be_bytes([
read_data[5],
read_data[6],
read_data[7],
read_data[8],
read_data[9],
read_data[10],
read_data[11],
read_data[12],
]);
}
}
@@ -218,6 +270,7 @@ where
I2C::Error: Sync,
I2C::Error: 'static,
{
#[allow(clippy::too_many_lines)]
fn init(&mut self) -> Result<()> {
trace!("Mct8316AVDriver::init(self: {self:?})");
@@ -312,7 +365,7 @@ where
})?
.set_constant_speed(ConstantSpeed {
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_lower_limit: LowerPercentLimit::Percent10,
mode: ClosedLoopMode::Disabled,
@@ -365,10 +418,12 @@ where
})?
.set_fault_config1(FaultConfig1 {
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_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,
motor_lock_mode: LockMode::RecoverRetryTristated,
lock_retry_time: LockRetryTime::Milliseconds5000,
@@ -421,7 +476,8 @@ where
overvoltage_protection_level: OvervoltageProtectionLevel::Volts32,
overvoltage_protection_enable: true,
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_level: OvercurrentProtectionLevel::Amps16,
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::constant_power::ConstantPower;
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::isd_config::IsdConfig;
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::trap_config::{TrapConfig1, TrapConfig2};
use crate::hardware::mct8316a::Mct8316AVDriver;
use anyhow::Result;
use embedded_hal::i2c::I2c;
use log::trace;
@@ -30,7 +32,7 @@ where
modified: bool,
}
impl<'a, I2C> Debug for Mct8316AVEeprom<'a, I2C>
impl<I2C> Debug for Mct8316AVEeprom<'_, I2C>
where
I2C: I2c + Send + Sync,
I2C::Error: Send,
@@ -38,10 +40,18 @@ where
I2C::Error: 'static,
{
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>
where
I2C: I2c + Send + Sync,
@@ -50,9 +60,12 @@ where
I2C::Error: 'static,
{
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
sleep(Duration::from_millis(100));
@@ -65,75 +78,105 @@ where
pub fn set_isd_config(self, isd_config: IsdConfig) -> Result<Self> {
trace!("Mct8316AVEeprom::set_isd_config(self: {self:?}, isd_config: {isd_config:?})");
let expected_value =
if isd_config.enable_isd { 0x40000000 } else { 0 }
| if isd_config.enable_brake { 0x20000000 } else { 0 }
| if isd_config.enable_high_impedance { 0x10000000 } else { 0 }
| if isd_config.enable_reverse_drive { 0x08000000 } else { 0 }
| if isd_config.enable_resynchronization { 0x04000000 } else { 0 }
| if isd_config.enable_stationary_brake { 0x02000000 } else { 0 }
| (((isd_config.stationary_detect_threshold as u32) & 0x7) << 22)
| ((isd_config.brake_mode as u32) & 0x1) << 21
| ((isd_config.brake_config as u32) & 0x1) << 20
| ((isd_config.brake_current_threshold as u32) & 0x7) << 17
| ((isd_config.brake_time as u32) & 0xF) << 13
| ((isd_config.high_impedence_time as u32) & 0xF) << 9
| ((isd_config.startup_break_time as u32) & 0x7) << 6
| ((isd_config.resync_minimum_threshold as u32) & 0x7) << 3;
let expected_value = if isd_config.enable_isd { 0x4000_0000 } else { 0 }
| if isd_config.enable_brake {
0x2000_0000
} else {
0
}
| if isd_config.enable_high_impedance {
0x1000_0000
} else {
0
}
| if isd_config.enable_reverse_drive {
0x0800_0000
} else {
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))
}
#[allow(clippy::identity_op)]
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 =
((motor_startup1.motor_startup_method as u32) & 0x3) << 29
| ((motor_startup1.align_ramp_rate as u32) & 0xF) << 25
| ((motor_startup1.align_time as u32) & 0xF) << 21
| ((motor_startup1.align_current_threshold as u32) & 0xF) << 17
| ((motor_startup1.ipd_clock_frequency as u32) & 0x7) << 14
| ((motor_startup1.ipd_current_threshold as u32) & 0xF) << 10
| ((motor_startup1.ipd_release_mode as u32) & 0x3) << 8
| ((motor_startup1.ipd_advance_angle as u32) & 0x3) << 6
| ((motor_startup1.ipd_repeat as u32) & 0x3) << 4
| ((motor_startup1.slow_first_cycle_frequency as u32) & 0xF) << 0;
let expected_value = ((motor_startup1.motor_startup_method as u32) & 0x3) << 29
| ((motor_startup1.align_ramp_rate as u32) & 0xF) << 25
| ((motor_startup1.align_time as u32) & 0xF) << 21
| ((motor_startup1.align_current_threshold as u32) & 0xF) << 17
| ((motor_startup1.ipd_clock_frequency as u32) & 0x7) << 14
| ((motor_startup1.ipd_current_threshold as u32) & 0xF) << 10
| ((motor_startup1.ipd_release_mode as u32) & 0x3) << 8
| ((motor_startup1.ipd_advance_angle as u32) & 0x3) << 6
| ((motor_startup1.ipd_repeat as u32) & 0x3) << 4
| ((motor_startup1.slow_first_cycle_frequency as u32) & 0xF) << 0;
self.assert_register(0x82, Mct8316AVData::Four(expected_value))
}
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 =
((motor_startup2.open_loop_current_limit_mode as u32) & 0x1) << 30
| ((motor_startup2.open_loop_duty_cycle as u32) & 0x7) << 27
| ((motor_startup2.open_loop_current_limit as u32) & 0xF) << 23
| ((motor_startup2.open_loop_acceleration1 as u32) & 0x1F) << 18
| ((motor_startup2.open_loop_acceleration2 as u32) & 0x1F) << 13
| ((motor_startup2.open_closed_handoff_threshold as u32) & 0x1F) << 8
| if motor_startup2.auto_handoff_enable { 1u32 << 7 } else { 0u32 }
| ((motor_startup2.first_cycle_frequency_select as u32) & 0x1) << 6
| ((motor_startup2.minimum_duty_cycle as u32) & 0xF) << 2;
let expected_value = ((motor_startup2.open_loop_current_limit_mode as u32) & 0x1) << 30
| ((motor_startup2.open_loop_duty_cycle as u32) & 0x7) << 27
| ((motor_startup2.open_loop_current_limit as u32) & 0xF) << 23
| ((motor_startup2.open_loop_acceleration1 as u32) & 0x1F) << 18
| ((motor_startup2.open_loop_acceleration2 as u32) & 0x1F) << 13
| ((motor_startup2.open_closed_handoff_threshold as u32) & 0x1F) << 8
| if motor_startup2.auto_handoff_enable {
1u32 << 7
} else {
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))
}
// 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> {
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 =
((closed_loop1.commutation_mode as u32) & 0x2) << 29
| ((closed_loop1.closed_loop_acceleration_rate as u32) & 0x1F) << 24
| ((closed_loop1.closed_loop_deceleration_mode as u32) & 0x1) << 23
| ((closed_loop1.closed_loop_deceleration_rate as u32) & 0x1F) << 18
| ((closed_loop1.pwm_frequency as u32) & 0x1F) << 13
| ((closed_loop1.pwm_modulation as u32) & 0x3) << 11
| ((closed_loop1.pwm_mode as u32) & 0x1) << 10
| ((closed_loop1.lead_angle_polarity as u32) & 0x1) << 9
| (lead_angle as u32) << 1;
let expected_value = ((closed_loop1.commutation_mode as u32) & 0x2) << 29
| ((closed_loop1.closed_loop_acceleration_rate as u32) & 0x1F) << 24
| ((closed_loop1.closed_loop_deceleration_mode as u32) & 0x1) << 23
| ((closed_loop1.closed_loop_deceleration_rate as u32) & 0x1F) << 18
| ((closed_loop1.pwm_frequency as u32) & 0x1F) << 13
| ((closed_loop1.pwm_modulation as u32) & 0x3) << 11
| ((closed_loop1.pwm_mode as u32) & 0x1) << 10
| ((closed_loop1.lead_angle_polarity as u32) & 0x1) << 9
| u32::from(lead_angle) << 1;
self.assert_register(0x86, Mct8316AVData::Four(expected_value))
}
@@ -141,17 +184,20 @@ where
pub fn set_closed_loop2(self, closed_loop2: ClosedLoop2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_closed_loop2(self: {self:?}, closed_loop2: {closed_loop2:?})");
let expected_value =
((closed_loop2.speed_feedback_mode as u32) & 0x2) << 29
| ((closed_loop2.speed_feedback_division as u32) & 0xF) << 25
| ((closed_loop2.speed_feedback_config as u32) & 0x1) << 24
| ((closed_loop2.bemf_threshold as u32) & 0x7) << 21
| ((closed_loop2.motor_stop_mode as u32) & 0x7) << 18
| ((closed_loop2.motor_stop_brake_time as u32) & 0xF) << 14
| ((closed_loop2.active_low_high_brake_threshold as u32) & 0x7) << 11
| ((closed_loop2.brake_pin_threshold as u32) & 0x7) << 8
| if closed_loop2.avs_enable { 1u32 << 7 } else { 0u32 }
| ((closed_loop2.cycle_current_limit as u32) & 0xF) << 3;
let expected_value = ((closed_loop2.speed_feedback_mode as u32) & 0x2) << 29
| ((closed_loop2.speed_feedback_division as u32) & 0xF) << 25
| ((closed_loop2.speed_feedback_config as u32) & 0x1) << 24
| ((closed_loop2.bemf_threshold as u32) & 0x7) << 21
| ((closed_loop2.motor_stop_mode as u32) & 0x7) << 18
| ((closed_loop2.motor_stop_brake_time as u32) & 0xF) << 14
| ((closed_loop2.active_low_high_brake_threshold as u32) & 0x7) << 11
| ((closed_loop2.brake_pin_threshold as u32) & 0x7) << 8
| if closed_loop2.avs_enable {
1u32 << 7
} else {
0u32
}
| ((closed_loop2.cycle_current_limit as u32) & 0xF) << 3;
self.assert_register(0x88, Mct8316AVData::Four(expected_value))
}
@@ -159,67 +205,101 @@ where
pub fn set_closed_loop3(self, closed_loop3: ClosedLoop3) -> Result<Self> {
trace!("Mct8316AVEeprom::set_closed_loop3(self: {self:?}, closed_loop3: {closed_loop3:?})");
let expected_value =
((closed_loop3.degauss_samples as u32) & 0x2) << 29
| ((closed_loop3.degauss_upper_bound as u32) & 0x3) << 27
| ((closed_loop3.degauss_lower_bound as u32) & 0x3) << 25
| ((closed_loop3.integration_cycle_low_threshold as u32) & 0x3) << 23
| ((closed_loop3.integration_cycle_high_threshold as u32) & 0x3) << 21
| ((closed_loop3.integration_duty_cycle_low_threshold as u32) & 0x3) << 19
| ((closed_loop3.integration_duty_cycle_high_threshold as u32) & 0x3) << 17
| ((closed_loop3.bemf_threshold2 as u32) & 0x3F) << 11
| ((closed_loop3.bemf_threshold1 as u32) & 0x3F) << 5
| ((closed_loop3.commutation_method as u32) & 0x1) << 4
| ((closed_loop3.degauss_window as u32) & 0x7) << 1
| if closed_loop3.degauss_enable { 1 } else { 0u32 };
let expected_value = ((closed_loop3.degauss_samples as u32) & 0x2) << 29
| ((closed_loop3.degauss_upper_bound as u32) & 0x3) << 27
| ((closed_loop3.degauss_lower_bound as u32) & 0x3) << 25
| ((closed_loop3.integration_cycle_low_threshold as u32) & 0x3) << 23
| ((closed_loop3.integration_cycle_high_threshold as u32) & 0x3) << 21
| ((closed_loop3.integration_duty_cycle_low_threshold as u32) & 0x3) << 19
| ((closed_loop3.integration_duty_cycle_high_threshold as u32) & 0x3) << 17
| ((closed_loop3.bemf_threshold2 as u32) & 0x3F) << 11
| ((closed_loop3.bemf_threshold1 as u32) & 0x3F) << 5
| ((closed_loop3.commutation_method as u32) & 0x1) << 4
| ((closed_loop3.degauss_window as u32) & 0x7) << 1
| u32::from(closed_loop3.degauss_enable);
self.assert_register(0x8A, Mct8316AVData::Four(expected_value))
}
#[allow(clippy::identity_op)]
pub fn set_closed_loop4(self, closed_loop4: ClosedLoop4) -> Result<Self> {
trace!("Mct8316AVEeprom::set_closed_loop4(self: {self:?}, closed_loop4: {closed_loop4:?})");
let expected_value =
if closed_loop4.wcomp_blanking_enable { 1u32 << 19 } else { 0u32 }
| ((closed_loop4.fast_deceleration_duty_window as u32) & 0x7) << 16
| ((closed_loop4.fast_deceleration_duty_threshold as u32) & 0x7) << 13
| ((closed_loop4.dynamic_brake_current_lower_threshold as u32) & 0x7) << 9
| if closed_loop4.dynamic_braking_current_enable { 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;
let expected_value = if closed_loop4.wcomp_blanking_enable {
1u32 << 19
} else {
0u32
} | ((closed_loop4.fast_deceleration_duty_window as u32) & 0x7) << 16
| ((closed_loop4.fast_deceleration_duty_threshold as u32) & 0x7) << 13
| ((closed_loop4.dynamic_brake_current_lower_threshold as u32) & 0x7) << 9
| if closed_loop4.dynamic_braking_current_enable {
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))
}
// 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> {
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_ki = (constant_speed.speed_power_ki * 1000000f64).round().clamp(0.0f64, 0xFFF as f64) as u32;
let speed_power_kp = (constant_speed.speed_power_kp * 10000f64)
.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 =
speed_power_kp << 20
| speed_power_ki << 8
| ((constant_speed.speed_power_upper_limit as u32) & 0x7) << 5
| ((constant_speed.speed_power_lower_limit as u32) & 0x7) << 2
| ((constant_speed.mode as u32) & 0x3) << 9;
let expected_value = speed_power_kp << 20
| speed_power_ki << 8
| ((constant_speed.speed_power_upper_limit as u32) & 0x7) << 5
| ((constant_speed.speed_power_lower_limit as u32) & 0x7) << 2
| ((constant_speed.mode as u32) & 0x3) << 9;
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> {
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_power = (constant_power.max_power * 4f64).round().clamp(0.0f64, 0x3FF as f64) as u32;
let max_speed = (constant_power.max_speed * 16f64)
.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 =
max_speed << 15
| if constant_power.dead_time_compensation_enable { 1u32 << 14 } else { 0u32 }
| max_power << 4
| ((constant_power.power_hysteresis as u32) & 0x3) << 2
| ((constant_power.mode as u32) & 0x3) << 0;
let expected_value = max_speed << 15
| if constant_power.dead_time_compensation_enable {
1u32 << 14
} else {
0u32
}
| 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))
}
@@ -227,15 +307,14 @@ where
pub fn set_two_phase_profile(self, profile: TwoPhase150DegreeProfile) -> Result<Self> {
trace!("Mct8316AVEeprom::set_two_phase_profile(self: {self:?}, profile: {profile:?})");
let expected_value =
((profile.steps[0] as u32) & 0x7) << 28
| ((profile.steps[1] as u32) & 0x7) << 25
| ((profile.steps[2] as u32) & 0x7) << 22
| ((profile.steps[3] as u32) & 0x7) << 19
| ((profile.steps[4] as u32) & 0x7) << 16
| ((profile.steps[5] as u32) & 0x7) << 13
| ((profile.steps[6] as u32) & 0x7) << 10
| ((profile.steps[7] as u32) & 0x7) << 7;
let expected_value = ((profile.steps[0] as u32) & 0x7) << 28
| ((profile.steps[1] as u32) & 0x7) << 25
| ((profile.steps[2] as u32) & 0x7) << 22
| ((profile.steps[3] as u32) & 0x7) << 19
| ((profile.steps[4] as u32) & 0x7) << 16
| ((profile.steps[5] as u32) & 0x7) << 13
| ((profile.steps[6] as u32) & 0x7) << 10
| ((profile.steps[7] as u32) & 0x7) << 7;
self.assert_register(0x96, Mct8316AVData::Four(expected_value))
}
@@ -243,32 +322,30 @@ where
pub fn set_three_phase_profile(self, profile: ThreePhase150DegreeProfile) -> Result<Self> {
trace!("Mct8316AVEeprom::set_three_phase_profile(self: {self:?}, profile: {profile:?})");
let expected_value =
((profile.steps[0] as u32) & 0x7) << 28
| ((profile.steps[1] as u32) & 0x7) << 25
| ((profile.steps[2] as u32) & 0x7) << 22
| ((profile.steps[3] as u32) & 0x7) << 19
| ((profile.steps[4] as u32) & 0x7) << 16
| ((profile.steps[5] as u32) & 0x7) << 13
| ((profile.steps[6] as u32) & 0x7) << 10
| ((profile.steps[7] as u32) & 0x7) << 7
| ((profile.lead_angle as u32) & 0x3) << 5;
let expected_value = ((profile.steps[0] as u32) & 0x7) << 28
| ((profile.steps[1] as u32) & 0x7) << 25
| ((profile.steps[2] as u32) & 0x7) << 22
| ((profile.steps[3] as u32) & 0x7) << 19
| ((profile.steps[4] as u32) & 0x7) << 16
| ((profile.steps[5] as u32) & 0x7) << 13
| ((profile.steps[6] as u32) & 0x7) << 10
| ((profile.steps[7] as u32) & 0x7) << 7
| ((profile.lead_angle as u32) & 0x3) << 5;
self.assert_register(0x98, Mct8316AVData::Four(expected_value))
}
#[allow(clippy::identity_op)]
pub fn set_trap_config1(self, trap_config1: TrapConfig1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_trap_config1(self: {self:?}, trap_config1: {trap_config1:?})");
let expected_value =
((trap_config1.open_loop_handoff_cycles as u32) & 0x3) << 22
| ((trap_config1.avs_negative_current_limit as u32) & 0x7) << 16
| ((trap_config1.avs_limit_hysteresis as u32) & 0x1) << 15
| ((trap_config1.isd_bemf_threshold as u32) & 0x1F) << 10
| ((trap_config1.isd_cycle_threshold as u32) & 0x7) << 7
| ((trap_config1.open_loop_zc_detection_threshold as u32) & 0x7) << 2
| ((trap_config1.fast_startup_div_factor as u32) & 0x3) << 0
;
let expected_value = ((trap_config1.open_loop_handoff_cycles as u32) & 0x3) << 22
| ((trap_config1.avs_negative_current_limit as u32) & 0x7) << 16
| ((trap_config1.avs_limit_hysteresis as u32) & 0x1) << 15
| ((trap_config1.isd_bemf_threshold as u32) & 0x1F) << 10
| ((trap_config1.isd_cycle_threshold as u32) & 0x7) << 7
| ((trap_config1.open_loop_zc_detection_threshold as u32) & 0x7) << 2
| ((trap_config1.fast_startup_div_factor as u32) & 0x3) << 0;
self.assert_register(0x9A, Mct8316AVData::Four(expected_value))
}
@@ -276,48 +353,60 @@ where
pub fn set_trap_config2(self, trap_config2: TrapConfig2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_trap_config2(self: {self:?}, trap_config2: {trap_config2:?})");
let expected_value =
((trap_config2.blanking_time_microseconds as u32) & 0xF) << 27
| ((trap_config2.comparator_deglitch_time_microseconds as u32) & 0x7) << 24
| ((trap_config2.align_duty_cycle as u32) & 0x7) << 18;
let expected_value = (u32::from(trap_config2.blanking_time_microseconds) & 0xF) << 27
| (u32::from(trap_config2.comparator_deglitch_time_microseconds) & 0x7) << 24
| ((trap_config2.align_duty_cycle as u32) & 0x7) << 18;
self.assert_register(0x9C, Mct8316AVData::Four(expected_value))
}
#[allow(clippy::identity_op)]
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 =
((fault_config1.no_motor_detect_deglitch_time as u32) & 0x7) << 27
| ((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_mode as u32) & 0xF) << 15
| ((fault_config1.lock_detection_current_limit_deglitch_time as u32) & 0xF) << 11
| ((fault_config1.cycle_by_cycle_pwm_limit as u32) & 0x7) << 8
| ((fault_config1.motor_lock_mode as u32) & 0xF) << 3
| ((fault_config1.lock_retry_time as u32) & 0x7) << 0;
let expected_value = ((fault_config1.no_motor_detect_deglitch_time as u32) & 0x7) << 27
| ((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_mode as u32) & 0xF) << 15
| ((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.motor_lock_mode as u32) & 0xF) << 3
| ((fault_config1.lock_retry_time as u32) & 0x7) << 0;
self.assert_register(0x92, Mct8316AVData::Four(expected_value))
}
#[allow(clippy::identity_op)]
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 =
if fault_config2.lock_abnormal_speed_enable { 1u32 << 30 } else { 0u32 }
| if fault_config2.lock_loss_of_sync_enable { 1u32 << 29 } else { 0u32 }
| if fault_config2.lock_no_motor_enable { 1u32 << 28 } else { 0u32 }
| ((fault_config2.abnormal_speed_lock_threshold as u32) & 0xF) << 24
| ((fault_config2.loss_sync_times as u32) & 0x7) << 21
| ((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;
let expected_value = if fault_config2.lock_abnormal_speed_enable {
1u32 << 30
} else {
0u32
} | if fault_config2.lock_loss_of_sync_enable {
1u32 << 29
} else {
0u32
} | if fault_config2.lock_no_motor_enable {
1u32 << 28
} else {
0u32
} | ((fault_config2.abnormal_speed_lock_threshold as u32) & 0xF) << 24
| ((fault_config2.loss_sync_times as u32) & 0x7) << 21
| ((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))
}
@@ -325,12 +414,11 @@ where
pub fn set_pin_config1(self, pin_config1: PinConfig1) -> Result<Self> {
trace!("Mct8316AVEeprom::set_pin_config1(self: {self:?}, pin_config1: {pin_config1:?})");
let expected_value =
((pin_config1.dacout1_address as u32) & 0xFFF) << 19
| ((pin_config1.dacout2_address as u32) & 0xFFF) << 7
| ((pin_config1.brake_input_config as u32) & 0x3) << 5
| ((pin_config1.direction_input_config as u32) & 0x3) << 3
| ((pin_config1.speed_input_config as u32) & 0x3) << 1;
let expected_value = (u32::from(pin_config1.dacout1_address) & 0xFFF) << 19
| (u32::from(pin_config1.dacout2_address) & 0xFFF) << 7
| ((pin_config1.brake_input_config as u32) & 0x3) << 5
| ((pin_config1.direction_input_config as u32) & 0x3) << 3
| ((pin_config1.speed_input_config as u32) & 0x3) << 1;
self.assert_register(0xA4, Mct8316AVData::Four(expected_value))
}
@@ -338,74 +426,122 @@ where
pub fn set_pin_config2(self, pin_config2: PinConfig2) -> Result<Self> {
trace!("Mct8316AVEeprom::set_pin_config2(self: {self:?}, pin_config2: {pin_config2:?})");
let expected_value =
((pin_config2.pin36config as u32) & 0x3) << 29
| ((pin_config2.pin37_38config as u32) & 0x1) << 27
| ((pin_config2.sleep_time as u32) & 0x3) << 18
| if pin_config2.enable_external_watchdog { 1u32 << 17 } else { 0u32 }
| ((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;
let expected_value = ((pin_config2.pin36config as u32) & 0x3) << 29
| ((pin_config2.pin37_38config as u32) & 0x1) << 27
| ((pin_config2.sleep_time as u32) & 0x3) << 18
| if pin_config2.enable_external_watchdog {
1u32 << 17
} else {
0u32
}
| ((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))
}
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 =
((device_config.max_frequency as u32) & 0x7FFF) << 16
| if device_config.stl_enable { 1u32 << 15 } else { 0u32 }
| if device_config.ssm_enable { 1u32 << 14 } 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;
let expected_value = (u32::from(device_config.max_frequency) & 0x7FFF) << 16
| if device_config.stl_enable {
1u32 << 15
} else {
0u32
}
| if device_config.ssm_enable {
1u32 << 14
} 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))
}
#[allow(clippy::identity_op)]
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 =
((gate_driver_config1.slew_rate as u32) & 0x3) << 26
| ((gate_driver_config1.overvoltage_protection_level as u32) & 0x1) << 19
| if gate_driver_config1.overvoltage_protection_enable { 1u32 << 18 } else { 0u32 }
| if gate_driver_config1.overtemperature_warning_enable { 1u32 << 16 } else { 0u32 }
| ((gate_driver_config1.overcurrent_protection_deglitch_time as u32) & 0x3) << 12
| ((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;
let expected_value = ((gate_driver_config1.slew_rate as u32) & 0x3) << 26
| ((gate_driver_config1.overvoltage_protection_level as u32) & 0x1) << 19
| if gate_driver_config1.overvoltage_protection_enable {
1u32 << 18
} else {
0u32
}
| if gate_driver_config1.overtemperature_warning_enable {
1u32 << 16
} else {
0u32
}
| ((gate_driver_config1.overcurrent_protection_deglitch_time as u32) & 0x3) << 12
| ((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))
}
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 =
if gate_driver_config2.driver_delay_compensation_enable { 1u32 << 30 } else { 0u32 }
| ((gate_driver_config2.target_delay as u32) & 0xF) << 26
| ((gate_driver_config2.buck_slew_rate as u32) & 0x1) << 25
| if gate_driver_config2.buck_power_sequencing_disable { 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 };
let expected_value = if gate_driver_config2.driver_delay_compensation_enable {
1u32 << 30
} else {
0u32
} | ((gate_driver_config2.target_delay as u32) & 0xF) << 26
| ((gate_driver_config2.buck_slew_rate as u32) & 0x1) << 25
| if gate_driver_config2.buck_power_sequencing_disable {
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))
}
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)?;
@@ -428,10 +564,11 @@ where
pub fn commit(self) -> Result<()> {
trace!("Mct8316AVEeprom::commit(self: {self:?})");
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
sleep(Duration::from_millis(100));
}
Ok(())
}
}
}

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

@@ -17,10 +17,12 @@ impl Default for FaultConfig1 {
fn default() -> Self {
Self {
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_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,
motor_lock_mode: LockMode::LatchFaultTristated,
lock_retry_time: LockRetryTime::Milliseconds100,

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

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

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

@@ -1,5 +1,7 @@
#![allow(dead_code)]
// This comes from the ICD
#[allow(clippy::struct_excessive_bools)]
#[derive(Debug, Clone)]
pub struct IsdConfig {
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 eeprom;
mod fault_config;
mod gate_driver_config;
mod isd_config;
mod motor_startup;
mod closed_loop;
mod constant_speed;
mod constant_power;
mod phase_profile;
mod trap_config;
mod fault_config;
mod pin_config;
mod device_config;
mod gate_driver_config;
mod trap_config;
use anyhow::Result;

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

@@ -35,7 +35,7 @@ impl Default for MotorStartup1 {
pub enum MotorStartupMethod {
Align = 0x0,
DoubleAlign = 0x1,
IPD = 0x2,
Ipd = 0x2,
SlowFirstCycle = 0x3,
}
@@ -200,7 +200,6 @@ impl Default for MotorStartup2 {
}
}
#[derive(Debug, Copy, Clone)]
pub enum OpenLoopCurrentLimitMode {
OpenLoopCurrentLimit = 0x0,
@@ -358,6 +357,3 @@ pub enum MinimumDutyCycle {
Percent25 = 0xE,
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)]
pub fn initialize() -> Result<impl Hardware> {
trace!("initialize()");
Ok(sim::SimHardware::new())
sim::SimHardware::new()
}
pub mod error;
pub mod channelization;
pub mod mcp23017;
#[cfg(feature = "raspi")]
mod mcp3208;
pub mod channelization;
pub mod mct8316a;
mod sim;
pub mod pin;
mod sim;

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

@@ -1,11 +1,11 @@
mod pwm;
use crate::hardware::mcp23017::Mcp23017Driver;
use crate::hardware::Hardware;
use crate::hardware::mcp3208::Mcp3208;
use crate::hardware::mcp23017::Mcp23017Driver;
use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a};
use crate::hardware::raspi::pwm::PwmWrapper;
use crate::hardware::sim::mct8316a::SimMct8316a;
use crate::hardware::Hardware;
use anyhow::Result;
use embedded_hal_bus::i2c::MutexDevice;
use log::{debug, info, trace};
@@ -38,12 +38,16 @@ impl RaspiHardware {
Ok(Self {
_gpio: Gpio::new()?,
i2c_bus: Mutex::new(I2c::with_bus(0u8)?),
mcp3208: Mcp3208::new(SimpleHalSpiDevice::new(Spi::new(
Bus::Spi1,
SlaveSelect::Ss1,
CLOCK_1MHZ,
Mode::Mode0,
)?), 3.3f64).into(),
mcp3208: Mcp3208::new(
SimpleHalSpiDevice::new(Spi::new(
Bus::Spi1,
SlaveSelect::Ss1,
CLOCK_1MHZ,
Mode::Mode0,
)?),
3.3f64,
)
.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<'_>> {
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<'_>> {
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> {
@@ -75,10 +85,12 @@ impl Hardware for RaspiHardware {
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
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> {
trace!("RaspiHardware::get_battery_voltage(self: {self:?})");
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.enable()?;
pwm.set_reset_on_drop(true);
Ok(Self {
pwm
})
Ok(Self { pwm })
}
}
@@ -35,6 +33,8 @@ impl SetDutyCycle for PwmWrapper {
fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
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::mct8316a::{Mct8316AVDriver, Mct8316a};
use crate::hardware::sim::mcp23017::SimMcp23017;
use crate::hardware::sim::mct8316a::SimMct8316a;
use crate::hardware::sim::pwm::SimPwm;
use crate::hardware::Hardware;
use anyhow::Result;
use embedded_hal_bus::i2c::MutexDevice;
use log::trace;
@@ -18,14 +18,16 @@ pub struct 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()");
Self {
Ok(Self {
mcp23017a: SimMcp23017::new().into(),
mcp23017b: SimMcp23017::new().into(),
mct8316a: SimMct8316a::new().into(),
battery_voltage: 12.4f64,
}
})
}
}
@@ -35,12 +37,18 @@ impl Hardware for SimHardware {
fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> {
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<'_>> {
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> {
@@ -50,11 +58,14 @@ impl Hardware for SimHardware {
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
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> {
trace!("SimHardware::get_battery_voltage(self: {self:?})");
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 {
fn transaction(&mut self, _address: SevenBitAddress, operations: &mut [Operation<'_>]) -> Result<(), Self::Error> {
trace!("SimMcp23017::transaction(self: {self:?}, _address: {_address}, operations: {operations:?})");
fn transaction(
&mut self,
address: SevenBitAddress,
operations: &mut [Operation<'_>],
) -> Result<(), Self::Error> {
trace!(
"SimMcp23017::transaction(self: {self:?}, address: {address}, operations: {operations:?})"
);
for operation in operations {
match operation {
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 {
fn transaction(&mut self, i2c_addr: SevenBitAddress, operations: &mut [Operation<'_>]) -> Result<(), Self::Error> {
trace!("SimMct8316a::transaction(self: {self:?}, i2c_addr: {i2c_addr}, operations: {operations:?})");
fn transaction(
&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 include_crc = false;
let mut data_length = 2;
@@ -47,38 +53,53 @@ impl I2c for SimMct8316a {
} else if write_buffer[0] & 0x20 > 0 {
data_length = 8;
}
address |= ((write_buffer[0] & 0xF) as u32) << 16;
address |= (write_buffer[1] as u32) << 8;
address |= (write_buffer[2] as u32) << 0;
address |= u32::from(write_buffer[0] & 0xF) << 16;
address |= u32::from(write_buffer[1]) << 8;
address |= u32::from(write_buffer[2]);
if !do_read_operation {
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 {
todo!("Unimplemented");
} 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);
if data_length == 8 {
todo!("Unimplemented");
}
}
} else {
if write_buffer.len() != 3 {
return Err(WrappingError(anyhow!("Write for read has wrong length {}. {} expected", write_buffer.len(), 3)));
}
} else if write_buffer.len() != 3 {
return Err(WrappingError(anyhow!(
"Write for read has wrong length {}. {} expected",
write_buffer.len(),
3
)));
}
crc.update(&write_buffer);
crc.update(write_buffer);
}
Operation::Read(read_buffer) => {
if !do_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 {
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 {
todo!("Unimplemented");
} else if data_length == 4 {

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

@@ -10,9 +10,7 @@ pub struct SimPwm {
impl SimPwm {
pub fn new() -> Self {
trace!("SimPwm::new()");
Self {
duty_cycle: 0,
}
Self { 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::hardware::Hardware;
use crate::hardware::channelization::{LED_A, LED_B};
use crate::hardware::initialize;
use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task};
use crate::hardware::mct8316a::Mct8316a;
use crate::hardware::pin::Pin;
use crate::hardware::Hardware;
use crate::scheduler::Scheduler;
use crate::state_vector::StateVector;
use anyhow::Result;
@@ -13,13 +17,17 @@ use embedded_hal::pwm::SetDutyCycle;
use log::{debug, info};
use nautilus_common::add_ctrlc_handler;
use nautilus_common::telemetry::{SwitchBank, TelemetryMessage};
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
use std::thread::sleep;
use std::time::{Duration, Instant};
mod hardware;
/// Run the flight software
///
/// # Errors
/// An unrecoverable error had occurred
pub fn run() -> Result<()> {
info!(
"Project Nautilus Flight Software {}",
@@ -46,53 +54,77 @@ pub fn run() -> Result<()> {
mcp23017_b.init()?;
mct8316.init()?;
Scheduler::new(running.clone(), |s| {
let task_a = s.run_cyclic("mcp23017-a-task", Mcp23017Task::new(mcp23017_a, &state_vector), 10)?;
Scheduler::scope(running.clone(), |s| {
let task_a = s.run_cyclic(
"mcp23017-a-task",
Mcp23017Task::new(mcp23017_a, &state_vector),
10,
)?;
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 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;
s.run_cyclic("telemetry-producer", move || {
sv.access_section(&a_id, |state: &Mcp23017State| {
comms.send(TelemetryMessage::SwitchState {
bank: SwitchBank::A,
switches: state.pins.clone(),
})
});
sv.access_section(&b_id, |state: &Mcp23017State| {
comms.send(TelemetryMessage::SwitchState {
bank: SwitchBank::B,
switches: state.pins.clone(),
})
});
}, 1)?;
s.run_cyclic(
"telemetry-producer",
move || {
sv.access_section(&a_id, |state: &Mcp23017State| {
comms.send(TelemetryMessage::SwitchState {
bank: SwitchBank::A,
switches: state.pins,
});
});
sv.access_section(&b_id, |state: &Mcp23017State| {
comms.send(TelemetryMessage::SwitchState {
bank: SwitchBank::B,
switches: state.pins,
});
});
},
1,
)?;
let mut led_pin_a = LED_A.new(&task_a, &task_b)?;
let mut led_pin_b = LED_B.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.get_pin(&task_a, &task_b);
info!("Starting Main Loop");
loop {
debug!("A On");
led_pin_a.set(PinState::High, Instant::now() + Duration::from_secs(2), 0);
sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; };
if !running.load(Ordering::Relaxed) {
break;
}
debug!("B On");
led_pin_b.set(PinState::High, Instant::now() + Duration::from_secs(2), 0);
sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; };
if !running.load(Ordering::Relaxed) {
break;
}
debug!("A Off");
sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; };
if !running.load(Ordering::Relaxed) {
break;
}
debug!("B Off");
sleep(Duration::from_secs(1));
if !running.load(Ordering::Relaxed) { break; };
if !running.load(Ordering::Relaxed) {
break;
}
}
anyhow::Ok(())
@@ -112,11 +144,11 @@ pub fn run() -> Result<()> {
Ok(())
}
#[cfg(test)]
mod test_utils;
mod comms;
mod data;
mod on_drop;
mod rcs;
mod comms;
mod scheduler;
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_flight::run;
fn main() {
setup_logger(env!("CARGO_PKG_NAME")).expect("Failed to setup logger");
match run() {
Ok(_) => {}
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> {
trace!("on_drop<F={}>()", type_name::<F>());
OnDrop {
func
}
OnDrop { func }
}
impl<F: FnMut()> Drop for OnDrop<F> {
fn drop(&mut self) {
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::fmt::Debug;
use std::ops::Deref;
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::mpsc::{channel, Receiver, Sender};
use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::mpsc::{Receiver, Sender, channel};
use std::thread;
use std::thread::{sleep, Scope};
use std::thread::{Scope, sleep};
use std::time::{Duration, Instant};
#[derive(Clone, Debug)]
@@ -46,13 +46,12 @@ pub trait CyclicTask {
impl<F> CyclicTask for F
where
F: Fn() -> (),
F: Fn(),
{
type Message = ();
type Data = ();
fn get_data(&self) -> Self::Data {
()
}
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>,
running: Arc<AtomicBool>,
}
impl<'s, 'e> Scheduler<'s, 'e> {
pub fn new<'env, F, R>(running: Arc<AtomicBool>, f: F) -> R
impl<'s> Scheduler<'s, '_> {
pub fn scope<'env, F, R>(running: Arc<AtomicBool>, f: F) -> R
where
F: FnOnce(Scheduler<'_, 'env>) -> R,
{
trace!("Scheduler::new(running: {running:?}, f)");
thread::scope(|scope: &Scope| {
let running_result = running.clone();
// This will automatically set running to false when it drops
// This means that if the function returns any side branches
// 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 {
scope,
running: running.clone(),
running: running_result,
})
})
}
@@ -90,12 +89,16 @@ impl<'s, 'e> Scheduler<'s, 'e> {
&self,
name: impl Into<String>,
task: T,
) -> Result<TaskHandle<T::Message, T::Data>> where
) -> Result<TaskHandle<T::Message, T::Data>>
where
T: Task + Send + Debug + 's,
T::Message: Send,
{
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 (sender, receiver) = channel::<T::Message>();
let data = task.get_data();
@@ -104,11 +107,7 @@ impl<'s, 'e> Scheduler<'s, 'e> {
.spawn_scoped(self.scope, move || {
task.run(receiver, running);
})?;
Ok(TaskHandle {
name,
sender,
data,
})
Ok(TaskHandle { name, sender, data })
}
pub fn run_cyclic<T>(
@@ -116,12 +115,16 @@ impl<'s, 'e> Scheduler<'s, 'e> {
name: impl Into<String>,
mut task: T,
frequency: u64,
) -> Result<TaskHandle<T::Message, T::Data>> where
) -> Result<TaskHandle<T::Message, T::Data>>
where
T: CyclicTask + Send + 's,
T::Message: Send,
{
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 (sender, receiver) = channel::<T::Message>();
let data = task.get_data();
@@ -140,10 +143,6 @@ impl<'s, 'e> Scheduler<'s, 'e> {
}
}
})?;
Ok(TaskHandle {
name,
sender,
data,
})
Ok(TaskHandle { name, sender, data })
}
}

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

@@ -1,10 +1,10 @@
use log::trace;
use std::any::{type_name, Any};
use std::any::{Any, type_name};
use std::collections::HashMap;
use std::fmt::{Debug, Formatter};
use std::marker::PhantomData;
use std::sync::atomic::{AtomicUsize, Ordering};
use std::sync::RwLock;
use std::sync::atomic::{AtomicUsize, Ordering};
#[derive(Debug)]
pub struct StateVector {
@@ -21,15 +21,24 @@ pub struct SectionWriter<'a, 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 {
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 {
trace!("SectionWriter<T={}>::get_identifier(self: {self:?})", type_name::<T>());
trace!(
"SectionWriter<T={}>::get_identifier(self: {self:?})",
type_name::<T>()
);
self.id.clone()
}
@@ -37,7 +46,10 @@ impl<'a, T: 'static> SectionWriter<'a, T> {
where
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();
let sections = self.state_vector.sections.read().unwrap();
let section = sections.get(&self.id).unwrap();
@@ -60,7 +72,10 @@ impl StateVector {
where
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 lock = Box::new(RwLock::new(initial_value));
@@ -74,7 +89,7 @@ impl StateVector {
SectionWriter {
id,
state_vector: &self,
state_vector: self,
_phantom_data: PhantomData,
}
}
@@ -84,13 +99,22 @@ impl StateVector {
T: 'static,
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();
let Ok(sections) = self.sections.read() else { return None; };
let Some(section) = sections.get(id) else { return None; };
let Ok(sections) = self.sections.read() else {
return None;
};
let section = sections.get(id)?;
section.clear_poison();
let Ok(data) = section.read() else { return None; };
let Some(inner) = data.downcast_ref::<T>() else { return None; };
let Ok(data) = section.read() else {
return None;
};
let inner = data.downcast_ref::<T>()?;
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> {
pub fn new(inner: T) -> 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) {
let prev_hook = panic::take_hook();
panic::set_hook(Box::new(|_| {}));
let inner = unsafe {
ManuallyDrop::take(&mut self.inner)
};
let inner = unsafe { ManuallyDrop::take(&mut self.inner) };
let destroy_closure = || {
drop(inner);
};
if panic::catch_unwind(destroy_closure).is_err() {};
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 log::{error, info};
use nautilus_common::add_ctrlc_handler;
@@ -6,10 +10,14 @@ use nautilus_common::telemetry::Telemetry;
use nautilus_common::udp::{UdpRecvCborError, UdpSocketExt};
use std::io::Cursor;
use std::net::{IpAddr, Ipv4Addr, SocketAddr, UdpSocket};
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::Arc;
use std::sync::atomic::{AtomicBool, Ordering};
use std::time::Duration;
/// Run the Ground Software
///
/// # Errors
/// If any unhandled error occurred in the Ground Software
pub fn run() -> Result<()> {
info!(
"Project Nautilus Ground Software {}",
@@ -43,8 +51,8 @@ pub fn run() -> Result<()> {
if let Some(flight_addr) = flight_addr {
let cmd = Command::Shutdown;
udp.send_cbor(&cmd, &mut buffer, &flight_addr)?;
udp.send_cbor(&cmd, &mut buffer, flight_addr)?;
}
Ok(())
}
}

6
ground/src/main.rs
View File

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