adds simulated hardware

flight/src/data/mod.rs

@@ -1,4 +1,4 @@
-#[cfg(not(feature = "raspi"))]
+// #[cfg(not(feature = "raspi"))]
-pub mod reader;
+// pub mod reader;
-#[cfg(not(feature = "raspi"))]
+// #[cfg(not(feature = "raspi"))]
-pub mod writer;
+// pub mod writer;

flight/src/hardware/error.rs

@@ -1,35 +0,0 @@
-use std::error::Error;
-use std::fmt::{Debug, Display, Formatter};
-
-#[derive(Copy, Clone, Debug)]
-pub struct I2cError<ERR: Debug + embedded_hal::i2c::Error>(pub ERR);
-
-impl<ERR: Debug + embedded_hal::i2c::Error> Display for I2cError<ERR> {
-    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
-        writeln!(f, "I2cError({:?})", self.0)
-    }
-}
-
-impl<ERR: Debug + embedded_hal::i2c::Error> Error for I2cError<ERR> {}
-
-#[derive(Copy, Clone, Debug)]
-pub struct SpiError<ERR: Debug + embedded_hal::spi::Error>(pub ERR);
-
-impl<ERR: Debug + embedded_hal::spi::Error> Display for SpiError<ERR> {
-    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
-        writeln!(f, "SpiError({:?})", self.0)
-    }
-}
-
-impl<ERR: Debug + embedded_hal::spi::Error> Error for SpiError<ERR> {}
-
-// #[derive(Copy, Clone, Debug)]
-// pub struct NotAvailableError;
-//
-// impl Display for NotAvailableError {
-//     fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
-//         writeln!(f, "NotAvailableError")
-//     }
-// }
-//
-// impl Error for NotAvailableError { }

flight/src/hardware/error/mod.rs

@@ -0,0 +1,3 @@
+mod wrapping;
+
+pub use wrapping::WrappingError;

flight/src/hardware/error/wrapping.rs

@@ -0,0 +1,26 @@
+use std::error::Error;
+use std::fmt::{Debug, Display, Formatter};
+use embedded_hal::i2c::ErrorKind;
+
+#[derive(Copy, Clone, Debug)]
+pub struct WrappingError<ERR: Debug>(pub ERR);
+
+impl<ERR: Debug> Display for WrappingError<ERR> {
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        writeln!(f, "WrappingError({:?})", self.0)
+    }
+}
+
+impl<ERR: Debug> Error for WrappingError<ERR> {}
+
+impl<ERR: Debug> embedded_hal::i2c::Error for WrappingError<ERR> {
+    fn kind(&self) -> ErrorKind {
+        ErrorKind::Other
+    }
+}
+
+impl<ERR: Debug> embedded_hal::pwm::Error for WrappingError<ERR> {
+    fn kind(&self) -> embedded_hal::pwm::ErrorKind {
+        embedded_hal::pwm::ErrorKind::Other
+    }
+}

flight/src/hardware/mcp23017/driver.rs

@@ -1,4 +1,4 @@
-use crate::hardware::error::I2cError;
+use crate::hardware::error::WrappingError;
 use crate::hardware::mcp23017::pin::{Mcp23017OutputPinDriver, Mcp23017Pins};
 use crate::hardware::mcp23017::{Mcp23017, Mcp23017OutputPin};
 use anyhow::bail;
@@ -109,7 +109,7 @@ where
         let data: [u8; _] = [0x00, 0x00, 0x00];
 
         match self.i2c.get_mut() {
-            Ok(lock) => lock.write(self.address, &data).map_err(I2cError)?,
+            Ok(lock) => lock.write(self.address, &data).map_err(WrappingError)?,
             Err(_) => bail!("Lock was poisoned"),
         }
 
@@ -150,7 +150,7 @@ where
             let data: [u8; _] = [0x12, bytes[0], bytes[1]];
             // This blocks while writing
             if let Ok(mut lock) = self.i2c.lock() {
-                lock.write(self.address, &data).map_err(I2cError)?;
+                lock.write(self.address, &data).map_err(WrappingError)?;
                 self.i2c.clear_poison();
             }
         }

flight/src/hardware/mcp3208/mod.rs

@@ -1,8 +1,8 @@
-use crate::hardware::error::SpiError;
 use anyhow::{ensure, Result};
 use embedded_hal::spi::SpiDevice;
 use log::trace;
 use std::fmt::{Debug, Formatter};
+use crate::hardware::error::WrappingError;
 
 pub struct Mcp3208<SPI> {
     spi: SPI,
@@ -40,7 +40,7 @@ where
         write_bits[1] |= (channel.unbounded_shl(6)) & 0xFF;
 
         let mut read_bits = [0u8; 3];
-        self.spi.transfer(&mut read_bits, &write_bits).map_err(SpiError)?;
+        self.spi.transfer(&mut read_bits, &write_bits).map_err(WrappingError)?;
 
         let value: u16 = u16::from_be_bytes([read_bits[1], read_bits[2]]) & 0x0FFF;
 

flight/src/hardware/mct8316a/driver.rs

@@ -1,4 +1,4 @@
-use crate::hardware::error::I2cError;
+use crate::hardware::error::WrappingError;
 use crate::hardware::mct8316a::closed_loop::{BemfThreshold, ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4, ClosedLoopDecelerationMode, ClosedLoopRate, CommutationMethod, CommutationMode, DegaussLowerBound, DegaussSamples, DegaussUpperBound, DegaussWindow, DutyCycleThreshold, FastBrakeDelta, IntegrationBemfThreshold, IntegrationCycleHighThreshold, IntegrationCycleLowThreshold, IntegrationDutyCycleThreshold, LeadAnglePolarity, LowerPercentLimit, MotorStopBrakeTime, MotorStopMode, PwmFrequency, PwmMode, PwmModulation, SpeedFeedbackConfig, SpeedFeedbackDivision, SpeedFeedbackMode, UpperPercentLimit};
 use crate::hardware::mct8316a::constant_power::{ConstantPower, PowerHysteresis, PowerMode};
 use crate::hardware::mct8316a::constant_speed::{ClosedLoopMode, ConstantSpeed};
@@ -142,7 +142,7 @@ where
         write_data[4 + data_length] = crc_result;
 
         match self.i2c.lock() {
-            Ok(mut lock) => lock.write(self.address, &write_data).map_err(I2cError)?,
+            Ok(mut lock) => lock.write(self.address, &write_data[1..(4 + data_length + 1)]).map_err(WrappingError)?,
             Err(_) => bail!("Lock was poisoned"),
         }
 
@@ -179,14 +179,14 @@ where
         ];
 
         match self.i2c.lock() {
-            Ok(mut lock) => lock.transaction(self.address, &mut i2c_ops).map_err(I2cError)?,
+            Ok(mut lock) => lock.transaction(self.address, &mut i2c_ops).map_err(WrappingError)?,
             Err(_) => bail!("Lock was poisoned"),
         }
         drop(i2c_ops);
 
         let expected_crc = CRC.checksum(&read_data[0..(5 + data_length)]);
 
-        ensure!(expected_crc == read_data[5+data_length], "CRC Mismatch");
+        ensure!(expected_crc == read_data[5+data_length], "CRC Mismatch. {expected_crc} expected. {}", read_data[5+data_length]);
 
         match data {
             Mct8316AVData::Two(val) => {

flight/src/hardware/mod.rs

@@ -27,18 +27,14 @@ pub fn initialize() -> Result<impl Hardware> {
 #[cfg(not(feature = "raspi"))]
 #[allow(unreachable_code)]
 pub fn initialize() -> Result<impl Hardware> {
-    panic!("Can not Initialize");
+    Ok(sim::SimHardware::new())
-
-    Ok(())
 }
 
-#[cfg(not(feature = "raspi"))]
+pub mod error;
-mod bno085;
-#[cfg(not(feature = "raspi"))]
-mod imu;
-mod error;
 
 pub mod mcp23017;
+#[cfg(feature = "raspi")]
 mod mcp3208;
 pub mod channelization;
-pub(crate) mod mct8316a;
+pub mod mct8316a;
+mod sim;

flight/src/hardware/raspi/mod.rs

@@ -15,6 +15,7 @@ use rpi_pal::spi::SimpleHalSpiDevice;
 use rpi_pal::spi::{Bus, Mode, SlaveSelect, Spi};
 use std::cell::RefCell;
 use std::sync::Mutex;
+use crate::hardware::sim::mct8316a::SimMct8316a;
 
 const CLOCK_1MHZ: u32 = 1_000_000;
 
@@ -22,6 +23,7 @@ pub struct RaspiHardware {
     _gpio: Gpio,
     i2c_bus: Mutex<I2c>,
     mcp3208: RefCell<Mcp3208<SimpleHalSpiDevice>>,
+    mct8316a: Mutex<SimMct8316a>,
 }
 
 impl RaspiHardware {
@@ -41,6 +43,7 @@ impl RaspiHardware {
                 CLOCK_1MHZ,
                 Mode::Mode0,
             )?), 3.3f64).into(),
+            mct8316a: SimMct8316a::new().into(),
         })
     }
 }
@@ -69,7 +72,7 @@ impl Hardware for RaspiHardware {
 
     fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
         trace!("RaspiHardware::new_mct8316a()");
-        Ok(Mct8316AVDriver::new(MutexDevice::new(&self.i2c_bus), 0b0000000))
+        Ok(Mct8316AVDriver::new(MutexDevice::new(&self.mct8316a), 0b0000000))
     }
 
 

flight/src/hardware/raspi/pwm.rs

@@ -1,8 +1,8 @@
-use embedded_hal::pwm::{ErrorKind, ErrorType, SetDutyCycle};
+use embedded_hal::pwm::{ErrorType, SetDutyCycle};
 use log::trace;
 use rpi_pal::pwm::Pwm;
-use std::fmt::{Display, Formatter};
 use std::time::Duration;
+use crate::hardware::error::WrappingError;
 
 const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz
 
@@ -22,25 +22,8 @@ impl PwmWrapper {
     }
 }
 
-#[derive(Debug)]
-pub struct ErrorWrapper(rpi_pal::pwm::Error);
-
-impl embedded_hal::pwm::Error for ErrorWrapper {
-    fn kind(&self) -> ErrorKind {
-        ErrorKind::Other
-    }
-}
-
-impl Display for ErrorWrapper {
-    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
-        self.0.fmt(f)
-    }
-}
-
-impl std::error::Error for ErrorWrapper {}
-
 impl ErrorType for PwmWrapper {
-    type Error = ErrorWrapper;
+    type Error = WrappingError<rpi_pal::pwm::Error>;
 }
 
 impl SetDutyCycle for PwmWrapper {
@@ -51,6 +34,6 @@ impl SetDutyCycle for PwmWrapper {
 
     fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
         trace!("PwmWrapper::set_duty_cycle(duty: {duty})");
-        self.pwm.set_duty_cycle((duty as f64) / (u16::MAX as f64)).map_err(ErrorWrapper)
+        self.pwm.set_duty_cycle((duty as f64) / (u16::MAX as f64)).map_err(WrappingError)
     }
 }

flight/src/hardware/sim/hardware.rs

@@ -0,0 +1,57 @@
+
+use std::sync::Mutex;
+use crate::hardware::Hardware;
+use crate::hardware::mcp23017::{Mcp23017, Mcp23017Driver};
+use crate::hardware::mct8316a::{Mct8316AVDriver, Mct8316a};
+use anyhow::Result;
+use embedded_hal_bus::i2c::MutexDevice;
+use log::trace;
+use crate::hardware::sim::mcp23017::SimMcp23017;
+use crate::hardware::sim::mct8316a::SimMct8316a;
+use crate::hardware::sim::pwm::SimPwm;
+
+pub struct SimHardware {
+    mcp23017a: Mutex<SimMcp23017>,
+    mcp23017b: Mutex<SimMcp23017>,
+    mct8316a: Mutex<SimMct8316a>,
+    battery_voltage: f64
+}
+
+impl SimHardware {
+    pub fn new() -> Self {
+        Self {
+            mcp23017a: SimMcp23017::new().into(),
+            mcp23017b: SimMcp23017::new().into(),
+            mct8316a: SimMct8316a::new().into(),
+            battery_voltage: 12.4f64,
+        }
+    }
+}
+
+impl Hardware for SimHardware {
+    type Pwm = SimPwm;
+
+    fn new_mcp23017_a(&self) -> Result<impl Mcp23017 + Sync> {
+        trace!("SimHardware::new_mcp23017_a()");
+        Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017a), 0b0100000))
+    }
+
+    fn new_mcp23017_b(&self) -> Result<impl Mcp23017 + Sync> {
+        trace!("SimHardware::new_mcp23017_b()");
+        Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017b), 0b0100000))
+    }
+
+    fn new_pwm0(&self) -> Result<Self::Pwm> {
+        trace!("SimHardware::new_pwm0()");
+        Ok(SimPwm::new())
+    }
+
+    fn new_mct8316a(&self) -> Result<impl Mct8316a + Sync> {
+        trace!("SimHardware::new_mct8316a()");
+        Ok(Mct8316AVDriver::new(MutexDevice::new(&self.mct8316a), 0b0000000))
+    }
+
+    fn get_battery_voltage(&self) -> Result<f64> {
+        Ok(self.battery_voltage)
+    }
+}

flight/src/hardware/sim/mcp23017.rs

@@ -0,0 +1,51 @@
+use std::fmt::{Display, Formatter};
+use embedded_hal::i2c::{ErrorKind, ErrorType, I2c, Operation, SevenBitAddress};
+
+pub struct SimMcp23017 {
+
+}
+
+impl SimMcp23017 {
+    pub fn new() -> Self {
+        Self {
+
+        }
+    }
+}
+
+#[derive(Debug)]
+pub struct ErrorWrapper(anyhow::Error);
+
+impl embedded_hal::i2c::Error for ErrorWrapper {
+    fn kind(&self) -> ErrorKind {
+        ErrorKind::Other
+    }
+}
+
+impl Display for ErrorWrapper {
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        self.0.fmt(f)
+    }
+}
+
+impl std::error::Error for ErrorWrapper {}
+
+impl ErrorType for SimMcp23017 {
+    type Error = ErrorWrapper;
+}
+
+impl I2c for SimMcp23017 {
+    fn transaction(&mut self, _address: SevenBitAddress, operations: &mut [Operation<'_>]) -> Result<(), Self::Error> {
+        for operation in operations {
+            match operation {
+                Operation::Write(_write_buffer) => {
+                    // Ignore incoming write operations
+                }
+                Operation::Read(_read_buffer) => {
+                    todo!("SimMcp23017 read")
+                }
+            }
+        }
+        Ok(())
+    }
+}

flight/src/hardware/sim/mct8316a.rs

@@ -0,0 +1,95 @@
+use crate::hardware::error::WrappingError;
+use anyhow::anyhow;
+use embedded_hal::i2c::{ErrorType, I2c, Operation, SevenBitAddress};
+use std::collections::HashMap;
+
+const CRC: crc::Crc<u8> = crc::Crc::<u8>::new(&crc::CRC_8_SMBUS);
+
+pub struct SimMct8316a {
+    data: HashMap<u32, u32>
+}
+
+impl SimMct8316a {
+    pub fn new() -> Self {
+        Self {
+            data: HashMap::new(),
+        }
+    }
+}
+
+impl ErrorType for SimMct8316a {
+    type Error = WrappingError<anyhow::Error>;
+}
+
+impl I2c for SimMct8316a {
+    fn transaction(&mut self, i2c_addr: SevenBitAddress, operations: &mut [Operation<'_>]) -> Result<(), Self::Error> {
+        let mut do_read_operation = false;
+        let mut include_crc = false;
+        let mut data_length = 2;
+        let mut address: u32 = 0;
+        let mut crc = CRC.digest();
+        for operation in operations {
+            match operation {
+                Operation::Write(write_buffer) => {
+                    // Ignore incoming write operations
+                    if write_buffer.len() < 3 {
+                        return Err(WrappingError(anyhow!("Not enough data in write")));
+                    }
+                    // We don't care about the first byte
+                    do_read_operation = write_buffer[0] & 0x80 > 0;
+                    include_crc = write_buffer[0] & 0x40 > 0;
+                    if write_buffer[0] & 0x10 > 0 {
+                        data_length = 4;
+                    } else if write_buffer[0] & 0x20 > 0 {
+                        data_length = 8;
+                    }
+                    address |= ((write_buffer[0] & 0xF) as u32) << 16;
+                    address |= (write_buffer[1] as u32) << 8;
+                    address |= (write_buffer[2] as u32) << 0;
+                    if !do_read_operation {
+                        if write_buffer.len() != 3 + data_length + 1 {
+                            return Err(WrappingError(anyhow!("Write for write has wrong length {}. {} expected", write_buffer.len(), 3 + data_length + 1)));
+                        }
+                        if data_length == 2 {
+                            todo!("Unimplemented");
+                        } else {
+                            let written_value = u32::from_be_bytes([write_buffer[3], write_buffer[4], write_buffer[5], write_buffer[6]]);
+                            self.data.insert(address, written_value);
+                            if data_length == 8 {
+                                todo!("Unimplemented");
+                            }
+                        }
+                    } else {
+                        if write_buffer.len() != 3 {
+                            return Err(WrappingError(anyhow!("Write for read has wrong length {}. {} expected", write_buffer.len(), 3)));
+                        }
+                    }
+                    crc.update(&write_buffer);
+                }
+                Operation::Read(read_buffer) => {
+                    if !do_read_operation {
+                        return Err(WrappingError(anyhow!("Unexpected Read Operation")));
+                    }
+                    let expected_length = data_length + if include_crc { 1 } else { 0 };
+                    if read_buffer.len() != expected_length {
+                        return Err(WrappingError(anyhow!("Unexpected length in read buffer {}. {} expected", read_buffer.len(), expected_length)));
+                    }
+                    crc.update(&[((i2c_addr as u8) << 1) | 0b1]);
+                    if data_length == 2 {
+                        todo!("Unimplemented");
+                    } else if data_length == 4 {
+                        let value = *self.data.get(&address).unwrap_or(&0);
+                        read_buffer[0..4].copy_from_slice(&value.to_be_bytes());
+                    } else {
+                        todo!("Unimplemented");
+                    }
+                    crc.update(&read_buffer[0..data_length]);
+                    if do_read_operation {
+                        read_buffer[data_length] = crc.clone().finalize();
+                    }
+                }
+            }
+        }
+        Ok(())
+    }
+}

flight/src/hardware/sim/mod.rs

@@ -0,0 +1,12 @@
+#[cfg(not(feature = "raspi"))]
+mod mcp23017;
+#[cfg(not(feature = "raspi"))]
+mod pwm;
+
+pub(super) mod mct8316a;
+
+#[cfg(not(feature = "raspi"))]
+pub mod hardware;
+
+#[cfg(not(feature = "raspi"))]
+pub use hardware::SimHardware;

flight/src/hardware/sim/pwm.rs

@@ -0,0 +1,49 @@
+use std::fmt::{Display, Formatter};
+use embedded_hal::pwm::{ErrorKind, ErrorType, SetDutyCycle};
+use log::trace;
+
+pub struct SimPwm {
+    duty_cycle: u16,
+}
+
+impl SimPwm {
+    pub fn new() -> Self {
+        Self {
+            duty_cycle: 0,
+        }
+    }
+}
+
+#[derive(Debug)]
+pub struct ErrorWrapper(anyhow::Error);
+
+impl embedded_hal::pwm::Error for ErrorWrapper {
+    fn kind(&self) -> ErrorKind {
+        ErrorKind::Other
+    }
+}
+
+impl Display for ErrorWrapper {
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        self.0.fmt(f)
+    }
+}
+
+impl std::error::Error for ErrorWrapper {}
+
+impl ErrorType for SimPwm {
+    type Error = ErrorWrapper;
+}
+
+impl SetDutyCycle for SimPwm {
+    fn max_duty_cycle(&self) -> u16 {
+        trace!("SimPwm::max_duty_cycle()");
+        u16::MAX
+    }
+
+    fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
+        trace!("SimPwm::set_duty_cycle(duty: {duty})");
+        self.duty_cycle = duty;
+        Ok(())
+    }
+}

flight/src/lib.rs

@@ -8,7 +8,7 @@ use crate::on_drop::on_drop;
 use anyhow::Result;
 use embedded_hal::digital::PinState;
 use embedded_hal::pwm::SetDutyCycle;
-use log::info;
+use log::{debug, info};
 use std::sync::atomic::{AtomicBool, Ordering};
 use std::sync::Arc;
 use std::thread;
@@ -66,20 +66,24 @@ pub fn run() -> Result<()> {
         let mut led_pin_b = mcp23017_b.new_output_pin(MCP23017_B_LED)?;
         led_pin_b.set_state_on_drop(PinState::Low);
 
-
+        info!("Starting Main Loop");
         loop {
+            debug!("A On");
             led_pin_a.set_state(PinState::High);
             sleep(Duration::from_secs(1));
             if !running.load(Ordering::Relaxed) { break; };
 
+            debug!("B On");
             led_pin_b.set_state(PinState::High);
             sleep(Duration::from_secs(1));
             if !running.load(Ordering::Relaxed) { break; };
 
+            debug!("A Off");
             led_pin_a.set_state(PinState::Low);
             sleep(Duration::from_secs(1));
             if !running.load(Ordering::Relaxed) { break; };
 
+            debug!("B Off");
             led_pin_b.set_state(PinState::Low);
             sleep(Duration::from_secs(1));
             if !running.load(Ordering::Relaxed) { break; };