navigator2 wip

Signed-off-by: Patrick José Pereira <[email protected]>

Cargo.toml

@@ -14,12 +14,17 @@ readme = "README.md"
 ads1x1x = { git = "https://github.com/eldruin/ads1x1x-rs" }
 ak09915_rs = { git = "https://github.com/bluerobotics/AK09915-rs" }
 approx = "0.5.1"
+bmp180-driver = "0.1.1"
+bmp180-embedded-hal = { version = "0.1.0", features = ["blocking"] }
 bmp280 = "0.4.0"
 dummy-pin = "1.0.0"
 embedded-hal = "1.0.0"
+ftdi = "0.1.3"
+ftdi-embedded-hal = "0.22.1"
 icm20689 = { git = "https://github.com/tstellanova/icm20689" }
 linux-embedded-hal = "0.4.0"
 log = "0.4.19"
+mpu6050 = { git = "https://github.com/patrickelectric/mpu6050", rev = "0fe2526" }
 nb = "1.1.0"
 pwm-pca9685 = "1.0.0"
 sk6812_rpi = "0.1"

src/bmp180.rs

@@ -0,0 +1,144 @@
+use std::error::Error;
+
+//use bmp180_embedded_hal::blocking::{UninitBMP180, BMP180}
+use bmp180_driver::{Common, InitializedBMP180, Resolution, BMP180};
+use linux_embedded_hal::{Delay, I2cdev};
+
+use crate::peripherals::{
+    AnyHardware, BarometerSensor, PeripheralClass, PeripheralInfo, Peripherals, TemperatureSensor,
+};
+
+pub struct Bmp180Device {
+    bmp180: InitializedBMP180<I2cdev, Delay>,
+    info: PeripheralInfo,
+}
+
+impl Bmp180Device {
+    pub fn builder() -> BarometerBuilder {
+        BarometerBuilder::new()
+    }
+
+    pub fn get_peripheral_info(&self) -> &PeripheralInfo {
+        &self.info
+    }
+}
+
+impl AnyHardware for Bmp180Device {
+    fn as_temperature_sensor(&mut self) -> Option<&mut dyn TemperatureSensor> {
+        Some(self)
+    }
+
+    fn as_barometer_sensor(&mut self) -> Option<&mut dyn BarometerSensor> {
+        Some(self)
+    }
+}
+
+pub struct BarometerBuilder {
+    i2c_bus: Result<I2cdev, Box<dyn Error>>,
+    address: u8,
+    info: PeripheralInfo,
+}
+
+impl BarometerBuilder {
+    pub fn new() -> Self {
+        BarometerBuilder {
+            i2c_bus: I2cdev::new("/dev/i2c-1").map_err(|e| Box::new(e) as Box<dyn Error>),
+            address: 0x76,
+            info: PeripheralInfo {
+                peripheral: Peripherals::Bmp180,
+                class: vec![PeripheralClass::Pressure, PeripheralClass::Temperature],
+            },
+        }
+    }
+
+    /// Sets the I²C bus to be used.
+    ///
+    /// # Arguments
+    ///
+    /// * `bus` - The I²C bus (e.g., "/dev/i2c-1").
+    pub fn with_i2c_bus(mut self, bus: &str) -> Self {
+        self.i2c_bus = I2cdev::new(bus).map_err(|e| Box::new(e) as Box<dyn Error>);
+        self
+    }
+
+    pub fn with_i2c(mut self, i2c: Result<I2cdev, Box<dyn Error>>) -> Self {
+        self.i2c_bus = i2c;
+        self
+    }
+
+    /// Sets the I²C address
+    ///
+    /// # Arguments
+    ///
+    /// * `address` - The I²C address (e.g., 0x76).
+    pub fn with_address(mut self, address: u8) -> Self {
+        self.address = address;
+        self
+    }
+
+    pub fn with_peripheral_info(mut self, info: PeripheralInfo) -> Self {
+        self.info = info;
+        self
+    }
+
+    pub fn build(self) -> Result<Bmp180Device, Box<dyn Error>> {
+        let i2c = self.i2c_bus?;
+        let delay = Delay {};
+
+        let mut bmp180 = BMP180::new(i2c, delay);
+        bmp180.check_connection()?;
+        let bmp180 = bmp180.initialize()?;
+
+        Ok(Bmp180Device {
+            bmp180,
+            info: self.info,
+        })
+    }
+}
+
+impl TemperatureSensor for Bmp180Device {
+    fn read_temperature(&mut self) -> Result<f32, Box<dyn Error>> {
+        let temperature = self.bmp180.temperature()?;
+        Ok(temperature / 10.0)
+    }
+}
+
+impl BarometerSensor for Bmp180Device {
+    fn read_pressure(&mut self) -> Result<f32, Box<dyn Error>> {
+        let pressure = self.bmp180.pressure(Resolution::Standard)? as f32;
+        Ok(pressure)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use ftdi_embedded_hal as hal;
+
+    #[test]
+    fn ftdi() {
+        let device = ftdi::find_by_vid_pid(0x0403, 0x6011)
+            .interface(ftdi::Interface::A)
+            .open()
+            .unwrap();
+
+        let hal = hal::FtHal::init_default(device).unwrap();
+        let mut i2c = hal
+            .i2c()
+            .map_err(|e| Box::new(e) as Box<dyn Error>)
+            .unwrap();
+
+        let mut bmp180 = BMP180::new(i2c, Delay {});
+        bmp180.check_connection().unwrap();
+        let mut bmp180 = bmp180.initialize().unwrap();
+
+        loop {
+            let temperature = bmp180.temperature().unwrap();
+            let pressure = bmp180.pressure(Resolution::Standard).unwrap() as f32;
+            println!(
+                "Temperature: {:.2}°C, Pressure: {:.2}hPa",
+                temperature, pressure
+            );
+        }
+    }
+}

src/lib.rs

@@ -6,11 +6,13 @@ use std::fmt;
 
 mod ads1115;
 mod ak09915;
+mod bmp180;
 mod bmp280;
 mod bmp390;
 mod icm20689;
 mod leak;
 mod led;
+mod mpu6050;
 mod pca9685;
 mod peripherals;
 mod rgb;
@@ -21,9 +23,11 @@ use crate::bmp280::Bmp280Device;
 use crate::icm20689::Icm20689Device;
 use ads1115::Ads1115Device;
 use ak09915::Ak09915Device;
+use bmp180::Bmp180Device;
 use bmp390::Bmp390Device;
 use leak::LeakDetector;
 use led::LedController;
+use mpu6050::Mpu6050Device;
 use pca9685::Pca9685Device;
 use rgb::RgbController;
 

src/mpu6050.rs

@@ -0,0 +1,133 @@
+use crate::peripherals::{
+    AccelerometerSensor, AnyHardware, GyroscopeSensor, PeripheralClass, PeripheralInfo,
+    Peripherals, TemperatureSensor,
+};
+use embedded_hal::i2c::I2c;
+use linux_embedded_hal::{Delay, I2cdev};
+use mpu6050::{Mpu6050, Mpu6050Error};
+use std::error::Error;
+
+pub struct Mpu6050Device {
+    mpu: Mpu6050<I2cdev>,
+    info: PeripheralInfo,
+}
+
+impl Mpu6050Device {
+    pub fn builder() -> Mpu6050Builder {
+        Mpu6050Builder::new()
+    }
+
+    pub fn get_peripheral_info(&self) -> &PeripheralInfo {
+        &self.info
+    }
+}
+
+impl AnyHardware for Mpu6050Device {
+    fn as_gyroscope_sensor(&mut self) -> Option<&mut dyn GyroscopeSensor> {
+        Some(self)
+    }
+
+    fn as_accelerometer_sensor(&mut self) -> Option<&mut dyn AccelerometerSensor> {
+        Some(self)
+    }
+
+    fn as_temperature_sensor(&mut self) -> Option<&mut dyn TemperatureSensor> {
+        Some(self)
+    }
+}
+
+pub struct Mpu6050Builder {
+    i2c_device: String,
+    info: PeripheralInfo,
+}
+
+impl Mpu6050Builder {
+    pub fn new() -> Self {
+        Mpu6050Builder {
+            i2c_device: "/dev/i2c-1".to_string(),
+            info: PeripheralInfo {
+                peripheral: Peripherals::Mpu6050,
+                class: vec![PeripheralClass::Accelerometer, PeripheralClass::Gyroscope],
+            },
+        }
+    }
+
+    pub fn with_i2c_device(mut self, device: &str) -> Self {
+        self.i2c_device = device.to_string();
+        self
+    }
+
+    pub fn with_peripheral_info(mut self, info: PeripheralInfo) -> Self {
+        self.info = info;
+        self
+    }
+
+    pub fn build(self) -> Result<Mpu6050Device, Box<dyn Error>> {
+        let i2c = I2cdev::new(&self.i2c_device)?;
+        let mut mpu = Mpu6050::new(i2c);
+        mpu.init(&mut Delay).unwrap();
+        Ok(Mpu6050Device {
+            mpu,
+            info: self.info,
+        })
+    }
+}
+
+impl GyroscopeSensor for Mpu6050Device {
+    fn read_angular_velocity(&mut self) -> Result<(f32, f32, f32), Box<dyn Error>> {
+        let gyro = self.mpu.get_gyro().unwrap();
+        Ok((gyro.x, gyro.y, gyro.z))
+    }
+}
+
+impl AccelerometerSensor for Mpu6050Device {
+    fn read_acceleration(&mut self) -> Result<(f32, f32, f32), Box<dyn Error>> {
+        let acc = self.mpu.get_acc().unwrap();
+        Ok((acc.x, acc.y, acc.z))
+    }
+}
+
+impl TemperatureSensor for Mpu6050Device {
+    fn read_temperature(&mut self) -> Result<f32, Box<dyn Error>> {
+        let temp = self.mpu.get_temp().unwrap();
+        Ok(temp)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use ftdi_embedded_hal as hal;
+    use linux_embedded_hal::Delay;
+    use mpu6050::Mpu6050;
+    use std::error::Error;
+
+    #[test]
+    fn ftdi() -> Result<(), Box<dyn Error>> {
+        let device = ftdi::find_by_vid_pid(0x0403, 0x6011)
+            .interface(ftdi::Interface::A)
+            .open()?;
+
+        let hal = hal::FtHal::init_default(device)?;
+        let i2c = hal.i2c().map_err(|e| Box::new(e) as Box<dyn Error>)?;
+
+        let mut mpu = Mpu6050::new(i2c);
+        mpu.init(&mut Delay).unwrap();
+
+        // Necessary for GY87 board
+        // i2c detect will work after this
+        mpu.set_master_interrupt_enabled(false).unwrap();
+        mpu.set_bypass_enabled(true).unwrap();
+        mpu.set_sleep_enabled(false).unwrap();
+
+        loop {
+            let gyro = mpu.get_gyro().unwrap();
+            let acc = mpu.get_acc().unwrap();
+            println!(
+                "Gyro: x={:.2} y={:.2} z={:.2}, Acc: x={:.2} y={:.2} z={:.2}",
+                gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z
+            );
+        }
+
+        Ok(())
+    }
+}

src/peripherals.rs

@@ -3,11 +3,13 @@ use std::error::Error;
 pub enum Peripherals {
     Ads1115, // ADC
     Ak09915, // 3-axis magnetometer
+    Bmp180,  // Pressure and Temperature
     Bmp280,  // Pressure and Temperature
     Bmp390,  // Pressure and Temperature
     Gpio,
     Icm20689, // Accelerometer and Gyroscope
     Leak,
+    Mpu6050, // Accelerometer, Gyroscope and Temperature
     Pca9685, // PWM controller
     Sk6812,  // Neopixel RGB LED
 }