Use extra byte to avoid bogus SOC clock-stretching problem

controller/python/controller.py

+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:
 
     I2C_BUS = 1
     I2C_ADDR = 0x57
 
-    WHO_AM_I = 0x0f
+    WHO_AM_I = 0x0F
     MAX_MOTOR_PERCENTAGE = 0x11
     MOTOR_SPEED = 0x30
     ENCODER_TICKS = 0x32
 
     def __init__(self):
-        self.i2c = smbus.SMBus(1)
+        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)
 
     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.i2c.read_i2c_block_data(self.I2C_ADDR, self.WHO_AM_I, 1)[0]
+        return self._read_i2c_block_data(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.i2c.write_i2c_block_data(self.I2C_ADDR, self.MAX_MOTOR_PERCENTAGE, [percent])
-        
+        self._write_i2c_block_data(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 an integer between -127 and 127 or None")
+                raise ValueError(f"{arg} must be [-127..127] or None")
             return v
-        self.i2c.write_i2c_block_data(self.I2C_ADDR, self.MOTOR_SPEED,
-                                      list(struct.pack('bb', convert(left, "left"), convert(right, "right"))))
+
+        self._write_i2c_block_data(
+            self.MOTOR_SPEED,
+            list(struct.pack("bb", convert(left, "left"), convert(right, "right"))),
+        )
 
     def set_left_motor_speed(self, speed: int):
         """Set the left motor speed between -127 and 127."""
@@ -50,9 +74,14 @@ class Controller:
     def standby(self):
         """Stop the motors by putting the controller board in standby mode."""
         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."""
-        return struct.unpack('hh', bytes(self.i2c.read_i2c_block_data(self.I2C_ADDR, self.ENCODER_TICKS, 4)))
+        """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))
+        )

i2c2-slave/src/lib.rs

@@ -16,7 +16,7 @@ enum I2cRxEvent {
 }
 
 static RX_QUEUE: Channel<CriticalSectionRawMutex, I2cRxEvent, 9> = Channel::new();
-static TX_QUEUE: Channel<CriticalSectionRawMutex, u8, 8> = Channel::new();
+static TX_QUEUE: Channel<CriticalSectionRawMutex, u8, 9> = Channel::new();
 static mut STATE: State = State::Idle { after_write: false };
 
 #[derive(Debug)]
@@ -128,6 +128,11 @@ impl I2C2 {
 
     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() {