make some updates

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