diff --git a/Cargo.lock b/Cargo.lock index 3889c2d..52064f5 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -238,7 +238,7 @@ version = "0.1.0" [[package]] name = "nautilus_flight" -version = "0.1.0" +version = "0.0.1" dependencies = [ "anyhow", "chrono", diff --git a/flight/Cargo.toml b/flight/Cargo.toml index f017ee3..8398a34 100644 --- a/flight/Cargo.toml +++ b/flight/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "nautilus_flight" -version = "0.1.0" +version = "0.0.1" edition = "2024" [dependencies] diff --git a/flight/src/data/mod.rs b/flight/src/data/mod.rs index c9134a0..458ce01 100644 --- a/flight/src/data/mod.rs +++ b/flight/src/data/mod.rs @@ -1,2 +1,4 @@ +#[cfg(not(feature = "raspi"))] pub mod reader; +#[cfg(not(feature = "raspi"))] pub mod writer; diff --git a/flight/src/data/writer.rs b/flight/src/data/writer.rs index 32f336a..e13c86d 100644 --- a/flight/src/data/writer.rs +++ b/flight/src/data/writer.rs @@ -43,7 +43,7 @@ primitive_writeable!(isize, 8); primitive_writeable!(f32, 4); primitive_writeable!(f64, 8); -trait DataWriteAccess { +pub trait DataWriteAccess { fn get_data(&mut self) -> &mut [u8]; fn get_position(&self) -> usize; fn set_position(&mut self, position: usize); diff --git a/flight/src/hardware/error.rs b/flight/src/hardware/error.rs index d88b840..a2e3a62 100644 --- a/flight/src/hardware/error.rs +++ b/flight/src/hardware/error.rs @@ -1,24 +1,35 @@ 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.kind()) + 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 { } +// #[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/mcp23017/mod.rs b/flight/src/hardware/mcp23017/mod.rs new file mode 100644 index 0000000..66ed9fc --- /dev/null +++ b/flight/src/hardware/mcp23017/mod.rs @@ -0,0 +1,63 @@ +use anyhow::{bail, Result}; +use embedded_hal::i2c::I2c; +use crate::hardware::error::I2cError; + +pub struct Mcp23017 { + i2c: I2C, + address: u8, + bank: [u8; 2], + dirty: bool, +} + +impl Mcp23017 +where + I2C: I2c, + I2C::Error : Send, + I2C::Error : Sync, + I2C::Error : 'static +{ + pub fn new(i2c: I2C, address: u8) -> Self { + Self { + i2c, + address, + bank: [0u8; _], + dirty: false, + } + } + + pub fn init(&self) -> Result<()> { + Ok(()) + } + + pub fn set_pin(&mut self, pin: u8, value: bool) -> Result<()> { + let (pin_bank, dirty_flag, pin_index) = match pin { + 0..8 => { + (&mut self.bank[0], &mut self.dirty, pin as u32) + }, + 8 ..16 => { + (&mut self.bank[1], &mut self.dirty, (pin as u32) - 8) + }, + _ => bail!("Invalid Pin ID"), + }; + let pin_mask = 1u8.unbounded_shl(pin_index); + let initial = *pin_bank; + if value { + *pin_bank |= pin_mask; + } else { + *pin_bank &= !pin_mask; + } + let result = *pin_bank; + if initial != result { + *dirty_flag = true; + } + Ok(()) + } + + pub fn flush(&mut self) -> Result<()> { + if self.dirty { + let data: [u8; _] = [0x12, self.bank[0], self.bank[1]]; + self.i2c.write(self.address, &data).map_err(I2cError)?; + } + Ok(()) + } +} diff --git a/flight/src/hardware/mcp3208/mod.rs b/flight/src/hardware/mcp3208/mod.rs new file mode 100644 index 0000000..81206ed --- /dev/null +++ b/flight/src/hardware/mcp3208/mod.rs @@ -0,0 +1,39 @@ +use embedded_hal::spi::{Operation, SpiDevice}; +use anyhow::{ensure, Result}; +use crate::hardware::error::SpiError; + +pub struct Mcp3208 { + spi: SPI, + vref: f64 +} + +impl Mcp3208 +where + SPI : SpiDevice, + SPI::Error : Send, + SPI::Error : Sync, + SPI::Error : 'static, +{ + pub fn new(spi: SPI, vref: f64) -> Self { + Self { + spi, + vref + } + } + + pub fn read_single(&mut self, channel: u8) -> Result { + ensure!(channel < 8, "Invalid Channel {channel}"); + + // We don't care what the third byte written is + let mut write_bits = [0b00000110, 0b00000000, 0b00000000]; + write_bits[0] |= (channel.unbounded_shr(2)) & 0xFF; + 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)?; + + let value: u16 = u16::from_be_bytes([read_bits[1], read_bits[2]]) & 0x0FFF; + + Ok(((value as f64) / (0xFFF as f64)) * self.vref) + } +} \ No newline at end of file diff --git a/flight/src/hardware/mod.rs b/flight/src/hardware/mod.rs index 90eac85..fdd7076 100644 --- a/flight/src/hardware/mod.rs +++ b/flight/src/hardware/mod.rs @@ -1,11 +1,16 @@ use anyhow::Result; +use embedded_hal::i2c::I2c; pub trait Hardware { + fn set_mcp0_pin(&self, pin: u8, value: bool) -> Result<()>; + fn get_battery_voltage(&self) -> Result; } -impl Hardware for () { - -} +// impl Hardware for () { +// fn get_i2c0_bus(&self) -> impl I2c { +// panic!() +// } +// } #[cfg(feature = "raspi")] mod raspi; @@ -23,6 +28,11 @@ pub fn initialize() -> Result { Ok(()) } +#[cfg(not(feature = "raspi"))] mod bno085; +#[cfg(not(feature = "raspi"))] mod imu; mod error; + +mod mcp23017; +mod mcp3208; diff --git a/flight/src/hardware/raspi/mod.rs b/flight/src/hardware/raspi/mod.rs index 2a6ad24..dd0da3e 100644 --- a/flight/src/hardware/raspi/mod.rs +++ b/flight/src/hardware/raspi/mod.rs @@ -1,36 +1,52 @@ +use std::cell::RefCell; use crate::hardware::Hardware; use anyhow::Result; -use log::info; +use embedded_hal::i2c::SevenBitAddress; +use log::{debug, info}; use rppal::gpio::Gpio; +use rppal::i2c::I2c; +use rppal::spi::{Bus, Mode, SlaveSelect, Spi}; +use crate::hardware::mcp23017::Mcp23017; +use crate::hardware::mcp3208::Mcp3208; use rppal::spi::SimpleHalSpiDevice; -const CLOCK_3MHZ: u32 = 3_000_000; +const CLOCK_1MHZ: u32 = 1_000_000; pub struct RaspiHardware { gpio: Gpio, + mcp23017_a: RefCell>, + mcp3208: RefCell>, } impl RaspiHardware { pub fn new() -> Result { let device = rppal::system::DeviceInfo::new()?; - info!( - "Running on Raspberry Pi Emulated Hardware Model {} with {}", - device.model(), - device.soc() - ); - + info!("Running on {}", device.model()); + debug!("SOC: {}", device.soc()); + + let mut i2c = I2c::with_bus(1u8)?; + i2c.set_timeout(1000)?; + Ok(Self { gpio: Gpio::new()?, - // spi_bno085: Spi::new( - // Bus::Spi1, - // SlaveSelect::Ss0, - // CLOCK_3MHZ, - // Mode::Mode3, - // )? + mcp23017_a: Mcp23017::new(i2c, 0b0100000).into(), + mcp3208: Mcp3208::new(SimpleHalSpiDevice::new(Spi::new( + Bus::Spi1, + SlaveSelect::Ss1, + CLOCK_1MHZ, + Mode::Mode0, + )?), 3.3f64).into() }) } } impl Hardware for RaspiHardware { - + fn set_mcp0_pin(&self, pin: u8, value: bool) -> Result<()> { + self.mcp23017_a.borrow_mut().set_pin(pin, value)?; + self.mcp23017_a.borrow_mut().flush() + } + + fn get_battery_voltage(&self) -> Result { + self.mcp3208.borrow_mut().read_single(1) + } } diff --git a/flight/src/lib.rs b/flight/src/lib.rs index a77ee5c..1d505dc 100644 --- a/flight/src/lib.rs +++ b/flight/src/lib.rs @@ -1,7 +1,10 @@ +use std::thread::sleep; +use std::time::Duration; use crate::hardware::initialize; use crate::logger::setup_logger; use anyhow::Result; use log::info; +use crate::hardware::Hardware; mod hardware; mod logger; @@ -15,6 +18,15 @@ pub fn run() -> Result<()> { let hal = initialize()?; + for i in 0..100 { + sleep(Duration::from_millis(1000)); + info!("Battery Voltage: {}", hal.get_battery_voltage()?); + } + + // hal.set_mcp0_pin(15u8, true)?; + // sleep(Duration::from_secs(1)); + // hal.set_mcp0_pin(15u8, false)?; + drop(hal); Ok(()) diff --git a/flight/src/logger.rs b/flight/src/logger.rs index 1f38960..229e7ea 100644 --- a/flight/src/logger.rs +++ b/flight/src/logger.rs @@ -2,6 +2,7 @@ use anyhow::Result; use std::env; use std::fs::create_dir_all; use std::str::FromStr; +use log::trace; pub fn setup_logger() -> Result<()> { let log_file = env::var("LOG_FILE").or_else(|_| { @@ -33,7 +34,9 @@ pub fn setup_logger() -> Result<()> { .level(log_level) .chain(std::io::stdout()), ) - .chain(fern::log_file(log_file)?) + .chain(fern::log_file(log_file.clone())?) .apply()?; + + trace!("Logging to {} at level {}", log_file, log_level); Ok(()) }