make some updates

flight/src/data/mod.rs

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

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);

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<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.kind())
+        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 { }
+// #[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/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: I2C,
+    address: u8,
+    bank: [u8; 2],
+    dirty: bool,
+}
+
+impl<I2C> Mcp23017<I2C>
+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(())
+    }
+}

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: SPI,
+    vref: f64
+}
+
+impl<SPI> Mcp3208<SPI>
+where
+    SPI : SpiDevice<u8>,
+    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<f64> {
+        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)
+    }
+}

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<f64>;
 }
 
-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<impl Hardware> {
     Ok(())
 }
 
+#[cfg(not(feature = "raspi"))]
 mod bno085;
+#[cfg(not(feature = "raspi"))]
 mod imu;
 mod error;
+
+mod mcp23017;
+mod mcp3208;

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<Mcp23017<I2c>>,
+    mcp3208: RefCell<Mcp3208<SimpleHalSpiDevice>>,
 }
 
 impl RaspiHardware {
     pub fn new() -> Result<Self> {
         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<f64> {
+        self.mcp3208.borrow_mut().read_single(1)
+    }
 }

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(())

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(())
 }