diff --git a/flight/src/data/mod.rs b/flight/src/data/mod.rs index 458ce01..8e29372 100644 --- a/flight/src/data/mod.rs +++ b/flight/src/data/mod.rs @@ -1,4 +1,4 @@ -#[cfg(not(feature = "raspi"))] -pub mod reader; -#[cfg(not(feature = "raspi"))] -pub mod writer; +// #[cfg(not(feature = "raspi"))] +// pub mod reader; +// #[cfg(not(feature = "raspi"))] +// pub mod writer; diff --git a/flight/src/hardware/error.rs b/flight/src/hardware/error.rs deleted file mode 100644 index c207d52..0000000 --- a/flight/src/hardware/error.rs +++ /dev/null @@ -1,35 +0,0 @@ -use std::error::Error; -use std::fmt::{Debug, Display, Formatter}; - -#[derive(Copy, Clone, Debug)] -pub struct I2cError(pub ERR); - -impl Display for I2cError { - fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { - writeln!(f, "I2cError({:?})", self.0) - } -} - -impl Error for I2cError {} - -#[derive(Copy, Clone, Debug)] -pub struct SpiError(pub ERR); - -impl Display for SpiError { - fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { - writeln!(f, "SpiError({:?})", self.0) - } -} - -impl Error for SpiError {} - -// #[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 { } diff --git a/flight/src/hardware/error/mod.rs b/flight/src/hardware/error/mod.rs new file mode 100644 index 0000000..1c05794 --- /dev/null +++ b/flight/src/hardware/error/mod.rs @@ -0,0 +1,3 @@ +mod wrapping; + +pub use wrapping::WrappingError; diff --git a/flight/src/hardware/error/wrapping.rs b/flight/src/hardware/error/wrapping.rs new file mode 100644 index 0000000..42f827d --- /dev/null +++ b/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(pub ERR); + +impl Display for WrappingError { + fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { + writeln!(f, "WrappingError({:?})", self.0) + } +} + +impl Error for WrappingError {} + +impl embedded_hal::i2c::Error for WrappingError { + fn kind(&self) -> ErrorKind { + ErrorKind::Other + } +} + +impl embedded_hal::pwm::Error for WrappingError { + fn kind(&self) -> embedded_hal::pwm::ErrorKind { + embedded_hal::pwm::ErrorKind::Other + } +} diff --git a/flight/src/hardware/mcp23017/driver.rs b/flight/src/hardware/mcp23017/driver.rs index 7aea830..cf51464 100644 --- a/flight/src/hardware/mcp23017/driver.rs +++ b/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(); } } diff --git a/flight/src/hardware/mcp3208/mod.rs b/flight/src/hardware/mcp3208/mod.rs index d05581b..e8fc433 100644 --- a/flight/src/hardware/mcp3208/mod.rs +++ b/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, @@ -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; diff --git a/flight/src/hardware/mct8316a/driver.rs b/flight/src/hardware/mct8316a/driver.rs index 853b0b0..11d317b 100644 --- a/flight/src/hardware/mct8316a/driver.rs +++ b/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) => { diff --git a/flight/src/hardware/mod.rs b/flight/src/hardware/mod.rs index cf2f153..92b112c 100644 --- a/flight/src/hardware/mod.rs +++ b/flight/src/hardware/mod.rs @@ -27,18 +27,14 @@ pub fn initialize() -> Result { #[cfg(not(feature = "raspi"))] #[allow(unreachable_code)] pub fn initialize() -> Result { - panic!("Can not Initialize"); - - Ok(()) + Ok(sim::SimHardware::new()) } -#[cfg(not(feature = "raspi"))] -mod bno085; -#[cfg(not(feature = "raspi"))] -mod imu; -mod error; +pub mod error; pub mod mcp23017; +#[cfg(feature = "raspi")] mod mcp3208; pub mod channelization; -pub(crate) mod mct8316a; +pub mod mct8316a; +mod sim; diff --git a/flight/src/hardware/raspi/mod.rs b/flight/src/hardware/raspi/mod.rs index 391962d..231192c 100644 --- a/flight/src/hardware/raspi/mod.rs +++ b/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, mcp3208: RefCell>, + mct8316a: Mutex, } 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 { trace!("RaspiHardware::new_mct8316a()"); - Ok(Mct8316AVDriver::new(MutexDevice::new(&self.i2c_bus), 0b0000000)) + Ok(Mct8316AVDriver::new(MutexDevice::new(&self.mct8316a), 0b0000000)) } diff --git a/flight/src/hardware/raspi/pwm.rs b/flight/src/hardware/raspi/pwm.rs index 797cd20..d57b3ba 100644 --- a/flight/src/hardware/raspi/pwm.rs +++ b/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; } 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) } } diff --git a/flight/src/hardware/sim/hardware.rs b/flight/src/hardware/sim/hardware.rs new file mode 100644 index 0000000..77d95af --- /dev/null +++ b/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, + mcp23017b: Mutex, + mct8316a: Mutex, + 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 { + trace!("SimHardware::new_mcp23017_a()"); + Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017a), 0b0100000)) + } + + fn new_mcp23017_b(&self) -> Result { + trace!("SimHardware::new_mcp23017_b()"); + Ok(Mcp23017Driver::new(MutexDevice::new(&self.mcp23017b), 0b0100000)) + } + + fn new_pwm0(&self) -> Result { + trace!("SimHardware::new_pwm0()"); + Ok(SimPwm::new()) + } + + fn new_mct8316a(&self) -> Result { + trace!("SimHardware::new_mct8316a()"); + Ok(Mct8316AVDriver::new(MutexDevice::new(&self.mct8316a), 0b0000000)) + } + + fn get_battery_voltage(&self) -> Result { + 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 new file mode 100644 index 0000000..b5805ed --- /dev/null +++ b/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(()) + } +} diff --git a/flight/src/hardware/sim/mct8316a.rs b/flight/src/hardware/sim/mct8316a.rs new file mode 100644 index 0000000..9e173b5 --- /dev/null +++ b/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 = crc::Crc::::new(&crc::CRC_8_SMBUS); + +pub struct SimMct8316a { + data: HashMap +} + +impl SimMct8316a { + pub fn new() -> Self { + Self { + data: HashMap::new(), + } + } +} + +impl ErrorType for SimMct8316a { + type Error = WrappingError; +} + +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(()) + } +} diff --git a/flight/src/hardware/sim/mod.rs b/flight/src/hardware/sim/mod.rs new file mode 100644 index 0000000..339975a --- /dev/null +++ b/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; diff --git a/flight/src/hardware/sim/pwm.rs b/flight/src/hardware/sim/pwm.rs new file mode 100644 index 0000000..3624c30 --- /dev/null +++ b/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(()) + } +} diff --git a/flight/src/lib.rs b/flight/src/lib.rs index e9a6e47..63fe036 100644 --- a/flight/src/lib.rs +++ b/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; };