From 5455935f3aa8453e4e49408fff83e1ae31f1a71e Mon Sep 17 00:00:00 2001 From: Sergey Savelyev Date: Sun, 26 Oct 2025 08:56:59 -0700 Subject: [PATCH] cleanup --- flight/src/comms/mod.rs | 17 ++++++-- flight/src/hardware/channelization.rs | 3 ++ flight/src/hardware/mcp23017/driver.rs | 5 ++- flight/src/hardware/mcp23017/task.rs | 36 ++++++++++++---- flight/src/hardware/mcp3208/mod.rs | 5 ++- flight/src/hardware/mct8316a/driver.rs | 7 ++- flight/src/hardware/mct8316a/eeprom.rs | 60 ++++++++++++++++---------- flight/src/hardware/mod.rs | 3 ++ flight/src/hardware/raspi/mod.rs | 11 ++--- flight/src/hardware/raspi/pwm.rs | 5 ++- flight/src/hardware/sim/hardware.rs | 11 +++-- flight/src/hardware/sim/mcp23017.rs | 4 ++ flight/src/hardware/sim/mct8316a.rs | 4 ++ flight/src/hardware/sim/pwm.rs | 2 + flight/src/lib.rs | 12 +++--- flight/src/on_drop.rs | 4 +- flight/src/scheduler/mod.rs | 34 ++++++++++++--- flight/src/state_vector/mod.rs | 25 +++++++++-- 18 files changed, 180 insertions(+), 68 deletions(-) diff --git a/flight/src/comms/mod.rs b/flight/src/comms/mod.rs index 2328b91..747c82b 100644 --- a/flight/src/comms/mod.rs +++ b/flight/src/comms/mod.rs @@ -4,6 +4,7 @@ use log::{error, trace}; use nautilus_common::command::Command; use nautilus_common::telemetry::{Telemetry, TelemetryMessage}; use nautilus_common::udp::{UdpRecvCborError, UdpSocketExt}; +use std::any::type_name; use std::fmt::Debug; use std::io::Cursor; use std::net::{IpAddr, Ipv4Addr, SocketAddr, ToSocketAddrs, UdpSocket}; @@ -12,10 +13,11 @@ use std::sync::mpsc::Receiver; use std::sync::Arc; use std::time::Instant; -pub type TelemetrySender = TaskHandle; +pub type TelemetrySender = TaskHandle; impl TelemetrySender { pub fn send(&self, telemetry_message: TelemetryMessage) { + trace!("TelemetrySender::send(self: {self:?}, telemetry_message: {telemetry_message:?}"); // Ignore failure let _ = self.sender.send(Telemetry { timestamp: chrono::Utc::now(), @@ -25,7 +27,7 @@ impl TelemetrySender { } #[derive(Debug)] -pub struct CommsTask { +pub struct CommsTask { udp: UdpSocket, ground_address: A, running: Arc, @@ -37,7 +39,7 @@ impl CommsTask { ground_address: A, running: Arc, ) -> Result { - trace!("CommsTask::new(local_port: {local_port}, ground_address: {ground_address:?})"); + trace!("CommsTask::new(local_port: {local_port}, ground_address: {ground_address:?})", type_name::()); 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)?; @@ -50,10 +52,17 @@ impl CommsTask { } } -impl CyclicTask for CommsTask { +impl CyclicTask for CommsTask { type Message = Telemetry; + type Data = (); + + fn get_data(&self) -> Self::Data { + trace!("CommsTask::get_data(self: {self:?})", type_name::()); + () + } fn step(&mut self, receiver: &Receiver, _step_time: Instant) { + trace!("CommsTask::step(self: {self:?}, receiver: {receiver:?}, _step_time: {_step_time:?})", type_name::()); let mut buffer = Cursor::new([0u8; 512]); match self.udp.recv_cbor::(&mut buffer) { diff --git a/flight/src/hardware/channelization.rs b/flight/src/hardware/channelization.rs index 2404370..05fda5c 100644 --- a/flight/src/hardware/channelization.rs +++ b/flight/src/hardware/channelization.rs @@ -4,6 +4,7 @@ use crate::hardware::pin::{Pin, PinDevice}; use anyhow::Result; use embedded_hal::digital::PinState; use log::trace; +use std::any::type_name; use std::fmt::Debug; use std::time::Instant; @@ -53,6 +54,7 @@ pub struct DevicePin<'a, Device: PinDevice> { impl<'a, Device: PinDevice> Pin for DevicePin<'a, Device> { fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) { + trace!("ChannelPin::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})", type_name::()); 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> { fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) { + trace!("ChannelPin::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})", type_name::(), type_name::()); match self { ChannelPin::ExtA(pin) => pin.set(value, valid_until, priority), ChannelPin::ExtB(pin) => pin.set(value, valid_until, priority), diff --git a/flight/src/hardware/mcp23017/driver.rs b/flight/src/hardware/mcp23017/driver.rs index ca6dd23..bb1e7fe 100644 --- a/flight/src/hardware/mcp23017/driver.rs +++ b/flight/src/hardware/mcp23017/driver.rs @@ -4,6 +4,7 @@ use anyhow::bail; use embedded_hal::digital::PinState; use embedded_hal::i2c::I2c; use log::{error, trace}; +use std::any::type_name; use std::fmt::{Debug, Formatter}; use std::sync::Mutex; @@ -28,7 +29,7 @@ where I2C::Error: 'static, { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { - write!(f, "Mcp23017Driver {{ address: {} }}", self.address) + write!(f, "Mcp23017Driver {{ address: {} }}", type_name::(), self.address) } } @@ -40,7 +41,7 @@ where I2C::Error: 'static, { pub fn new(i2c: I2C, address: u8) -> Self { - trace!("Mcp23017Driver::new(i2c, address: 0x{address:02x})"); + trace!("Mcp23017Driver::new(i2c, address: 0x{address:02x})", type_name::()); Self { i2c: i2c.into(), address, diff --git a/flight/src/hardware/mcp23017/task.rs b/flight/src/hardware/mcp23017/task.rs index f92952b..2a17a66 100644 --- a/flight/src/hardware/mcp23017/task.rs +++ b/flight/src/hardware/mcp23017/task.rs @@ -18,7 +18,7 @@ pub enum Mcp23017Message { }, } -impl PinDevice for TaskHandle { +impl PinDevice for TaskHandle { fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8) { 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 @@ -32,11 +32,22 @@ impl PinDevice for TaskHandle { } } -#[derive(Default)] +#[derive(Debug, Default)] pub struct Mcp23017State { 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> { mcp23017: M, pins: AllPins, @@ -55,13 +66,14 @@ struct AllPins { impl AllPins { fn new() -> Self { + trace!("AllPins::new()"); Self { pins: [PinData::new(); _], } } } -#[derive(Copy, Clone)] +#[derive(Copy, Clone, Debug)] struct PinData { state: PinState, valid_until: Option, @@ -76,6 +88,7 @@ struct PinData { impl PinData { fn new() -> Self { + trace!("PinData::new()"); Self { state: PinState::Low, valid_until: None, @@ -90,6 +103,7 @@ impl PinData { } 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 // If the current is currently invalid, we'd upgrade the next to current for _ in 0..2 { @@ -114,6 +128,7 @@ impl PinData { } 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); @@ -158,20 +173,25 @@ impl<'a, M: Mcp23017 + Debug> Mcp23017Task<'a, M> { state: state_vector.create_section(Mcp23017State::default()), } } - - pub fn get_state(&self) -> SectionIdentifier { - self.state.get_identifier() - } } -impl CyclicTask for Mcp23017Task<'_, M> { +impl CyclicTask for Mcp23017Task<'_, M> { 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( &mut self, receiver: &Receiver, step_time: Instant, ) { + trace!("Mcp23017Task::step(self: {self:?}, receiver, step_time: {step_time:?})"); let mut changed = false; for pin in 0u8..16u8 { diff --git a/flight/src/hardware/mcp3208/mod.rs b/flight/src/hardware/mcp3208/mod.rs index e6228e8..c1c1f6a 100644 --- a/flight/src/hardware/mcp3208/mod.rs +++ b/flight/src/hardware/mcp3208/mod.rs @@ -2,6 +2,7 @@ use crate::hardware::error::WrappingError; use anyhow::{ensure, Result}; use embedded_hal::spi::SpiDevice; use log::trace; +use std::any::type_name; use std::fmt::{Debug, Formatter}; pub struct Mcp3208 { @@ -11,7 +12,7 @@ pub struct Mcp3208 { impl Debug for Mcp3208 { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { - write!(f, "Mcp3208 {{ vref: {} }}", self.vref) + write!(f, "Mcp3208 {{ vref: {} }}", type_name::(), self.vref) } } @@ -23,7 +24,7 @@ where SPI::Error: 'static, { pub fn new(spi: SPI, vref: f64) -> Self { - trace!("Mcp3208::new(spi, vref: {vref})"); + trace!("Mcp3208::new(spi, vref: {vref})", type_name::()); Self { spi, vref, diff --git a/flight/src/hardware/mct8316a/driver.rs b/flight/src/hardware/mct8316a/driver.rs index 11d317b..d385501 100644 --- a/flight/src/hardware/mct8316a/driver.rs +++ b/flight/src/hardware/mct8316a/driver.rs @@ -15,6 +15,7 @@ use crate::hardware::mct8316a::Mct8316a; use anyhow::{bail, ensure, Result}; use embedded_hal::i2c::{I2c, Operation}; use log::trace; +use std::any::type_name; use std::fmt::{Debug, Display, Formatter}; use std::sync::Mutex; @@ -38,6 +39,7 @@ impl Display for Mct8316AVData { } } +#[derive(Debug)] enum OperationRW { Read, Write, @@ -49,6 +51,7 @@ fn control_word( data: Mct8316AVData, address: u32, ) -> [u8; 3] { + 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 { @@ -90,7 +93,7 @@ where I2C::Error: 'static, { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { - write!(f, "Mct8316AVDriver {{ address: {} }}", self.address) + write!(f, "Mct8316AVDriver {{ address: {} }}", type_name::(), self.address) } } @@ -102,7 +105,7 @@ where I2C::Error: 'static, { pub fn new(i2c: I2C, address: u8) -> Self { - trace!("Mct8316AVDriver::new(i2c, address: 0x{address:02x})"); + trace!("Mct8316AVDriver::new(i2c, address: 0x{address:02x})", type_name::()); Self { i2c: i2c.into(), address, diff --git a/flight/src/hardware/mct8316a/eeprom.rs b/flight/src/hardware/mct8316a/eeprom.rs index f4bde5a..da5957b 100644 --- a/flight/src/hardware/mct8316a/eeprom.rs +++ b/flight/src/hardware/mct8316a/eeprom.rs @@ -14,6 +14,8 @@ use crate::hardware::mct8316a::Mct8316AVDriver; use anyhow::Result; use embedded_hal::i2c::I2c; use log::trace; +use std::any::type_name; +use std::fmt::{Debug, Formatter}; use std::thread::sleep; use std::time::Duration; @@ -28,6 +30,18 @@ where 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 {{ driver: {:?}, modified: {} }}", type_name::(), self.driver, self.modified) + } +} + impl<'a, I2C> Mct8316AVEeprom<'a, I2C> where I2C: I2c + Send + Sync, @@ -36,7 +50,7 @@ where I2C::Error: 'static, { pub fn load(driver: &'a mut Mct8316AVDriver) -> Result { - trace!("Mct8316AVEeprom::load()"); + trace!("Mct8316AVEeprom::load(driver: {driver:?})", type_name::()); driver.write(0x0000E6, Mct8316AVData::Four(0x40000000))?; // Wait 100ms for the EEPROM operation to complete @@ -49,7 +63,7 @@ where } pub fn set_isd_config(self, isd_config: IsdConfig) -> Result { - trace!("Mct8316AVEeprom::set_isd_config(isd_config: {isd_config:?})"); + trace!("Mct8316AVEeprom::set_isd_config(self: {self:?}, isd_config: {isd_config:?})"); let expected_value = if isd_config.enable_isd { 0x40000000 } else { 0 } @@ -71,7 +85,7 @@ where } pub fn set_motor_startup1(self, motor_startup1: MotorStartup1) -> Result { - trace!("Mct8316AVEeprom::set_motor_startup1(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 @@ -89,7 +103,7 @@ where } pub fn set_motor_startup2(self, motor_startup2: MotorStartup2) -> Result { - trace!("Mct8316AVEeprom::set_motor_startup2(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 @@ -106,7 +120,7 @@ where } pub fn set_closed_loop1(self, closed_loop1: ClosedLoop1) -> Result { - 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; @@ -125,7 +139,7 @@ where } pub fn set_closed_loop2(self, closed_loop2: ClosedLoop2) -> Result { - trace!("Mct8316AVEeprom::set_closed_loop2(closed_loop2: {closed_loop2:?})"); + trace!("Mct8316AVEeprom::set_closed_loop2(self: {self:?}, closed_loop2: {closed_loop2:?})"); let expected_value = ((closed_loop2.speed_feedback_mode as u32) & 0x2) << 29 @@ -143,7 +157,7 @@ where } pub fn set_closed_loop3(self, closed_loop3: ClosedLoop3) -> Result { - trace!("Mct8316AVEeprom::set_closed_loop3(closed_loop3: {closed_loop3:?})"); + trace!("Mct8316AVEeprom::set_closed_loop3(self: {self:?}, closed_loop3: {closed_loop3:?})"); let expected_value = ((closed_loop3.degauss_samples as u32) & 0x2) << 29 @@ -163,7 +177,7 @@ where } pub fn set_closed_loop4(self, closed_loop4: ClosedLoop4) -> Result { - trace!("Mct8316AVEeprom::set_closed_loop4(closed_loop4: {closed_loop4:?})"); + trace!("Mct8316AVEeprom::set_closed_loop4(self: {self:?}, closed_loop4: {closed_loop4:?})"); let expected_value = 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 { - 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_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 { - 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_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 { - trace!("Mct8316AVEeprom::set_two_phase_profile(profile: {profile:?})"); + trace!("Mct8316AVEeprom::set_two_phase_profile(self: {self:?}, profile: {profile:?})"); let expected_value = ((profile.steps[0] as u32) & 0x7) << 28 @@ -227,7 +241,7 @@ where } pub fn set_three_phase_profile(self, profile: ThreePhase150DegreeProfile) -> Result { - trace!("Mct8316AVEeprom::set_three_phase_profile(profile: {profile:?})"); + trace!("Mct8316AVEeprom::set_three_phase_profile(self: {self:?}, profile: {profile:?})"); let expected_value = ((profile.steps[0] as u32) & 0x7) << 28 @@ -244,7 +258,7 @@ where } pub fn set_trap_config1(self, trap_config1: TrapConfig1) -> Result { - trace!("Mct8316AVEeprom::set_trap_config1(trap_config1: {trap_config1:?})"); + trace!("Mct8316AVEeprom::set_trap_config1(self: {self:?}, trap_config1: {trap_config1:?})"); let expected_value = ((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 { - trace!("Mct8316AVEeprom::set_trap_config2(trap_config2: {trap_config2:?})"); + trace!("Mct8316AVEeprom::set_trap_config2(self: {self:?}, trap_config2: {trap_config2:?})"); let expected_value = ((trap_config2.blanking_time_microseconds as u32) & 0xF) << 27 @@ -271,7 +285,7 @@ where } pub fn set_fault_config1(self, fault_config1: FaultConfig1) -> Result { - trace!("Mct8316AVEeprom::set_fault_config1(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 @@ -287,7 +301,7 @@ where } pub fn set_fault_config2(self, fault_config2: FaultConfig2) -> Result { - trace!("Mct8316AVEeprom::set_fault_config2(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 } @@ -309,7 +323,7 @@ where } pub fn set_pin_config1(self, pin_config1: PinConfig1) -> Result { - trace!("Mct8316AVEeprom::set_pin_config1(pin_config1: {pin_config1:?})"); + trace!("Mct8316AVEeprom::set_pin_config1(self: {self:?}, pin_config1: {pin_config1:?})"); let expected_value = ((pin_config1.dacout1_address as u32) & 0xFFF) << 19 @@ -322,7 +336,7 @@ where } pub fn set_pin_config2(self, pin_config2: PinConfig2) -> Result { - trace!("Mct8316AVEeprom::set_pin_config2(pin_config2: {pin_config2:?})"); + trace!("Mct8316AVEeprom::set_pin_config2(self: {self:?}, pin_config2: {pin_config2:?})"); let expected_value = ((pin_config2.pin36config as u32) & 0x3) << 29 @@ -337,7 +351,7 @@ where } pub fn set_device_config(self, device_config: DeviceConfig) -> Result { - trace!("Mct8316AVEeprom::set_device_config(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 @@ -353,7 +367,7 @@ where } pub fn set_gate_driver_config1(self, gate_driver_config1: GateDriverConfig1) -> Result { - 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 = ((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 { - 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 = 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 { - 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(); @@ -412,7 +426,7 @@ where } pub fn commit(self) -> Result<()> { - trace!("Mct8316AVEeprom::commit()"); + trace!("Mct8316AVEeprom::commit(self: {self:?})"); if self.modified { self.driver.write(0x0000E6, Mct8316AVData::Four(0x80000000))?; // Wait for EEPROM operation to complete diff --git a/flight/src/hardware/mod.rs b/flight/src/hardware/mod.rs index 4abe141..002168a 100644 --- a/flight/src/hardware/mod.rs +++ b/flight/src/hardware/mod.rs @@ -2,6 +2,7 @@ use crate::hardware::mcp23017::Mcp23017; use crate::hardware::mct8316a::Mct8316a; use anyhow::Result; use embedded_hal::pwm::SetDutyCycle; +use log::trace; use std::fmt::Debug; pub trait Hardware { @@ -25,12 +26,14 @@ mod raspi; #[cfg(feature = "raspi")] pub fn initialize() -> Result { + trace!("initialize()"); raspi::RaspiHardware::new() } #[cfg(not(feature = "raspi"))] #[allow(unreachable_code)] pub fn initialize() -> Result { + trace!("initialize()"); Ok(sim::SimHardware::new()) } diff --git a/flight/src/hardware/raspi/mod.rs b/flight/src/hardware/raspi/mod.rs index 0430024..aef5b29 100644 --- a/flight/src/hardware/raspi/mod.rs +++ b/flight/src/hardware/raspi/mod.rs @@ -19,6 +19,7 @@ use std::sync::Mutex; const CLOCK_1MHZ: u32 = 1_000_000; +#[derive(Debug)] pub struct RaspiHardware { _gpio: Gpio, i2c_bus: Mutex, @@ -54,17 +55,17 @@ impl Hardware for RaspiHardware { type Pwm = PwmWrapper; fn new_mcp23017_a(&self) -> Result> { - trace!("RaspiHardware::new_mcp23017_a()"); + trace!("RaspiHardware::new_mcp23017_a(self: {self:?})"); Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100000)) } fn new_mcp23017_b(&self) -> Result> { - trace!("RaspiHardware::new_mcp23017_b()"); + trace!("RaspiHardware::new_mcp23017_b(self: {self:?})"); Ok(Mcp23017Driver::new(MutexDevice::new(&self.i2c_bus), 0b0100001)) } fn new_pwm0(&self) -> Result { - trace!("RaspiHardware::new_pwm0()"); + trace!("RaspiHardware::new_pwm0(self: {self:?})"); // Unfortunately the current version of rpi_pal assumes an older version // of the kernel where pwmchip for RPi5 was 2 const PWMCHIP: u8 = 0; @@ -73,13 +74,13 @@ impl Hardware for RaspiHardware { } fn new_mct8316a(&self) -> Result { - trace!("RaspiHardware::new_mct8316a()"); + trace!("RaspiHardware::new_mct8316a(self: {self:?})"); Ok(Mct8316AVDriver::new(MutexDevice::new(&self.mct8316a), 0b0000000)) } fn get_battery_voltage(&self) -> Result { - trace!("RaspiHardware::get_battery_voltage()"); + trace!("RaspiHardware::get_battery_voltage(self: {self:?})"); self.mcp3208.borrow_mut().read_single(1) } } diff --git a/flight/src/hardware/raspi/pwm.rs b/flight/src/hardware/raspi/pwm.rs index 4ace1b3..cf558d1 100644 --- a/flight/src/hardware/raspi/pwm.rs +++ b/flight/src/hardware/raspi/pwm.rs @@ -6,6 +6,7 @@ use std::time::Duration; const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz +#[derive(Debug)] pub struct PwmWrapper { pwm: Pwm, } @@ -28,12 +29,12 @@ impl ErrorType for PwmWrapper { impl SetDutyCycle for PwmWrapper { fn max_duty_cycle(&self) -> u16 { - trace!("PwmWrapper::max_duty_cycle()"); + trace!("PwmWrapper::max_duty_cycle(self: {self:?})"); u16::MAX } 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) } } diff --git a/flight/src/hardware/sim/hardware.rs b/flight/src/hardware/sim/hardware.rs index 6f8661d..13e84b6 100644 --- a/flight/src/hardware/sim/hardware.rs +++ b/flight/src/hardware/sim/hardware.rs @@ -9,6 +9,7 @@ use embedded_hal_bus::i2c::MutexDevice; use log::trace; use std::sync::Mutex; +#[derive(Debug)] pub struct SimHardware { mcp23017a: Mutex, mcp23017b: Mutex, @@ -18,6 +19,7 @@ pub struct SimHardware { impl SimHardware { pub fn new() -> Self { + trace!("SimHardware::new()"); Self { mcp23017a: SimMcp23017::new().into(), mcp23017b: SimMcp23017::new().into(), @@ -32,26 +34,27 @@ impl Hardware for SimHardware { type Pwm = SimPwm; fn new_mcp23017_a(&self) -> Result> { - trace!("SimHardware::new_mcp23017_a()"); + trace!("SimHardware::new_mcp23017_a(self: {self:?})"); Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017a), 0b0100000)) } fn new_mcp23017_b(&self) -> Result> { - trace!("SimHardware::new_mcp23017_b()"); + trace!("SimHardware::new_mcp23017_b(self: {self:?})"); Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017b), 0b0100000)) } fn new_pwm0(&self) -> Result { - trace!("SimHardware::new_pwm0()"); + trace!("SimHardware::new_pwm0(self: {self:?})"); Ok(SimPwm::new()) } fn new_mct8316a(&self) -> Result { - trace!("SimHardware::new_mct8316a()"); + trace!("SimHardware::new_mct8316a(self: {self:?})"); Ok(Mct8316AVDriver::new(MutexDevice::new(&self.mct8316a), 0b0000000)) } fn get_battery_voltage(&self) -> Result { + trace!("SimHardware::get_battery_voltage(self: {self:?})"); Ok(self.battery_voltage) } } \ No newline at end of file diff --git a/flight/src/hardware/sim/mcp23017.rs b/flight/src/hardware/sim/mcp23017.rs index b416a42..959e0de 100644 --- a/flight/src/hardware/sim/mcp23017.rs +++ b/flight/src/hardware/sim/mcp23017.rs @@ -1,10 +1,13 @@ use embedded_hal::i2c::{ErrorKind, ErrorType, I2c, Operation, SevenBitAddress}; +use log::trace; use std::fmt::{Display, Formatter}; +#[derive(Debug)] pub struct SimMcp23017 {} impl SimMcp23017 { pub fn new() -> Self { + trace!("SimMcp23017::new()"); Self {} } } @@ -32,6 +35,7 @@ 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:?})"); for operation in operations { match operation { Operation::Write(_write_buffer) => { diff --git a/flight/src/hardware/sim/mct8316a.rs b/flight/src/hardware/sim/mct8316a.rs index 26293d1..a674345 100644 --- a/flight/src/hardware/sim/mct8316a.rs +++ b/flight/src/hardware/sim/mct8316a.rs @@ -1,16 +1,19 @@ use crate::hardware::error::WrappingError; use anyhow::anyhow; use embedded_hal::i2c::{ErrorType, I2c, Operation, SevenBitAddress}; +use log::trace; use std::collections::HashMap; const CRC: crc::Crc = crc::Crc::::new(&crc::CRC_8_SMBUS); +#[derive(Debug)] pub struct SimMct8316a { data: HashMap, } impl SimMct8316a { pub fn new() -> Self { + trace!("SimMct8316a::new()"); Self { data: HashMap::new(), } @@ -23,6 +26,7 @@ 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:?})"); let mut do_read_operation = false; let mut include_crc = false; let mut data_length = 2; diff --git a/flight/src/hardware/sim/pwm.rs b/flight/src/hardware/sim/pwm.rs index 7ba97ab..a608729 100644 --- a/flight/src/hardware/sim/pwm.rs +++ b/flight/src/hardware/sim/pwm.rs @@ -2,12 +2,14 @@ use embedded_hal::pwm::{ErrorKind, ErrorType, SetDutyCycle}; use log::trace; use std::fmt::{Display, Formatter}; +#[derive(Debug)] pub struct SimPwm { duty_cycle: u16, } impl SimPwm { pub fn new() -> Self { + trace!("SimPwm::new()"); Self { duty_cycle: 0, } diff --git a/flight/src/lib.rs b/flight/src/lib.rs index 114d9cd..2d361b0 100644 --- a/flight/src/lib.rs +++ b/flight/src/lib.rs @@ -47,15 +47,13 @@ pub fn run() -> Result<()> { mct8316.init()?; Scheduler::new(running.clone(), |s| { - let task_a = Mcp23017Task::new(mcp23017_a, &state_vector); - let a_id = task_a.get_state(); - let task_a = s.run_cyclic("mcp23017-a", task_a, 10)?; + let task_a = s.run_cyclic("mcp23017-a-task", Mcp23017Task::new(mcp23017_a, &state_vector), 10)?; + let a_id = task_a.get_id(); - let task_b = Mcp23017Task::new(mcp23017_b, &state_vector); - let b_id = task_b.get_state(); - let task_b = s.run_cyclic("mcp23017-b", task_b, 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", 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; s.run_cyclic("telemetry-producer", move || { diff --git a/flight/src/on_drop.rs b/flight/src/on_drop.rs index 04be51b..784bf55 100644 --- a/flight/src/on_drop.rs +++ b/flight/src/on_drop.rs @@ -1,10 +1,12 @@ use log::trace; +use std::any::type_name; pub struct OnDrop { func: F, } pub fn on_drop(func: F) -> OnDrop { + trace!("on_drop()", type_name::()); OnDrop { func } @@ -12,7 +14,7 @@ pub fn on_drop(func: F) -> OnDrop { impl Drop for OnDrop { fn drop(&mut self) { - trace!("OnDrop::drop()"); + trace!("OnDrop::drop()", type_name::()); (self.func)() } } diff --git a/flight/src/scheduler/mod.rs b/flight/src/scheduler/mod.rs index ba31aed..083791f 100644 --- a/flight/src/scheduler/mod.rs +++ b/flight/src/scheduler/mod.rs @@ -1,7 +1,9 @@ use crate::on_drop::on_drop; use anyhow::Result; 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; @@ -10,22 +12,35 @@ use std::thread::{sleep, Scope}; use std::time::{Duration, Instant}; #[derive(Clone, Debug)] -pub struct TaskHandle { +pub struct TaskHandle { #[allow(dead_code)] pub name: String, pub sender: Sender, + data: Data, +} + +impl Deref for TaskHandle { + type Target = Data; + + fn deref(&self) -> &Self::Target { + &self.data + } } #[allow(dead_code)] pub trait Task { type Message; + type Data; + fn get_data(&self) -> Self::Data; fn run(self, receiver: Receiver, running: Arc); } pub trait CyclicTask { type Message; + type Data; + fn get_data(&self) -> Self::Data; fn step(&mut self, receiver: &Receiver, step_time: Instant); } @@ -34,6 +49,11 @@ where F: Fn() -> (), { type Message = (); + type Data = (); + + fn get_data(&self) -> Self::Data { + () + } fn step(&mut self, _receiver: &Receiver, _step_time: Instant) { self(); @@ -70,14 +90,15 @@ impl<'s, 'e> Scheduler<'s, 'e> { &self, name: impl Into, task: T, - ) -> Result> where + ) -> Result> where T: Task + Send + Debug + 's, T::Message: Send, { let name = name.into(); - trace!("Scheduler::run(name: {name}, task: {task:?})"); + trace!("Scheduler::run(name: {name}, task: {task:?})", type_name::()); let running = self.running.clone(); let (sender, receiver) = channel::(); + let data = task.get_data(); let _ = thread::Builder::new() .name(name.clone()) .spawn_scoped(self.scope, move || { @@ -86,6 +107,7 @@ impl<'s, 'e> Scheduler<'s, 'e> { Ok(TaskHandle { name, sender, + data, }) } @@ -94,14 +116,15 @@ impl<'s, 'e> Scheduler<'s, 'e> { name: impl Into, mut task: T, frequency: u64, - ) -> Result> where + ) -> Result> where T: CyclicTask + Send + 's, T::Message: Send, { let name = name.into(); - trace!("Scheduler::run_cyclic(name: {name}, task, frequency: {frequency})"); + trace!("Scheduler::run_cyclic(name: {name}, task, frequency: {frequency})", type_name::()); let running = self.running.clone(); let (sender, receiver) = channel::(); + let data = task.get_data(); let _ = thread::Builder::new() .name(name.clone()) .spawn_scoped(self.scope, move || { @@ -120,6 +143,7 @@ impl<'s, 'e> Scheduler<'s, 'e> { Ok(TaskHandle { name, sender, + data, }) } } diff --git a/flight/src/state_vector/mod.rs b/flight/src/state_vector/mod.rs index 083ca3e..2a41db4 100644 --- a/flight/src/state_vector/mod.rs +++ b/flight/src/state_vector/mod.rs @@ -1,15 +1,18 @@ -use std::any::Any; +use log::trace; +use std::any::{type_name, Any}; use std::collections::HashMap; +use std::fmt::{Debug, Formatter}; use std::marker::PhantomData; use std::sync::atomic::{AtomicUsize, Ordering}; use std::sync::RwLock; +#[derive(Debug)] pub struct StateVector { next_section: AtomicUsize, sections: RwLock>>>, } -#[derive(Clone, Eq, PartialEq, Hash)] +#[derive(Clone, Eq, PartialEq, Hash, Debug)] pub struct SectionIdentifier(usize); pub struct SectionWriter<'a, T> { @@ -18,8 +21,15 @@ pub struct SectionWriter<'a, T> { _phantom_data: PhantomData, } +impl<'a, T> Debug for SectionWriter<'a, T> { + fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { + write!(f, "SectionWriter {{ id: {:?}, state_vector: {:?} }}", type_name::(), self.id, self.state_vector) + } +} + impl<'a, T: 'static> SectionWriter<'a, T> { pub fn get_identifier(&self) -> SectionIdentifier { + trace!("SectionWriter::get_identifier(self: {self:?})", type_name::()); self.id.clone() } @@ -27,6 +37,7 @@ impl<'a, T: 'static> SectionWriter<'a, T> { where F: FnOnce(&mut T) -> R, { + trace!("SectionWriter::update(self: {self:?}, f)", type_name::()); self.state_vector.sections.clear_poison(); let sections = self.state_vector.sections.read().unwrap(); let section = sections.get(&self.id).unwrap(); @@ -38,6 +49,7 @@ impl<'a, T: 'static> SectionWriter<'a, T> { impl StateVector { pub fn new() -> Self { + trace!("StateVector::new()"); Self { next_section: AtomicUsize::new(0usize), sections: RwLock::new(HashMap::new()), @@ -48,11 +60,17 @@ impl StateVector { where T: Send + Sync + 'static, { + trace!("StateVector::create_section(self: {self:?}, initial_value)", type_name::()); let id = SectionIdentifier(self.next_section.fetch_add(1usize, Ordering::SeqCst)); let lock = Box::new(RwLock::new(initial_value)); 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 { id, @@ -66,6 +84,7 @@ impl StateVector { T: 'static, F: FnOnce(&T) -> R, { + trace!("StateVector::access_section(self: {self:?}, id: {id:?}, f)", type_name::(), type_name::(), type_name::()); self.sections.clear_poison(); let Ok(sections) = self.sections.read() else { return None; }; let Some(section) = sections.get(id) else { return None; };