From 7e8f3891c5e9439a4cbf52f69dc062cdcba38697 Mon Sep 17 00:00:00 2001
From: Niklas Kapelle <niklas@kapelle.org>
Date: Wed, 20 May 2026 14:36:27 +0200
Subject: [PATCH] sensor working

---
 Cargo.lock  | 23 +++++++++++++++++++
 Cargo.toml  |  1 +
 src/main.rs | 65 ++++++++++++++++++++++++++++++++++++++++-------------
 3 files changed, 74 insertions(+), 15 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 18a0374..12c242d 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -279,6 +279,7 @@ dependencies = [
  "heapless 0.9.2",
  "log",
  "mipidsi",
+ "mmc56x3",
 ]
 
 [[package]]
@@ -847,6 +848,17 @@ version = "0.4.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "5e5032e24019045c762d3c0f28f5b6b8bbf38563a65908389bf7978758920897"
 
+[[package]]
+name = "maybe-async"
+version = "0.2.11"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "746873a384ad60adc5db74471dfaba74bd278afbdcfd81db93fafcdfc8b5ca0c"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn 2.0.114",
+]
+
 [[package]]
 name = "memchr"
 version = "2.7.6"
@@ -871,6 +883,17 @@ dependencies = [
  "nb 1.1.0",
 ]
 
+[[package]]
+name = "mmc56x3"
+version = "0.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0f282be9f7322bc57ec9e86843e581c760f32f8084a3c85ef7a7573fdbe154bb"
+dependencies = [
+ "bitflags",
+ "embedded-hal-async",
+ "maybe-async",
+]
+
 [[package]]
 name = "nb"
 version = "0.1.3"
diff --git a/Cargo.toml b/Cargo.toml
index 3b3c2a5..bc62cf9 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -24,6 +24,7 @@ display-interface-spi = "0.5.0"
 mipidsi = "0.9.0"
 embedded-hal-bus = "0.3.0"
 embedded-graphics = "0.8.1"
+mmc56x3 = "0.1.0"
 
 [profile.dev]
 codegen-units = 1
diff --git a/src/main.rs b/src/main.rs
index 27c18fa..18d47c5 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -3,36 +3,38 @@
 
 use embassy_executor::Spawner;
 use embassy_rp::{
-    Peri, bind_interrupts, gpio,
-    peripherals::USB,
-    spi::{self, ClkPin, Instance, Spi},
+    bind_interrupts, gpio,
+    i2c::{self, I2c},
+    peripherals::{self, USB},
+    spi::{self, Spi},
     usb::Driver,
 };
 use embassy_time::Timer;
 use embedded_graphics::prelude::DrawTargetExt;
 use embedded_hal_bus::spi::ExclusiveDevice;
 use gpio::{Level, Output};
-use log::error;
+use log::{error, info};
 use mipidsi::{
     Builder,
     interface::SpiInterface,
     models::ST7789,
     options::{Orientation, Rotation},
 };
+use mmc56x3::MMC56X3;
 
 use crate::display::CoMoDisplay;
 
 mod display;
 
 #[panic_handler]
-fn panic(info: &core::panic::PanicInfo) -> ! {
-    error!("PANIC: {info}");
-
+fn panic(_info: &core::panic::PanicInfo) -> ! {
+    // TODO: How to log panic? 
     loop {}
 }
 
 bind_interrupts!(struct Irqs {
     USBCTRL_IRQ => embassy_rp::usb::InterruptHandler<USB>;
+    I2C1_IRQ => i2c::InterruptHandler<peripherals::I2C1>;
 });
 
 #[embassy_executor::task]
@@ -51,14 +53,13 @@ async fn main(spawner: Spawner) {
     spi_cfg.frequency = 62_500_000;
 
     let inner = p.SPI0;
-    let clk = p.PIN_18;
-    let mosi = p.PIN_19;
-    let cs = p.PIN_17;
-    let dc = p.PIN_16;
-    let rst = p.PIN_15;
+    let clk = p.PIN_6;
+    let mosi = p.PIN_7;
+    let cs = p.PIN_5;
+    let dc = p.PIN_9;
+    let rst = p.PIN_10;
 
     let spi = Spi::new_blocking_txonly(inner, clk, mosi, spi_cfg);
-    // let spi = Spi::new_txonly(inner, clk, mosi, p.DMA_CH0, spi_cfg);
     let cs_display = Output::new(cs, Level::Low);
     let dc_display = Output::new(dc, Level::Low);
     let mut rst_display = Output::new(rst, Level::Low);
@@ -83,7 +84,41 @@ async fn main(spawner: Spawner) {
 
     let _ = como_display.draw();
 
-    loop {
-        Timer::after_secs(1).await;
+    let i2c_sda = p.PIN_14;
+    let i2c_scl = p.PIN_15;
+
+    let i2c = I2c::new_async(p.I2C1, i2c_scl, i2c_sda, Irqs, i2c::Config::default());
+
+    let mut device = MMC56X3::new(i2c, embassy_time::Delay);
+
+    if let Err(e) = device.init().await {
+        error!("Failed to init {:?}", e);
+        reboot().await;
+    };
+
+    if let Err(e) = device.set_data_rate(mmc56x3::DataRate::Max1000Hz).await {
+        error!("Failed to set data rate {:?}", e);
+        reboot().await;
     }
+
+    if let Err(e) = device.set_continuous_mode(true).await {
+        error!("Failed to set_continuous_mode {:?}", e);
+        reboot().await;
+    }
+    for _ in 0..30 {
+        Timer::after_secs(1).await;
+
+        match device.read_messurement().await {
+            Ok(d) => info!("Got: {:?}", d),
+            Err(e) => error!("Error: {:?}", e),
+        }
+    }
+
+    reboot().await;
+}
+
+async fn reboot() {
+    info!("Doing cold boot");
+    Timer::after_secs(1).await;
+    embassy_rp::rom_data::reset_to_usb_boot(0, 0);
 }