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::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<Telemetry>;
pub type TelemetrySender = TaskHandle<Telemetry, ()>;
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<A: ToSocketAddrs> {
pub struct CommsTask<A: ToSocketAddrs + Debug> {
udp: UdpSocket,
ground_address: A,
running: Arc<AtomicBool>,
@@ -37,7 +39,7 @@ impl<A: ToSocketAddrs + Debug> CommsTask<A> {
ground_address: A,
running: Arc<AtomicBool>,
) -> 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::V6(Ipv6Addr::UNSPECIFIED), local_port);
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 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) {
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) {

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

@@ -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<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,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<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),

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

@@ -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<I2C={}> {{ address: {} }}", type_name::<I2C>(), 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<I2C={}>::new(i2c, address: 0x{address:02x})", type_name::<I2C>());
Self {
i2c: i2c.into(),
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) {
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<Mcp23017Message> {
}
}
#[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<Instant>,
@@ -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<M: Mcp23017> CyclicTask for Mcp23017Task<'_, M> {
impl<M: Mcp23017 + Debug> 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<Self::Message>,
step_time: Instant,
) {
trace!("Mcp23017Task::step(self: {self:?}, receiver, step_time: {step_time:?})");
let mut changed = false;
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 embedded_hal::spi::SpiDevice;
use log::trace;
use std::any::type_name;
use std::fmt::{Debug, Formatter};
pub struct Mcp3208<SPI> {
@@ -11,7 +12,7 @@ pub struct Mcp3208<SPI> {
impl<SPI> Debug for Mcp3208<SPI> {
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,
{
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 {
spi,
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 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<I2C={}> {{ address: {} }}", type_name::<I2C>(), 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<I2C={}>::new(i2c, address: 0x{address:02x})", type_name::<I2C>());
Self {
i2c: i2c.into(),
address,

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

@@ -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<I2C={}> {{ driver: {:?}, modified: {} }}", type_name::<I2C>(), 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<I2C>) -> Result<Self> {
trace!("Mct8316AVEeprom::load()");
trace!("Mct8316AVEeprom<I2C={}>::load(driver: {driver:?})", type_name::<I2C>());
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<Self> {
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<Self> {
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<Self> {
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<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;
@@ -125,7 +139,7 @@ where
}
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 =
((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> {
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<Self> {
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<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_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> {
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<Self> {
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<Self> {
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<Self> {
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<Self> {
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<Self> {
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<Self> {
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<Self> {
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<Self> {
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<Self> {
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<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 =
((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> {
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<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();
@@ -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

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

@@ -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<impl Hardware> {
trace!("initialize()");
raspi::RaspiHardware::new()
}
#[cfg(not(feature = "raspi"))]
#[allow(unreachable_code)]
pub fn initialize() -> Result<impl Hardware> {
trace!("initialize()");
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;
#[derive(Debug)]
pub struct RaspiHardware {
_gpio: Gpio,
i2c_bus: Mutex<I2c>,
@@ -54,17 +55,17 @@ impl Hardware for RaspiHardware {
type Pwm = PwmWrapper;
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))
}
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))
}
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
// 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<impl Mct8316a + Sync> {
trace!("RaspiHardware::new_mct8316a()");
trace!("RaspiHardware::new_mct8316a(self: {self:?})");
Ok(Mct8316AVDriver::new(MutexDevice::new(&self.mct8316a), 0b0000000))
}
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)
}
}

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
#[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)
}
}

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

@@ -9,6 +9,7 @@ use embedded_hal_bus::i2c::MutexDevice;
use log::trace;
use std::sync::Mutex;
#[derive(Debug)]
pub struct SimHardware {
mcp23017a: Mutex<SimMcp23017>,
mcp23017b: Mutex<SimMcp23017>,
@@ -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<Self::Mcp23017<'_>> {
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<Self::Mcp23017<'_>> {
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<Self::Pwm> {
trace!("SimHardware::new_pwm0()");
trace!("SimHardware::new_pwm0(self: {self:?})");
Ok(SimPwm::new())
}
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))
}
fn get_battery_voltage(&self) -> Result<f64> {
trace!("SimHardware::get_battery_voltage(self: {self:?})");
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 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) => {

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

@@ -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<u8> = crc::Crc::<u8>::new(&crc::CRC_8_SMBUS);
#[derive(Debug)]
pub struct SimMct8316a {
data: HashMap<u32, u32>,
}
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;

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

@@ -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,
}

12
flight/src/lib.rs
View File

@@ -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 || {

4
flight/src/on_drop.rs
View File

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

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

@@ -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<Message> {
pub struct TaskHandle<Message, Data> {
#[allow(dead_code)]
pub name: String,
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)]
pub trait Task {
type Message;
type Data;
fn get_data(&self) -> Self::Data;
fn run(self, receiver: Receiver<Self::Message>, running: Arc<AtomicBool>);
}
pub trait CyclicTask {
type Message;
type Data;
fn get_data(&self) -> Self::Data;
fn step(&mut self, receiver: &Receiver<Self::Message>, 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<Self::Message>, _step_time: Instant) {
self();
@@ -70,14 +90,15 @@ impl<'s, 'e> Scheduler<'s, 'e> {
&self,
name: impl Into<String>,
task: T,
) -> Result<TaskHandle<T::Message>> where
) -> Result<TaskHandle<T::Message, T::Data>> where
T: Task + Send + Debug + 's,
T::Message: Send,
{
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 (sender, receiver) = channel::<T::Message>();
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<String>,
mut task: T,
frequency: u64,
) -> Result<TaskHandle<T::Message>> where
) -> Result<TaskHandle<T::Message, T::Data>> 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<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();
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,
})
}
}

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::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<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 SectionWriter<'a, T> {
@@ -18,8 +21,15 @@ pub struct SectionWriter<'a, 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> {
pub fn get_identifier(&self) -> SectionIdentifier {
trace!("SectionWriter<T={}>::get_identifier(self: {self:?})", type_name::<T>());
self.id.clone()
}
@@ -27,6 +37,7 @@ impl<'a, T: 'static> SectionWriter<'a, T> {
where
F: FnOnce(&mut T) -> R,
{
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();
@@ -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<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));
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<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; };