Fixed I2C MPU Reading (hopefully)

src/initialize.rs

@@ -63,7 +63,12 @@ pub fn initialize(heap_memory: &'static mut [MaybeUninit<u8>], debug: bool) {
     if debug {
         let _ = send_bytes(b"RTC driver initialized\n");
     }
-    twi::initialize(nrf51_peripherals.TWI0, gpio.p0_04, gpio.p0_02, &mut cortex_m_peripherals.NVIC);
+    twi::initialize(
+        nrf51_peripherals.TWI0,
+        gpio.p0_04,
+        gpio.p0_02,
+        &mut cortex_m_peripherals.NVIC,
+    );
     if debug {
         let _ = send_bytes(b"TWI initialized\n");
     }

src/twi.rs

-use core::sync::atomic::{AtomicBool, Ordering};
-use cortex_m::peripheral::NVIC;
 use crate::mutex::Mutex;
 use crate::once_cell::OnceCell;
+use core::sync::atomic::{AtomicBool, Ordering};
+use cortex_m::peripheral::NVIC;
 use nrf51_hal::gpio::p0::{P0_02, P0_04};
 use nrf51_hal::gpio::{Disconnected, Pin};
 use nrf51_hal::twi::Error;
-use nrf51_pac::twi0::frequency::FREQUENCY_A;
-use nrf51_pac::{GPIO, Interrupt, TWI0};
 use nrf51_pac::interrupt;
+use nrf51_pac::twi0::frequency::FREQUENCY_A;
+use nrf51_pac::{Interrupt, GPIO, TWI0};
 
 const FREQ: FREQUENCY_A = FREQUENCY_A::K400;
 
@@ -24,24 +24,12 @@ impl embedded_hal::blocking::i2c::Write for TwiWrapper {
     type Error = Error;
 
     fn write(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Error> {
-        // Make sure all previously used shortcuts are disabled.
-        self.twi
-            .shorts
-            .write(|w| w.bb_stop().disabled().bb_suspend().disabled());
-
-        // Set Slave I2C address.
-        self.twi
-            .address
-            .write(|w| unsafe { w.address().bits(addr.into()) });
-
-        // Start data transmission.
-        self.twi.tasks_starttx.write(|w| unsafe { w.bits(1) });
-
-        // Clock out all bytes.
-        for byte in bytes {
-            self.send_byte(*byte)?;
+        if bytes.is_empty() {
+            return Ok(());
         }
 
+        self.send_without_stop(addr, bytes)?;
+
         // Send stop.
         self.send_stop()?;
         Ok(())
@@ -54,7 +42,9 @@ impl TwiWrapper {
         self.twi.txd.write(|w| unsafe { w.bits(u32::from(byte)) });
 
         // Wait until transmission was confirmed.
-        while !self.sent.load(Ordering::SeqCst) {}
+        while !self.sent.load(Ordering::SeqCst) {
+            core::hint::spin_loop();
+        }
         self.sent.store(false, Ordering::SeqCst);
 
         Ok(())
@@ -62,7 +52,9 @@ impl TwiWrapper {
 
     fn recv_byte(&self) -> Result<u8, Error> {
         // Wait until something ended up in the buffer.
-        while !self.rec.load(Ordering::SeqCst) {}
+        while !self.rec.load(Ordering::SeqCst) {
+            core::hint::spin_loop();
+        }
         self.rec.store(false, Ordering::SeqCst);
 
         // Read out data.
@@ -76,11 +68,44 @@ impl TwiWrapper {
         self.twi.tasks_stop.write(|w| unsafe { w.bits(1) });
 
         // Wait until stop was sent.
-        while !self.stopped.load(Ordering::SeqCst) {}
+        while !self.stopped.load(Ordering::SeqCst) {
+            core::hint::spin_loop();
+        }
         self.stopped.store(false, Ordering::SeqCst);
 
         Ok(())
     }
+
+    fn send_without_stop(&mut self, addr: u8, bytes: &[u8]) -> Result<(), Error> {
+        // Set Slave I2C address.
+        self.twi
+            .address
+            .write(|w| unsafe { w.address().bits(addr) });
+
+        // Make sure all previously used shortcuts are disabled.
+        self.twi.shorts.reset();
+
+        self.sent.store(false, Ordering::SeqCst);
+
+        self.twi
+            .txd
+            .write(|w| unsafe { w.bits(u32::from(bytes[0])) });
+
+        // Start data transmission.
+        self.twi.tasks_starttx.write(|w| unsafe { w.bits(1) });
+
+        while !self.sent.load(Ordering::SeqCst) {
+            core::hint::spin_loop();
+        }
+        self.sent.store(false, Ordering::SeqCst);
+
+        // Clock out all bytes.
+        for &byte in &bytes[1..] {
+            self.send_byte(byte)?;
+        }
+
+        Ok(())
+    }
 }
 
 impl embedded_hal::blocking::i2c::WriteRead for TwiWrapper {
@@ -92,22 +117,8 @@ impl embedded_hal::blocking::i2c::WriteRead for TwiWrapper {
         bytes: &'w [u8],
         buffer: &'w mut [u8],
     ) -> Result<(), Error> {
-        // Make sure all previously used shortcuts are disabled.
-        self.twi
-            .shorts
-            .write(|w| w.bb_stop().disabled().bb_suspend().disabled());
-
-        // Set Slave I2C address.
-        self.twi
-            .address
-            .write(|w| unsafe { w.address().bits(addr.into()) });
-
-        // Start data transmission.
-        self.twi.tasks_starttx.write(|w| unsafe { w.bits(1) });
-
-        // Send out all bytes in the outgoing buffer.
-        for byte in bytes {
-            self.send_byte(*byte)?;
+        if !bytes.is_empty() {
+            self.send_without_stop(addr, bytes)?;
         }
 
         // Turn around to read data.
@@ -122,7 +133,7 @@ impl embedded_hal::blocking::i2c::WriteRead for TwiWrapper {
             // Start data reception.
             self.twi.tasks_startrx.write(|w| unsafe { w.bits(1) });
 
-            for byte in &mut before.into_iter() {
+            for byte in before.iter_mut() {
                 self.twi.tasks_resume.write(|w| unsafe { w.bits(1) });
                 *byte = self.recv_byte()?;
             }
@@ -137,9 +148,14 @@ impl embedded_hal::blocking::i2c::WriteRead for TwiWrapper {
     }
 }
 
-pub(crate) fn initialize(twi: TWI0, scl_pin: P0_04<Disconnected>, sda_pin: P0_02<Disconnected>, nvic: &mut NVIC) {
-    let scl_pin = Pin::from(scl_pin.into_floating_input());
-    let sda_pin = Pin::from(sda_pin.into_floating_input());
+pub(crate) fn initialize(
+    twi: TWI0,
+    scl_pin: P0_04<Disconnected>,
+    sda_pin: P0_02<Disconnected>,
+    nvic: &mut NVIC,
+) {
+    let scl_pin = Pin::from(scl_pin.into_pullup_input());
+    let sda_pin = Pin::from(sda_pin.into_pullup_input());
 
     // The TWIM peripheral requires the pins to be in a mode that is not
     // exposed through the GPIO API, and might it might not make sense to
@@ -178,16 +194,30 @@ pub(crate) fn initialize(twi: TWI0, scl_pin: P0_04<Disconnected>, sda_pin: P0_02
     twi.frequency.write(|w| w.frequency().variant(FREQ));
 
     // Set which interrupts we want to receive
-    twi.intenset.write(|w| w.txdsent().set_bit().rxdready().set_bit().error().set_bit().stopped().set_bit());
+    twi.intenset.write(|w| {
+        w.txdsent()
+            .set_bit()
+            .rxdready()
+            .set_bit()
+            .error()
+            .set_bit()
+            .stopped()
+            .set_bit()
+    });
+
     twi.enable.write(|w| w.enable().enabled());
 
     // Initialize oncecell
-    TWI.modify(|t| t.initialize(TwiWrapper {
+    // twi.events_rxdready.reset();
+    // twi.events_txdsent.reset();
+    TWI.modify(|t| {
+        t.initialize(TwiWrapper {
             twi,
             rec: false.into(),
             sent: false.into(),
             stopped: false.into(),
-    }));
+        })
+    });
 
     // Setup NVIC
     NVIC::unpend(Interrupt::SPI0_TWI0);
@@ -219,7 +249,9 @@ unsafe fn SPI0_TWI0() {
 
     // Errors are silently ignored
     if twi.twi.events_error.read().bits() != 0 {
-        twi.twi.errorsrc.write(|w| w.anack().clear_bit().overrun().clear_bit()); // Clear error source
+        twi.twi
+            .errorsrc
+            .write(|w| w.anack().clear_bit().overrun().clear_bit()); // Clear error source
         twi.twi.events_error.reset();
     }
 }