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

cleanup

Browse Source
This commit is contained in:
Sergey Savelyev
2025-10-26 08:56:59 -07:00
parent e0f17649b2
commit 5455935f3a
18 changed files with 180 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,6 +4,7 @@ use log::{error, trace};
use nautilus_common::command::Command; use nautilus_common::command::Command;
use nautilus_common::telemetry::{Telemetry, TelemetryMessage}; use nautilus_common::telemetry::{Telemetry, TelemetryMessage};
use nautilus_common::udp::{UdpRecvCborError, UdpSocketExt}; use nautilus_common::udp::{UdpRecvCborError, UdpSocketExt};
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};
@@ -12,10 +13,11 @@ use std::sync::mpsc::Receiver;
use std::sync::Arc; use std::sync::Arc;
use std::time::Instant; use std::time::Instant;
pub type TelemetrySender = TaskHandle<Telemetry>; pub type TelemetrySender = TaskHandle<Telemetry, ()>;
impl TelemetrySender { impl TelemetrySender {
pub fn send(&self, telemetry_message: TelemetryMessage) { pub fn send(&self, telemetry_message: TelemetryMessage) {
trace!("TelemetrySender::send(self: {self:?}, telemetry_message: {telemetry_message:?}");
// Ignore failure // Ignore failure
let _ = self.sender.send(Telemetry { let _ = self.sender.send(Telemetry {
timestamp: chrono::Utc::now(), timestamp: chrono::Utc::now(),
@@ -25,7 +27,7 @@ impl TelemetrySender {
} }
#[derive(Debug)] #[derive(Debug)]
pub struct CommsTask<A: ToSocketAddrs> { pub struct CommsTask<A: ToSocketAddrs + Debug> {
udp: UdpSocket, udp: UdpSocket,
ground_address: A, ground_address: A,
running: Arc<AtomicBool>, running: Arc<AtomicBool>,
@@ -37,7 +39,7 @@ impl<A: ToSocketAddrs + Debug> CommsTask<A> {
ground_address: A, ground_address: A,
running: Arc<AtomicBool>, running: Arc<AtomicBool>,
) -> Result<Self> { ) -> Result<Self> {
trace!("CommsTask::new(local_port: {local_port}, ground_address: {ground_address:?})"); 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)?;
@@ -50,10 +52,17 @@ impl<A: ToSocketAddrs + Debug> CommsTask<A> {
} }
} }
impl<A: ToSocketAddrs> CyclicTask for CommsTask<A> { impl<A: ToSocketAddrs + Debug> CyclicTask for CommsTask<A> {
type Message = Telemetry; type Message = Telemetry;
type Data = ();
fn get_data(&self) -> Self::Data {
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>());
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) {

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

@@ -4,6 +4,7 @@ use crate::hardware::pin::{Pin, PinDevice};
use anyhow::Result; 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::fmt::Debug; use std::fmt::Debug;
use std::time::Instant; use std::time::Instant;
@@ -53,6 +54,7 @@ pub struct DevicePin<'a, Device: PinDevice> {
impl<'a, Device: PinDevice> Pin for DevicePin<'a, Device> { impl<'a, Device: PinDevice> Pin for DevicePin<'a, 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>());
self.device.set_pin(self.pin, value, valid_until, priority); self.device.set_pin(self.pin, value, valid_until, priority);
} }
} }
@@ -64,6 +66,7 @@ pub enum ChannelPin<'a, A: PinDevice, B: PinDevice> {
impl<'a, A: PinDevice, B: PinDevice> Pin for ChannelPin<'a, A, B> { impl<'a, A: PinDevice, B: PinDevice> Pin for ChannelPin<'a, 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>());
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),

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

@@ -4,6 +4,7 @@ use anyhow::bail;
use embedded_hal::digital::PinState; use embedded_hal::digital::PinState;
use embedded_hal::i2c::I2c; use embedded_hal::i2c::I2c;
use log::{error, trace}; use log::{error, trace};
use std::any::type_name;
use std::fmt::{Debug, Formatter}; use std::fmt::{Debug, Formatter};
use std::sync::Mutex; use std::sync::Mutex;
@@ -28,7 +29,7 @@ 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 {{ address: {} }}", self.address) write!(f, "Mcp23017Driver<I2C={}> {{ address: {} }}", type_name::<I2C>(), self.address)
} }
} }
@@ -40,7 +41,7 @@ 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::new(i2c, address: 0x{address:02x})"); trace!("Mcp23017Driver<I2C={}>::new(i2c, address: 0x{address:02x})", type_name::<I2C>());
Self { Self {
i2c: i2c.into(), i2c: i2c.into(),
address, address,

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

@@ -18,7 +18,7 @@ pub enum Mcp23017Message {
}, },
} }
impl PinDevice for TaskHandle<Mcp23017Message> { impl<D: Debug> PinDevice for TaskHandle<Mcp23017Message, D> {
fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8) { fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8) {
trace!("Mcp23017Task::set_pin(self: {self:?}, pin: {pin}, value: {value:?})"); trace!("Mcp23017Task::set_pin(self: {self:?}, pin: {pin}, value: {value:?})");
// This can only fail if the other end is disconnected - which we intentionally want to // This can only fail if the other end is disconnected - which we intentionally want to
@@ -32,11 +32,22 @@ impl PinDevice for TaskHandle<Mcp23017Message> {
} }
} }
#[derive(Default)] #[derive(Debug, Default)]
pub struct Mcp23017State { pub struct Mcp23017State {
pub pins: [bool; 16], pub pins: [bool; 16],
} }
#[derive(Debug, Clone)]
pub struct Mcp23017Data {
id: SectionIdentifier,
}
impl Mcp23017Data {
pub fn get_id(&self) -> SectionIdentifier {
self.id.clone()
}
}
pub struct Mcp23017Task<'a, M: Mcp23017> { pub struct Mcp23017Task<'a, M: Mcp23017> {
mcp23017: M, mcp23017: M,
pins: AllPins, pins: AllPins,
@@ -55,13 +66,14 @@ struct AllPins {
impl AllPins { impl AllPins {
fn new() -> Self { fn new() -> Self {
trace!("AllPins::new()");
Self { Self {
pins: [PinData::new(); _], pins: [PinData::new(); _],
} }
} }
} }
#[derive(Copy, Clone)] #[derive(Copy, Clone, Debug)]
struct PinData { struct PinData {
state: PinState, state: PinState,
valid_until: Option<Instant>, valid_until: Option<Instant>,
@@ -76,6 +88,7 @@ struct PinData {
impl PinData { impl PinData {
fn new() -> Self { fn new() -> Self {
trace!("PinData::new()");
Self { Self {
state: PinState::Low, state: PinState::Low,
valid_until: None, valid_until: None,
@@ -90,6 +103,7 @@ impl PinData {
} }
fn evaluate(&mut self, now: Instant) { fn evaluate(&mut self, now: Instant) {
trace!("PinData::evaluate(self: {self:?}, now: {now:?})");
// 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 {
@@ -114,6 +128,7 @@ impl PinData {
} }
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})");
let can_replace_current = self.valid_until.map(|current| current <= valid_until).unwrap_or(true); 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); let can_replace_next = self.next_validity.map(|next| next <= valid_until).unwrap_or(true);
@@ -158,20 +173,25 @@ impl<'a, M: Mcp23017 + Debug> Mcp23017Task<'a, M> {
state: state_vector.create_section(Mcp23017State::default()), state: state_vector.create_section(Mcp23017State::default()),
} }
} }
pub fn get_state(&self) -> SectionIdentifier {
self.state.get_identifier()
}
} }
impl<M: Mcp23017> CyclicTask for Mcp23017Task<'_, M> { impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
type Message = Mcp23017Message; type Message = Mcp23017Message;
type Data = Mcp23017Data;
fn get_data(&self) -> Self::Data {
trace!("Mcp23017Task::get_data(self: {self:?})");
Self::Data {
id: self.state.get_identifier()
}
}
fn step( fn step(
&mut self, &mut self,
receiver: &Receiver<Self::Message>, receiver: &Receiver<Self::Message>,
step_time: Instant, step_time: Instant,
) { ) {
trace!("Mcp23017Task::step(self: {self:?}, receiver, step_time: {step_time:?})");
let mut changed = false; let mut changed = false;
for pin in 0u8..16u8 { for pin in 0u8..16u8 {

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

@@ -2,6 +2,7 @@ use crate::hardware::error::WrappingError;
use anyhow::{ensure, Result}; use anyhow::{ensure, Result};
use embedded_hal::spi::SpiDevice; use embedded_hal::spi::SpiDevice;
use log::trace; use log::trace;
use std::any::type_name;
use std::fmt::{Debug, Formatter}; use std::fmt::{Debug, Formatter};
pub struct Mcp3208<SPI> { pub struct Mcp3208<SPI> {
@@ -11,7 +12,7 @@ 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 {{ vref: {} }}", self.vref) write!(f, "Mcp3208<SPI={}> {{ vref: {} }}", type_name::<SPI>(), self.vref)
} }
} }
@@ -23,7 +24,7 @@ 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::new(spi, vref: {vref})"); trace!("Mcp3208<SPI={}>::new(spi, vref: {vref})", type_name::<SPI>());
Self { Self {
spi, spi,
vref, vref,

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

@@ -15,6 +15,7 @@ use crate::hardware::mct8316a::Mct8316a;
use anyhow::{bail, ensure, Result}; use anyhow::{bail, ensure, Result};
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::fmt::{Debug, Display, Formatter}; use std::fmt::{Debug, Display, Formatter};
use std::sync::Mutex; use std::sync::Mutex;
@@ -38,6 +39,7 @@ impl Display for Mct8316AVData {
} }
} }
#[derive(Debug)]
enum OperationRW { enum OperationRW {
Read, Read,
Write, Write,
@@ -49,6 +51,7 @@ 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})");
let mut control_word = [0u8; _]; let mut control_word = [0u8; _];
control_word[0] |= match operation_rw { control_word[0] |= match operation_rw {
@@ -90,7 +93,7 @@ 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 {{ address: {} }}", self.address) write!(f, "Mct8316AVDriver<I2C={}> {{ address: {} }}", type_name::<I2C>(), self.address)
} }
} }
@@ -102,7 +105,7 @@ 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::new(i2c, address: 0x{address:02x})"); trace!("Mct8316AVDriver<I2C={}>::new(i2c, address: 0x{address:02x})", type_name::<I2C>());
Self { Self {
i2c: i2c.into(), i2c: i2c.into(),
address, address,

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

@@ -14,6 +14,8 @@ 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;
use std::any::type_name;
use std::fmt::{Debug, Formatter};
use std::thread::sleep; use std::thread::sleep;
use std::time::Duration; use std::time::Duration;
@@ -28,6 +30,18 @@ where
modified: bool, modified: bool,
} }
impl<'a, I2C> Debug for Mct8316AVEeprom<'a, I2C>
where
I2C: I2c + Send + Sync,
I2C::Error: Send,
I2C::Error: Sync,
I2C::Error: 'static,
{
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
write!(f, "Mct8316AVEeprom<I2C={}> {{ driver: {:?}, modified: {} }}", type_name::<I2C>(), self.driver, self.modified)
}
}
impl<'a, I2C> Mct8316AVEeprom<'a, I2C> impl<'a, I2C> Mct8316AVEeprom<'a, I2C>
where where
I2C: I2c + Send + Sync, I2C: I2c + Send + Sync,
@@ -36,7 +50,7 @@ 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::load()"); trace!("Mct8316AVEeprom<I2C={}>::load(driver: {driver:?})", type_name::<I2C>());
driver.write(0x0000E6, Mct8316AVData::Four(0x40000000))?; driver.write(0x0000E6, Mct8316AVData::Four(0x40000000))?;
// Wait 100ms for the EEPROM operation to complete // Wait 100ms for the EEPROM operation to complete
@@ -49,7 +63,7 @@ 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(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 { 0x40000000 } else { 0 } if isd_config.enable_isd { 0x40000000 } else { 0 }
@@ -71,7 +85,7 @@ where
} }
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(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
@@ -89,7 +103,7 @@ where
} }
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(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
@@ -106,7 +120,7 @@ where
} }
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(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, u8::MAX as f32) as u8;
@@ -125,7 +139,7 @@ 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(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
@@ -143,7 +157,7 @@ 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(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
@@ -163,7 +177,7 @@ where
} }
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(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 { 1u32 << 19 } else { 0u32 } if closed_loop4.wcomp_blanking_enable { 1u32 << 19 } else { 0u32 }
@@ -179,7 +193,7 @@ where
} }
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(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).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_ki = (constant_speed.speed_power_ki * 1000000f64).round().clamp(0.0f64, 0xFFF as f64) as u32;
@@ -195,7 +209,7 @@ where
} }
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(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).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_power = (constant_power.max_power * 4f64).round().clamp(0.0f64, 0x3FF as f64) as u32;
@@ -211,7 +225,7 @@ 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(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
@@ -227,7 +241,7 @@ 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(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
@@ -244,7 +258,7 @@ where
} }
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(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
@@ -260,7 +274,7 @@ 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(trap_config2: {trap_config2:?})"); trace!("Mct8316AVEeprom::set_trap_config2(self: {self:?}, trap_config2: {trap_config2:?})");
let expected_value = let expected_value =
((trap_config2.blanking_time_microseconds as u32) & 0xF) << 27 ((trap_config2.blanking_time_microseconds as u32) & 0xF) << 27
@@ -271,7 +285,7 @@ where
} }
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(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
@@ -287,7 +301,7 @@ where
} }
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(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 { 1u32 << 30 } else { 0u32 } if fault_config2.lock_abnormal_speed_enable { 1u32 << 30 } else { 0u32 }
@@ -309,7 +323,7 @@ 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(pin_config1: {pin_config1:?})"); trace!("Mct8316AVEeprom::set_pin_config1(self: {self:?}, pin_config1: {pin_config1:?})");
let expected_value = let expected_value =
((pin_config1.dacout1_address as u32) & 0xFFF) << 19 ((pin_config1.dacout1_address as u32) & 0xFFF) << 19
@@ -322,7 +336,7 @@ 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(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
@@ -337,7 +351,7 @@ where
} }
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(device_config: {device_config:?})"); trace!("Mct8316AVEeprom::set_device_config(self: {self:?}, device_config: {device_config:?})");
let expected_value = let expected_value =
((device_config.max_frequency as u32) & 0x7FFF) << 16 ((device_config.max_frequency as u32) & 0x7FFF) << 16
@@ -353,7 +367,7 @@ where
} }
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(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
@@ -374,7 +388,7 @@ where
} }
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(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 { 1u32 << 30 } else { 0u32 } if gate_driver_config2.driver_delay_compensation_enable { 1u32 << 30 } else { 0u32 }
@@ -389,7 +403,7 @@ where
} }
fn assert_register(self, address: u32, value: Mct8316AVData) -> Result<Self> { fn assert_register(self, address: u32, value: Mct8316AVData) -> Result<Self> {
trace!("Mct8316AVEeprom::assert_register(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.clone();
@@ -412,7 +426,7 @@ where
} }
pub fn commit(self) -> Result<()> { pub fn commit(self) -> Result<()> {
trace!("Mct8316AVEeprom::commit()"); trace!("Mct8316AVEeprom::commit(self: {self:?})");
if self.modified { if self.modified {
self.driver.write(0x0000E6, Mct8316AVData::Four(0x80000000))?; self.driver.write(0x0000E6, Mct8316AVData::Four(0x80000000))?;
// Wait for EEPROM operation to complete // Wait for EEPROM operation to complete

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

@@ -2,6 +2,7 @@ use crate::hardware::mcp23017::Mcp23017;
use crate::hardware::mct8316a::Mct8316a; use crate::hardware::mct8316a::Mct8316a;
use anyhow::Result; use anyhow::Result;
use embedded_hal::pwm::SetDutyCycle; use embedded_hal::pwm::SetDutyCycle;
use log::trace;
use std::fmt::Debug; use std::fmt::Debug;
pub trait Hardware { pub trait Hardware {
@@ -25,12 +26,14 @@ mod raspi;
#[cfg(feature = "raspi")] #[cfg(feature = "raspi")]
pub fn initialize() -> Result<impl Hardware> { pub fn initialize() -> Result<impl Hardware> {
trace!("initialize()");
raspi::RaspiHardware::new() raspi::RaspiHardware::new()
} }
#[cfg(not(feature = "raspi"))] #[cfg(not(feature = "raspi"))]
#[allow(unreachable_code)] #[allow(unreachable_code)]
pub fn initialize() -> Result<impl Hardware> { pub fn initialize() -> Result<impl Hardware> {
trace!("initialize()");
Ok(sim::SimHardware::new()) Ok(sim::SimHardware::new())
} }

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

@@ -19,6 +19,7 @@ use std::sync::Mutex;
const CLOCK_1MHZ: u32 = 1_000_000; const CLOCK_1MHZ: u32 = 1_000_000;
#[derive(Debug)]
pub struct RaspiHardware { pub struct RaspiHardware {
_gpio: Gpio, _gpio: Gpio,
i2c_bus: Mutex<I2c>, i2c_bus: Mutex<I2c>,
@@ -54,17 +55,17 @@ impl Hardware for RaspiHardware {
type Pwm = PwmWrapper; type Pwm = PwmWrapper;
fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> { fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> {
trace!("RaspiHardware::new_mcp23017_a()"); 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()"); 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> {
trace!("RaspiHardware::new_pwm0()"); trace!("RaspiHardware::new_pwm0(self: {self:?})");
// Unfortunately the current version of rpi_pal assumes an older version // Unfortunately the current version of rpi_pal assumes an older version
// of the kernel where pwmchip for RPi5 was 2 // of the kernel where pwmchip for RPi5 was 2
const PWMCHIP: u8 = 0; const PWMCHIP: u8 = 0;
@@ -73,13 +74,13 @@ impl Hardware for RaspiHardware {
} }
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> { fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
trace!("RaspiHardware::new_mct8316a()"); 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()"); trace!("RaspiHardware::get_battery_voltage(self: {self:?})");
self.mcp3208.borrow_mut().read_single(1) self.mcp3208.borrow_mut().read_single(1)
} }
} }

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

@@ -6,6 +6,7 @@ use std::time::Duration;
const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz
#[derive(Debug)]
pub struct PwmWrapper { pub struct PwmWrapper {
pwm: Pwm, pwm: Pwm,
} }
@@ -28,12 +29,12 @@ impl ErrorType for PwmWrapper {
impl SetDutyCycle for PwmWrapper { impl SetDutyCycle for PwmWrapper {
fn max_duty_cycle(&self) -> u16 { fn max_duty_cycle(&self) -> u16 {
trace!("PwmWrapper::max_duty_cycle()"); trace!("PwmWrapper::max_duty_cycle(self: {self:?})");
u16::MAX u16::MAX
} }
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(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)
} }
} }

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

@@ -9,6 +9,7 @@ use embedded_hal_bus::i2c::MutexDevice;
use log::trace; use log::trace;
use std::sync::Mutex; use std::sync::Mutex;
#[derive(Debug)]
pub struct SimHardware { pub struct SimHardware {
mcp23017a: Mutex<SimMcp23017>, mcp23017a: Mutex<SimMcp23017>,
mcp23017b: Mutex<SimMcp23017>, mcp23017b: Mutex<SimMcp23017>,
@@ -18,6 +19,7 @@ pub struct SimHardware {
impl SimHardware { impl SimHardware {
pub fn new() -> Self { pub fn new() -> Self {
trace!("SimHardware::new()");
Self { Self {
mcp23017a: SimMcp23017::new().into(), mcp23017a: SimMcp23017::new().into(),
mcp23017b: SimMcp23017::new().into(), mcp23017b: SimMcp23017::new().into(),
@@ -32,26 +34,27 @@ impl Hardware for SimHardware {
type Pwm = SimPwm; type Pwm = SimPwm;
fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> { fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>> {
trace!("SimHardware::new_mcp23017_a()"); trace!("SimHardware::new_mcp23017_a(self: {self:?})");
Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017a), 0b0100000)) Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017a), 0b0100000))
} }
fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>> { fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>> {
trace!("SimHardware::new_mcp23017_b()"); trace!("SimHardware::new_mcp23017_b(self: {self:?})");
Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017b), 0b0100000)) Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017b), 0b0100000))
} }
fn new_pwm0(&self) -> Result<Self::Pwm> { fn new_pwm0(&self) -> Result<Self::Pwm> {
trace!("SimHardware::new_pwm0()"); trace!("SimHardware::new_pwm0(self: {self:?})");
Ok(SimPwm::new()) Ok(SimPwm::new())
} }
fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> { fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
trace!("SimHardware::new_mct8316a()"); trace!("SimHardware::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!("SimHardware::get_battery_voltage(self: {self:?})");
Ok(self.battery_voltage) Ok(self.battery_voltage)
} }
} }

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

@@ -1,10 +1,13 @@
use embedded_hal::i2c::{ErrorKind, ErrorType, I2c, Operation, SevenBitAddress}; use embedded_hal::i2c::{ErrorKind, ErrorType, I2c, Operation, SevenBitAddress};
use log::trace;
use std::fmt::{Display, Formatter}; use std::fmt::{Display, Formatter};
#[derive(Debug)]
pub struct SimMcp23017 {} pub struct SimMcp23017 {}
impl SimMcp23017 { impl SimMcp23017 {
pub fn new() -> Self { pub fn new() -> Self {
trace!("SimMcp23017::new()");
Self {} Self {}
} }
} }
@@ -32,6 +35,7 @@ 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(&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) => {

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

@@ -1,16 +1,19 @@
use crate::hardware::error::WrappingError; use crate::hardware::error::WrappingError;
use anyhow::anyhow; use anyhow::anyhow;
use embedded_hal::i2c::{ErrorType, I2c, Operation, SevenBitAddress}; use embedded_hal::i2c::{ErrorType, I2c, Operation, SevenBitAddress};
use log::trace;
use std::collections::HashMap; use std::collections::HashMap;
const CRC: crc::Crc<u8> = crc::Crc::<u8>::new(&crc::CRC_8_SMBUS); const CRC: crc::Crc<u8> = crc::Crc::<u8>::new(&crc::CRC_8_SMBUS);
#[derive(Debug)]
pub struct SimMct8316a { pub struct SimMct8316a {
data: HashMap<u32, u32>, data: HashMap<u32, u32>,
} }
impl SimMct8316a { impl SimMct8316a {
pub fn new() -> Self { pub fn new() -> Self {
trace!("SimMct8316a::new()");
Self { Self {
data: HashMap::new(), data: HashMap::new(),
} }
@@ -23,6 +26,7 @@ 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(&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;

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

@@ -2,12 +2,14 @@ use embedded_hal::pwm::{ErrorKind, ErrorType, SetDutyCycle};
use log::trace; use log::trace;
use std::fmt::{Display, Formatter}; use std::fmt::{Display, Formatter};
#[derive(Debug)]
pub struct SimPwm { pub struct SimPwm {
duty_cycle: u16, duty_cycle: u16,
} }
impl SimPwm { impl SimPwm {
pub fn new() -> Self { pub fn new() -> Self {
trace!("SimPwm::new()");
Self { Self {
duty_cycle: 0, duty_cycle: 0,
} }

12
flight/src/lib.rs
View File

@@ -47,15 +47,13 @@ pub fn run() -> Result<()> {
mct8316.init()?; mct8316.init()?;
Scheduler::new(running.clone(), |s| { Scheduler::new(running.clone(), |s| {
let task_a = Mcp23017Task::new(mcp23017_a, &state_vector); let task_a = s.run_cyclic("mcp23017-a-task", Mcp23017Task::new(mcp23017_a, &state_vector), 10)?;
let a_id = task_a.get_state(); let a_id = task_a.get_id();
let task_a = s.run_cyclic("mcp23017-a", task_a, 10)?;
let task_b = Mcp23017Task::new(mcp23017_b, &state_vector); let task_b = s.run_cyclic("mcp23017-b-task", Mcp23017Task::new(mcp23017_b, &state_vector), 10)?;
let b_id = task_b.get_state(); let b_id = task_b.get_id();
let task_b = s.run_cyclic("mcp23017-b", task_b, 10)?;
let comms = s.run_cyclic("comms", CommsTask::new(15000, "192.168.50.157: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("telemetry-producer", move || {

4
flight/src/on_drop.rs
View File

@@ -1,10 +1,12 @@
use log::trace; use log::trace;
use std::any::type_name;
pub struct OnDrop<F: FnMut()> { pub struct OnDrop<F: FnMut()> {
func: F, func: F,
} }
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>());
OnDrop { OnDrop {
func func
} }
@@ -12,7 +14,7 @@ pub fn on_drop<F: FnMut()>(func: F) -> OnDrop<F> {
impl<F: FnMut()> Drop for OnDrop<F> { impl<F: FnMut()> Drop for OnDrop<F> {
fn drop(&mut self) { fn drop(&mut self) {
trace!("OnDrop::drop()"); trace!("OnDrop<F={}>::drop()", type_name::<F>());
(self.func)() (self.func)()
} }
} }

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

@@ -1,7 +1,9 @@
use crate::on_drop::on_drop; use crate::on_drop::on_drop;
use anyhow::Result; use anyhow::Result;
use log::trace; use log::trace;
use std::any::type_name;
use std::fmt::Debug; use std::fmt::Debug;
use std::ops::Deref;
use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::mpsc::{channel, Receiver, Sender}; use std::sync::mpsc::{channel, Receiver, Sender};
use std::sync::Arc; use std::sync::Arc;
@@ -10,22 +12,35 @@ use std::thread::{sleep, Scope};
use std::time::{Duration, Instant}; use std::time::{Duration, Instant};
#[derive(Clone, Debug)] #[derive(Clone, Debug)]
pub struct TaskHandle<Message> { pub struct TaskHandle<Message, Data> {
#[allow(dead_code)] #[allow(dead_code)]
pub name: String, pub name: String,
pub sender: Sender<Message>, pub sender: Sender<Message>,
data: Data,
}
impl<Message, Data> Deref for TaskHandle<Message, Data> {
type Target = Data;
fn deref(&self) -> &Self::Target {
&self.data
}
} }
#[allow(dead_code)] #[allow(dead_code)]
pub trait Task { pub trait Task {
type Message; type Message;
type Data;
fn get_data(&self) -> Self::Data;
fn run(self, receiver: Receiver<Self::Message>, running: Arc<AtomicBool>); fn run(self, receiver: Receiver<Self::Message>, running: Arc<AtomicBool>);
} }
pub trait CyclicTask { pub trait CyclicTask {
type Message; type Message;
type 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);
} }
@@ -34,6 +49,11 @@ where
F: Fn() -> (), F: Fn() -> (),
{ {
type Message = (); type Message = ();
type 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) {
self(); self();
@@ -70,14 +90,15 @@ impl<'s, 'e> Scheduler<'s, 'e> {
&self, &self,
name: impl Into<String>, name: impl Into<String>,
task: T, task: T,
) -> Result<TaskHandle<T::Message>> 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(name: {name}, task: {task:?})"); 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 _ = thread::Builder::new() let _ = thread::Builder::new()
.name(name.clone()) .name(name.clone())
.spawn_scoped(self.scope, move || { .spawn_scoped(self.scope, move || {
@@ -86,6 +107,7 @@ impl<'s, 'e> Scheduler<'s, 'e> {
Ok(TaskHandle { Ok(TaskHandle {
name, name,
sender, sender,
data,
}) })
} }
@@ -94,14 +116,15 @@ 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>> 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(name: {name}, task, frequency: {frequency})"); 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 _ = thread::Builder::new() let _ = thread::Builder::new()
.name(name.clone()) .name(name.clone())
.spawn_scoped(self.scope, move || { .spawn_scoped(self.scope, move || {
@@ -120,6 +143,7 @@ impl<'s, 'e> Scheduler<'s, 'e> {
Ok(TaskHandle { Ok(TaskHandle {
name, name,
sender, sender,
data,
}) })
} }
} }

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

@@ -1,15 +1,18 @@
use std::any::Any; use log::trace;
use std::any::{type_name, Any};
use std::collections::HashMap; use std::collections::HashMap;
use std::fmt::{Debug, Formatter};
use std::marker::PhantomData; use std::marker::PhantomData;
use std::sync::atomic::{AtomicUsize, Ordering}; use std::sync::atomic::{AtomicUsize, Ordering};
use std::sync::RwLock; use std::sync::RwLock;
#[derive(Debug)]
pub struct StateVector { pub struct StateVector {
next_section: AtomicUsize, next_section: AtomicUsize,
sections: RwLock<HashMap<SectionIdentifier, Box<RwLock<dyn Any + Send + Sync>>>>, sections: RwLock<HashMap<SectionIdentifier, Box<RwLock<dyn Any + Send + Sync>>>>,
} }
#[derive(Clone, Eq, PartialEq, Hash)] #[derive(Clone, Eq, PartialEq, Hash, Debug)]
pub struct SectionIdentifier(usize); pub struct SectionIdentifier(usize);
pub struct SectionWriter<'a, T> { pub struct SectionWriter<'a, T> {
@@ -18,8 +21,15 @@ pub struct SectionWriter<'a, T> {
_phantom_data: PhantomData<T>, _phantom_data: PhantomData<T>,
} }
impl<'a, T> Debug for SectionWriter<'a, 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)
}
}
impl<'a, T: 'static> SectionWriter<'a, T> { impl<'a, T: 'static> SectionWriter<'a, T> {
pub fn get_identifier(&self) -> SectionIdentifier { pub fn get_identifier(&self) -> SectionIdentifier {
trace!("SectionWriter<T={}>::get_identifier(self: {self:?})", type_name::<T>());
self.id.clone() self.id.clone()
} }
@@ -27,6 +37,7 @@ 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>());
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();
@@ -38,6 +49,7 @@ impl<'a, T: 'static> SectionWriter<'a, T> {
impl StateVector { impl StateVector {
pub fn new() -> Self { pub fn new() -> Self {
trace!("StateVector::new()");
Self { Self {
next_section: AtomicUsize::new(0usize), next_section: AtomicUsize::new(0usize),
sections: RwLock::new(HashMap::new()), sections: RwLock::new(HashMap::new()),
@@ -48,11 +60,17 @@ impl StateVector {
where where
T: Send + Sync + 'static, T: Send + Sync + 'static,
{ {
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));
self.sections.clear_poison(); self.sections.clear_poison();
self.sections.write().unwrap().insert(id.clone(), lock); let mut sections = self.sections.write().unwrap();
if !sections.contains_key(&id) {
sections.insert(id.clone(), lock);
}
drop(sections);
SectionWriter { SectionWriter {
id, id,
@@ -66,6 +84,7 @@ 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>());
self.sections.clear_poison(); self.sections.clear_poison();
let Ok(sections) = self.sections.read() else { return None; }; let Ok(sections) = self.sections.read() else { return None; };
let Some(section) = sections.get(id) else { return None; }; let Some(section) = sections.get(id) else { return None; };