diff --git a/Cargo.lock b/Cargo.lock
index f5040fab89ca2b83e1f24aba8edc340eda5abbd4..2a42843601ecbd27605bfdac6550da542c6cc157 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -145,6 +145,7 @@ dependencies = [
"embassy-executor",
"embassy-stm32",
"embassy-time",
+ "heapless",
"i2c2-slave",
"panic-probe",
]
@@ -546,7 +547,7 @@ version = "0.1.0"
dependencies = [
"defmt",
"embassy-stm32",
- "embassy-sync",
+ "heapless",
]
[[package]]
diff --git a/controller/Cargo.toml b/controller/Cargo.toml
index cee26e164b348095bba0761a01ab0e046ac19292..2dca1865d725950d1236af8639b3ddd35a1c01c6 100644
--- a/controller/Cargo.toml
+++ b/controller/Cargo.toml
@@ -12,5 +12,6 @@ defmt-rtt = "0.4.0"
embassy-executor = { git = "https://github.com/embassy-rs/embassy", features = ["arch-cortex-m", "executor-thread", "defmt", "integrated-timers"] }
embassy-stm32 = { git = "https://github.com/embassy-rs/embassy", features = ["defmt", "stm32f103c8", "unstable-pac", "time-driver-tim4", "memory-x"] }
embassy-time = { git = "https://github.com/embassy-rs/embassy", features = ["defmt", "defmt-timestamp-uptime", "tick-hz-32_768"] }
+heapless = "0.8.0"
i2c2-slave = { path = "../i2c2-slave" }
panic-probe = { version = "0.3.1", features = ["print-defmt"] }
diff --git a/controller/python/controller.py b/controller/python/controller.py
index 1d55bea64e0aa2b792cc86bd24369562f2d163a6..3994135bb886b5bca49c81b1cb6e26e731c1ccae 100644
--- a/controller/python/controller.py
+++ b/controller/python/controller.py
@@ -1,12 +1,7 @@
-import logging
import smbus
import struct
from typing import Optional
-logger = logging.getLogger(__name__)
-
-_CLOCK_STRETCH_WARNING = "flipped bit due to bogus SOC – set I²C bus speed to 400kHz"
-
class Controller:
@@ -19,46 +14,37 @@ class Controller:
ENCODER_TICKS = 0x32
def __init__(self):
- self.i2c = smbus.SMBus(self.I2C_BUS)
- self.clock_stretch_warning_sent = False
-
- def _read_i2c_block_data(self, cmd, len):
- response = self.i2c.read_i2c_block_data(self.I2C_ADDR, cmd, len + 1)
- if response[0] != 0 and not self.clock_stretch_warning_sent:
- logger.warn(_CLOCK_STRETCH_WARNING)
- self.clock_stretch_warning_sent = True
- return response[1:]
-
- def _write_i2c_block_data(self, cmd, data):
- self.i2c.write_i2c_block_data(self.IC2_ADDR, cmd, data)
+ self.i2c = smbus.SMBus(1)
def who_am_i(self) -> int:
"""Check that the motors controller board is present. This should return
the same value as Controller.I2C_ADDR."""
- return self._read_i2c_block_data(self.WHO_AM_I, 1)[0]
+ return self.i2c.read_i2c_block_data(self.I2C_ADDR, self.WHO_AM_I, 1)[0]
def set_max_percentage(self, percent: int):
- """Set the maximum percentage of power which will be used
- (between 1 and 100)."""
+ """Set the maximum percentage of power which will be used (between 1 and 100)."""
if percent <= 0 or percent > 100:
raise ValueError
- self._write_i2c_block_data(self.MAX_MOTOR_PERCENTAGE, [percent])
+ self.i2c.write_i2c_block_data(
+ self.I2C_ADDR, self.MAX_MOTOR_PERCENTAGE, [percent]
+ )
def set_motor_speed(self, left: Optional[int], right: Optional[int]):
- """Set the motor speed between -127 and 127. None means not to
- change the motor value. Using None for both motors will put
- the controller board in standby mode and motors will stop.
-
- """
+ """Set the motor speed between -127 and 127. None means not to change the
+ motor value. Using None for both motors will put the controller board in standby
+ mode and motors will stop."""
def convert(v, arg):
if v is None:
return -128
if not isinstance(v, int) or v < -127 or v > 127:
- raise ValueError(f"{arg} must be [-127..127] or None")
+ raise ValueError(
+ f"{arg} must be an integer between -127 and 127 or None"
+ )
return v
- self._write_i2c_block_data(
+ self.i2c.write_i2c_block_data(
+ self.I2C_ADDR,
self.MOTOR_SPEED,
list(struct.pack("bb", convert(left, "left"), convert(right, "right"))),
)
@@ -76,12 +62,10 @@ class Controller:
self.set_motor_speed(None, None)
def get_encoder_ticks(self) -> (int, int):
- """Retrieve the encoder ticks since the last time it was
- queried. The ticks must be retrieved before they overflow a 2
- byte signed integer (-32768..32767) or the result will make no
- sense. Return a pair with left and right data.
-
- """
+ """Retrieve the encoder ticks since the last time it was queried. The ticks must be
+ retrieved before they overflow a 2 byte signed integer (-32768..32767) or the result will
+ make no sense. Return a pair with left and right data."""
return struct.unpack(
- "hh", bytes(self._read_i2c_block_data(self.ENCODER_TICKS, 4))
+ "hh",
+ bytes(self.i2c.read_i2c_block_data(self.I2C_ADDR, self.ENCODER_TICKS, 4)),
)
diff --git a/controller/src/logic.rs b/controller/src/logic.rs
index c6d3e7b52f34beb03e026e438adc5a7fda690c2e..a9438bd859b08e556e1b8fb509c707a2e32246b7 100644
--- a/controller/src/logic.rs
+++ b/controller/src/logic.rs
@@ -2,10 +2,11 @@ use crate::{
encoders::Encoders,
tb6612fng::{Movement, Tb6612fng},
};
-use i2c2_slave::I2C2;
+use heapless::Deque;
+use i2c2_slave::{I2C2, MESSAGE_SIZE};
struct State {
- i2c: I2C2,
+ _i2c: I2C2,
motors: Tb6612fng<'static>,
encoders: Encoders<'static>,
max_motor_percentage: u8,
@@ -16,7 +17,7 @@ struct State {
impl State {
fn new(i2c2: I2C2, motors: Tb6612fng<'static>, encoders: Encoders<'static>) -> Self {
Self {
- i2c: i2c2,
+ _i2c: i2c2,
motors,
encoders,
max_motor_percentage: 100,
@@ -26,39 +27,19 @@ impl State {
}
}
-#[derive(Debug)]
-enum Error {
- #[allow(unused)]
- I2c(i2c2_slave::Error),
- #[allow(unused)]
- UnknownRegister(u8),
-}
-
-impl From<i2c2_slave::Error> for Error {
- fn from(value: i2c2_slave::Error) -> Self {
- Error::I2c(value)
- }
-}
-
-type Result<T, E = Error> = core::result::Result<T, E>;
+static mut STATE: Option<State> = None;
#[embassy_executor::task]
-pub async fn main_loop(i2c2: I2C2, mut motors: Tb6612fng<'static>, encoders: Encoders<'static>) {
+pub async fn start_i2c_target(
+ i2c2: I2C2,
+ mut motors: Tb6612fng<'static>,
+ encoders: Encoders<'static>,
+) {
motors.standby_enter();
- let mut state = State::new(i2c2, motors, encoders);
- let mut buffer = [0; 8];
- loop {
- match state.i2c.receive(&mut buffer).await {
- Ok(n) => {
- if let Err(e) = handle_command(&buffer[..n], &mut state) {
- defmt::warn!("Error processing command: {:?}", defmt::Debug2Format(&e));
- }
- }
- Err(e) => {
- defmt::warn!("I2C receive error: {:?}", defmt::Debug2Format(&e));
- }
- }
+ unsafe {
+ STATE = Some(State::new(i2c2, motors, encoders));
}
+ I2C2::set_callback(handle_command);
}
const WHO_AM_I: u8 = 0x0f;
@@ -66,10 +47,17 @@ const MAX_MOTOR_PERCENTAGE: u8 = 0x11;
const MOTOR_SPEED: u8 = 0x30;
const ENCODER_TICKS: u8 = 0x32;
-fn handle_command(command: &[u8], state: &mut State) -> Result<()> {
+fn handle_command(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
+ #[allow(static_mut_refs)]
+ let Some(state) = (unsafe { &mut STATE }) else {
+ defmt::trace!("state not installed yet");
+ return;
+ };
defmt::trace!("Processing command {:?}", command);
match command {
- [WHO_AM_I, ..] => state.i2c.send(&[0x57])?,
+ [WHO_AM_I, ..] => {
+ let _ = response.push_back(0x57);
+ }
&[MAX_MOTOR_PERCENTAGE, p, ..] => {
if (1..=100).contains(&p) {
state.max_motor_percentage = p;
@@ -77,7 +65,9 @@ fn handle_command(command: &[u8], state: &mut State) -> Result<()> {
defmt::warn!("Incorrect max percentage {}", p);
}
}
- [MAX_MOTOR_PERCENTAGE] => state.i2c.send(&[state.max_motor_percentage])?,
+ [MAX_MOTOR_PERCENTAGE] => {
+ let _ = response.push_back(state.max_motor_percentage);
+ }
[MOTOR_SPEED, ms @ ..] if ms.len() >= 2 => {
state.motor_speed = [ms[0], ms[1]];
if ms[0] == 0x80 && ms[1] == 0x80 {
@@ -101,14 +91,24 @@ fn handle_command(command: &[u8], state: &mut State) -> Result<()> {
}
}
}
- [MOTOR_SPEED, ..] => state.i2c.send(&state.motor_speed)?,
+ [MOTOR_SPEED, ..] => {
+ for &s in &state.motor_speed {
+ let _ = response.push_back(s);
+ }
+ }
[ENCODER_TICKS, ..] => {
let (left, right) = state.encoders.ticks();
let (left, right) = (left.to_le_bytes(), right.to_le_bytes());
- state.i2c.send(&[left[0], left[1], right[0], right[1]])?;
+ let _ = response.push_back(left[0]);
+ let _ = response.push_back(left[1]);
+ let _ = response.push_back(right[0]);
+ let _ = response.push_back(right[1]);
+ }
+ &[idx, ..] => {
+ defmt::warn!("unknown register {:#04x}", idx);
+ }
+ &[] => {
+ defmt::warn!("received empty command");
}
- &[idx, ..] => return Err(Error::UnknownRegister(idx)),
- [] => (),
}
- Ok(())
}
diff --git a/controller/src/main.rs b/controller/src/main.rs
index 6b20a278e688a9ccd02a805d48cc50b2fe6bd286..d824107ffdcb9582eecdce146f2d8b7d6314d5dd 100644
--- a/controller/src/main.rs
+++ b/controller/src/main.rs
@@ -48,7 +48,7 @@ async fn main(spawner: Spawner) {
let i2c2 = i2c2_slave::I2C2::new(p.I2C2, p.PB10, p.PB11, 0x57, Priority::P4, Priority::P3);
spawner
- .spawn(logic::main_loop(i2c2, motors, encoders))
+ .spawn(logic::start_i2c_target(i2c2, motors, encoders))
.unwrap();
}
diff --git a/i2c2-slave/Cargo.toml b/i2c2-slave/Cargo.toml
index 336c27dc1d9e15b9c353e13998cf5747cb543297..c8431e86392ab00b5dabbbf4211a7972589b46b6 100644
--- a/i2c2-slave/Cargo.toml
+++ b/i2c2-slave/Cargo.toml
@@ -7,4 +7,4 @@ edition = "2021"
[dependencies]
defmt = "0.3.5"
embassy-stm32 = { git = "https://github.com/embassy-rs/embassy", features = ["defmt", "stm32f103c8", "unstable-pac", "time-driver-tim4"] }
-embassy-sync = { git = "https://github.com/embassy-rs/embassy", features = ["defmt"] }
+heapless = "0.8.0"
diff --git a/i2c2-slave/src/lib.rs b/i2c2-slave/src/lib.rs
index 3b19b53c8298728c13506b16f7222b3c41ad10ab..57015a65527907063896bcdd27378019518a4e43 100644
--- a/i2c2-slave/src/lib.rs
+++ b/i2c2-slave/src/lib.rs
@@ -8,47 +8,97 @@ use embassy_stm32::{
pac,
peripherals::{self, PB10, PB11},
};
-use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel};
+use heapless::{Deque, Vec};
-enum I2cRxEvent {
- Byte(u8),
- Stop,
-}
+static mut STATE: State = State::default();
-static RX_QUEUE: Channel<CriticalSectionRawMutex, I2cRxEvent, 9> = Channel::new();
-static TX_QUEUE: Channel<CriticalSectionRawMutex, u8, 9> = Channel::new();
-static mut STATE: State = State::Idle { after_write: false };
+pub const MESSAGE_SIZE: usize = 8;
-#[derive(Debug)]
-pub enum Error {
- ExtraBytes(usize),
- FifoFull,
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+enum Operation {
+ Idle,
+ // The controller is reading data from the target
+ ReadInProgress,
+ // The controller is writing data to the target
+ WriteInProgress,
}
-pub type Result<T, E = Error> = core::result::Result<T, E>;
-
-#[derive(Clone, Copy, Debug)]
-enum State {
- Idle { after_write: bool },
- ReadInProgress { waiting_for_data: bool },
- WriteInProgress,
+type Callback = fn(&[u8], &mut Deque<u8, MESSAGE_SIZE>);
+
+struct State {
+ // The operation in progress
+ operation: Operation,
+ // Data from the controller to the target
+ incoming: Vec<u8, MESSAGE_SIZE>,
+ // Data from the target to the controller
+ outgoing: Deque<u8, MESSAGE_SIZE>,
+ // Callback for exchanging data
+ callback: Option<Callback>,
}
impl State {
- fn get_state() -> Self {
- unsafe { STATE }
+ pub const fn default() -> Self {
+ Self {
+ operation: Operation::Idle,
+ incoming: Vec::new(),
+ outgoing: Deque::new(),
+ callback: None,
+ }
}
- fn set_state(state: Self) {
- defmt::trace!("Setting new state to {:?}", defmt::Debug2Format(&state));
- unsafe { STATE = state }
+ pub fn set_idle(&mut self) {
+ match self.operation {
+ Operation::Idle => {
+ defmt::error!("idle following idle");
+ }
+ Operation::ReadInProgress => {
+ self.outgoing.clear();
+ }
+ Operation::WriteInProgress => {
+ self.callback();
+ }
+ }
+ self.operation = Operation::Idle;
}
- fn is_write(&self) -> bool {
- matches!(
- self,
- Self::Idle { after_write: true } | Self::WriteInProgress
- )
+ pub fn set_read_in_progress(&mut self) {
+ match self.operation {
+ Operation::Idle => {}
+ Operation::ReadInProgress => {
+ defmt::warn!("read in progress following read in progress");
+ }
+ Operation::WriteInProgress => {
+ self.callback();
+ }
+ }
+ self.incoming.clear();
+ defmt::trace!("switching state to ReadInProgress");
+ self.operation = Operation::ReadInProgress;
+ pac::I2C2
+ .dr()
+ .write(|w| w.set_dr(self.outgoing.pop_front().unwrap_or_default()));
+ }
+
+ pub fn set_write_in_progress(&mut self) {
+ self.incoming.clear();
+ self.outgoing.clear();
+ defmt::trace!("switching state to WriteInProgress");
+ self.operation = Operation::WriteInProgress;
+ }
+
+ pub fn callback(&mut self) {
+ self.outgoing.clear();
+ if let Some(callback) = &mut self.callback {
+ defmt::trace!("calling callback with {:?}", &self.incoming[..]);
+ callback(&self.incoming, &mut self.outgoing);
+ defmt::trace!(
+ "after callback, ready to send {} bytes",
+ self.outgoing.len()
+ );
+ } else {
+ defmt::error!("no callback installed");
+ }
+ self.incoming.clear();
}
}
@@ -82,8 +132,9 @@ impl I2C2 {
pac::I2C2.oar1().write(|w| w.set_add(addr << 1));
// Enable event interrupt for I2C2 and 2MHz frequency
pac::I2C2.cr2().write(|w| {
+ w.set_itbufen(true);
w.set_itevten(true);
- w.set_iterren(true);
+ // w.set_iterren(true);
w.set_freq(36);
});
// Enable I2C2
@@ -107,48 +158,8 @@ impl I2C2 {
}
}
- pub async fn receive(&mut self, buffer: &mut [u8]) -> Result<usize> {
- for (i, b) in buffer.iter_mut().enumerate() {
- match RX_QUEUE.receive().await {
- I2cRxEvent::Byte(v) => *b = v,
- I2cRxEvent::Stop => return Ok(i),
- }
- }
- for extra in 0..usize::MAX {
- if matches!(RX_QUEUE.receive().await, I2cRxEvent::Stop) {
- if extra > 0 {
- return Err(Error::ExtraBytes(extra));
- } else {
- return Ok(buffer.len());
- }
- }
- }
- unreachable!();
- }
-
- pub fn send(&mut self, data: &[u8]) -> Result<()> {
- defmt::trace!("Enqueuing {:?}", data);
- // Send an extra 0 byte to prevent clock stretching problems on
- // BCM2712 SOC. This will sometimes be received as 0x80.
- if TX_QUEUE.try_send(0).is_err() {
- return Err(Error::FifoFull);
- }
- // Enqueue the data to send next
- for &b in data {
- if TX_QUEUE.try_send(b).is_err() {
- return Err(Error::FifoFull);
- }
- }
- pac::I2C2.cr2().modify(|w| {
- defmt::trace!("Setting itbufen to start receiving TXE events");
- w.set_itbufen(true);
- });
- Ok(())
- }
-
- fn clear_tx_queue() {
- defmt::trace!("Emptying TX queue");
- while TX_QUEUE.try_receive().is_ok() {}
+ pub fn set_callback(callback: Callback) {
+ unsafe { STATE.callback = Some(callback) };
}
}
@@ -161,121 +172,60 @@ unsafe fn I2C2_EV() {
pac::I2C2.cr1().read().0,
pac::I2C2.cr2().read().0
);
+ if sr1.rxne() {
+ defmt::trace!("I2C2_EV RXNE");
+ let b = pac::I2C2.dr().read().0 as u8;
+ defmt::trace!("received byte {:#04x}", &b);
+ if unsafe { STATE.incoming.push(b) }.is_err() {
+ defmt::error!("I2C byte has overfilled the buffer");
+ }
+ }
+ if sr1.txe() {
+ defmt::trace!("I2C2_EV TXE");
+ let b = match unsafe { STATE.outgoing.pop_front() } {
+ Some(b) => Some(b),
+ None => {
+ if sr1.af() {
+ defmt::trace!("NACK received, stopping transmission");
+ pac::I2C2.sr1().modify(|w| w.set_af(false));
+ None
+ } else {
+ defmt::trace!("nothing to send, filling with a dummy 0 byte");
+ Some(0)
+ }
+ }
+ };
+ if let Some(b) = b {
+ pac::I2C2.dr().write(|w| w.set_dr(b));
+ }
+ }
if sr1.stopf() {
defmt::trace!(
"I2C2_EV STOPF while state is {:?}",
- defmt::Debug2Format(&State::get_state())
+ defmt::Debug2Format(unsafe { &STATE.operation })
);
// Clear stopf by writing to cr1
pac::I2C2.cr1().modify(|_w| ());
- // Opt out of the TXE/RXNE interrupt
- pac::I2C2.cr2().modify(|w| {
- w.set_itbufen(false);
- });
// Do a clearing sequence as recommended in the reference manual
// by reading and writing dr
pac::I2C2.dr().modify(|_w| ());
// If receiving, indicate the end of data
- let previous_state = State::get_state();
- if matches!(previous_state, State::WriteInProgress) {
- if RX_QUEUE.try_send(I2cRxEvent::Stop).is_err() {
- defmt::error!("Unable to send stop event to the buffer");
- }
- State::set_state(State::Idle { after_write: true });
- } else {
- State::set_state(State::Idle { after_write: false });
- }
- }
- if sr1.rxne() {
- defmt::trace!("I2C2_EV RXNE");
- let b = pac::I2C2.dr().read().0 as u8;
- defmt::trace!("Received byte {:#04x}", &b);
- if RX_QUEUE.try_send(I2cRxEvent::Byte(b)).is_err() {
- defmt::error!("I2C byte has overfilled the buffer");
+ unsafe {
+ STATE.set_idle();
}
}
if sr1.addr() {
- defmt::trace!("I2C2_EV addr");
+ defmt::trace!(
+ "I2C2_EV addr while state is {:?}",
+ defmt::Debug2Format(unsafe { &STATE.operation })
+ );
// Addr event is cleared by reading sr2
let sr2 = pac::I2C2.sr2().read();
- // If a write operation was in progress, send a simulated Stop to
- // the RX queue.
- let previous_state = State::get_state();
- if matches!(previous_state, State::WriteInProgress)
- && RX_QUEUE.try_send(I2cRxEvent::Stop).is_err()
- {
- defmt::error!("Cannot send STOP event due to repeated start to the receive queue");
- }
- let new_state = if sr2.tra() {
- let outgoing = if !previous_state.is_write() {
- // Another read transaction happened just before this one,
- // send 0 to avoid locking the bus.
- defmt::warn!("Read transaction without write, sending 0 to prevent bus locking");
- Some(0)
- } else if let Ok(b) = TX_QUEUE.try_receive() {
- // If bytes are ready to send already, send the first
- // one and enable the buffer events to receive the
- // next TXE.
- defmt::trace!("Sending already ready byte {:#04x}", b);
- Some(b)
- } else {
- // Otherwise, indicate that data is waiting and do not generate TXE events.
- defmt::trace!("No data ready, stretching clock");
- None
- };
- if let Some(b) = outgoing {
- pac::I2C2.dr().write(|w| w.set_dr(b));
- }
- State::ReadInProgress {
- waiting_for_data: outgoing.is_none(),
- }
+ if sr2.tra() {
+ unsafe { STATE.set_read_in_progress() };
} else {
- // In case of a write request, fill the transmit buffer
- // and enable the buffer events to receive only the RXNE
- // events, and not the TXE ones. Also, empty the receive
- // buffer in case it already contains something. The
- // outgoing buffer will later be cleared when the stop
- // event is received.
- pac::I2C2.dr().modify(|_| ());
- pac::I2C2.cr2().modify(|w| {
- w.set_itbufen(true);
- });
- defmt::trace!("Current state of CR1: {:#06x}", pac::I2C2.cr1().read().0);
- defmt::trace!("Current state of CR2: {:#06x}", pac::I2C2.cr2().read().0);
- // Empty a previously filled TX queue in case a read request
- // arrives right after.
- I2C2::clear_tx_queue();
- defmt::trace!("Waiting for RXNE or STOPF event");
- State::WriteInProgress
- };
- State::set_state(new_state);
- pac::I2C2.cr2().modify(|w| {
- w.set_itbufen(!matches!(
- new_state,
- State::ReadInProgress {
- waiting_for_data: true
- }
- ))
- });
- }
- if sr1.txe() && pac::I2C2.cr2().read().itbufen() {
- defmt::trace!("I2C2_EV TXE");
- if let Ok(b) = TX_QUEUE.try_receive() {
- defmt::trace!("Sending byte {:#04x}", b);
- pac::I2C2.dr().write(|w| w.set_dr(b));
- } else {
- defmt::trace!("Inhibiting TXE interrupts and noting that we are waiting");
- pac::I2C2.cr2().modify(|w| w.set_itbufen(false));
- State::set_state(State::ReadInProgress {
- waiting_for_data: true,
- });
- // defmt::trace!("Nothing to send, storing 0 in DR to avoid bus locking");
- // pac::I2C2.dr().write(|w| w.set_dr(0));
+ unsafe { STATE.set_write_in_progress() };
}
- } else if sr1.txe() && sr1.btf() {
- defmt::trace!("I2C2_EV TXE + BTF");
- defmt::warn!("Sending 0 to avoid locking the bus");
- pac::I2C2.dr().write(|w| w.set_dr(0));
}
}
@@ -284,16 +234,7 @@ unsafe fn I2C2_ER() {
let sr1 = pac::I2C2.sr1().read();
if sr1.af() {
defmt::trace!("I2C2_ER NACKF");
- pac::I2C2.sr1().modify(|w| w.set_af(false));
- // Nack are only used when transmitting, opt out of the RXNE interrupt
- pac::I2C2.cr2().modify(|w| {
- w.set_itbufen(false);
- });
- // Flush unsend bytes
- while let Ok(b) = TX_QUEUE.try_receive() {
- defmt::trace!("Discarding {:#04x}", b);
- }
- State::set_state(State::Idle { after_write: false });
+ unsafe { STATE.set_idle() };
} else {
defmt::error!(
"I2C2_ER sr1={:#06x} sr2={:#06x}",