Added firmware

firmware/src/main.rs

@@ -0,0 +1,186 @@
+#![no_main]
+#![no_std]
+#![feature(type_alias_impl_trait)]
+use defmt_brtt as _; // global logger
+
+use panic_probe as _;
+use stm32f4xx_hal as _;
+
+// same panicking *behavior* as `panic-probe` but doesn't print a panic message
+// this prevents the panic message being printed *twice* when `defmt::panic` is invoked
+#[defmt::panic_handler]
+fn panic() -> ! {
+    cortex_m::asm::udf()
+}
+
+use rtic::app;
+
+#[rtic::app(
+    device = stm32f4xx_hal::pac,
+    dispatchers = [SPI3]
+)]
+mod app {
+
+    use cortex_m::{asm, singleton};
+    use stm32f4xx_hal::{
+        gpio::{self, gpioa, gpiob, gpioc, Alternate, Analog, Input, OpenDrain, Output, PushPull},
+        i2c,
+        pac::I2C1,
+        prelude::*,
+    };
+
+    use num_traits::{Float, FloatConst};
+
+    use xca9548a::{Error, SlaveAddr, Xca9548a};
+
+    use qmc5883l::{self, OutputDataRate, OversampleRate, QMC5883L};
+
+    use rtic_monotonics::systick::prelude::*;
+
+    systick_monotonic!(Mono, 1000);
+
+    // Shared resources go here
+    #[shared]
+    struct Shared {
+        // TODO: Add resources
+    }
+
+    // Local resources go here
+    #[local]
+    struct Local {
+        i2cmux: Xca9548a<i2c::I2c<I2C1>>,
+        board_led: gpioc::PC13<Output<PushPull>>,
+    }
+
+    #[init]
+    fn init(cx: init::Context) -> (Shared, Local) {
+        defmt::info!("init");
+
+        let rcc = cx.device.RCC.constrain();
+
+        // Freeze the configuration of all the clocks in the system and store the frozen frequencies in
+        // `clocks`
+        let clocks = rcc
+            .cfgr
+            .use_hse(25.MHz())
+            .require_pll48clk()
+            .sysclk(84.MHz())
+            .hclk(84.MHz())
+            .pclk1(42.MHz())
+            .pclk2(84.MHz())
+            .freeze();
+
+        Mono::start(cx.core.SYST, clocks.sysclk().to_Hz());
+
+        defmt::info!("Clock Setup done");
+
+        defmt::info!("Clock Setup done");
+
+        // Acquire the GPIOC peripheral
+        let mut gpiob = cx.device.GPIOB.split();
+        let mut gpioc = cx.device.GPIOC.split();
+
+        let board_led = gpioc.pc13.into_push_pull_output();
+
+        let scl = gpiob.pb6.into_alternate_open_drain();
+        let sda = gpiob.pb7.into_alternate_open_drain();
+        let mut i2c = i2c::I2c::new(
+            cx.device.I2C1,
+            (scl, sda),
+            i2c::Mode::Standard {
+                frequency: 400.kHz(),
+            },
+            &clocks,
+        );
+
+        defmt::info!("I2C Setup done");
+
+        let mut i2cmux = Xca9548a::new(i2c, SlaveAddr::default());
+        i2cmux.select_channels(0b0000_0001).unwrap();
+
+        defmt::info!("I2C MUX Setup done");
+
+        poll_i2c::spawn().ok();
+
+        (
+            Shared {
+                // Initialization of shared resources go here
+            },
+            Local { i2cmux, board_led },
+        )
+    }
+
+    #[task(local = [i2cmux, board_led])]
+    async fn poll_i2c(cx: poll_i2c::Context) {
+        let i2cmux = cx.local.i2cmux;
+        let board_led = cx.local.board_led;
+
+        let parts = i2cmux.split();
+
+        let mut compass1 = QMC5883L::new(parts.i2c0).unwrap();
+        compass1.reset().unwrap();
+        compass1.continuous().unwrap();
+
+        let mut compass2 = QMC5883L::new(parts.i2c1).unwrap();
+        compass2.reset().unwrap();
+        compass2.continuous().unwrap();
+
+        let declination_rads: f32 = 65.0 / 180.0 * f32::PI();
+
+        loop {
+            board_led.toggle();
+
+            loop {
+                defmt::info!("Compass 1");
+                match compass1.mag() {
+                    Ok((x, y, z)) => {
+                        defmt::info!("x1: {} y1: {} z1: {}", x, y, z);
+
+                        let mut heading = (y as f32).atan2(x as f32); //+ declination_rads;
+                        if heading < 0.0 {
+                            heading += 2.0 * f32::PI();
+                        } else if heading > 2.0 * f32::PI() {
+                            heading -= 2.0 * f32::PI();
+                        }
+                        let heading_degrees = heading * 180.0 / f32::PI();
+                        defmt::info!("Heading1 {}", heading_degrees);
+                        break;
+                    }
+                    Err(qmc5883l::Error::NotReady) => {
+                        Mono::delay(1000.millis()).await;
+                    }
+                    e => {
+                        let _ = e.unwrap();
+                    }
+                }
+            }
+
+            loop {
+                defmt::info!("Compass 2");
+                match compass2.mag() {
+                    Ok((x, y, z)) => {
+                        defmt::info!("x2: {} y2: {} z2: {}", x, y, z);
+
+                        let mut heading = (y as f32).atan2(x as f32); //+ declination_rads;
+                        if heading < 0.0 {
+                            heading += 2.0 * f32::PI();
+                        } else if heading > 2.0 * f32::PI() {
+                            heading -= 2.0 * f32::PI();
+                        }
+                        let heading_degrees = heading * 180.0 / f32::PI();
+                        defmt::info!("Heading2 {}", heading_degrees);
+                        break;
+                    }
+                    Err(qmc5883l::Error::NotReady) => {
+                        Mono::delay(1000.millis()).await;
+                    }
+                    e => {
+                        let _ = e.unwrap();
+                    }
+                }
+            }
+
+            Mono::delay(100.millis()).await;
+        }
+    }
+}