From 9f28b6addf80f2dbf51345aa1eb74822c58bc86c Mon Sep 17 00:00:00 2001
From: Samuel Tardieu <sam@rfc1149.net>
Date: Tue, 9 Jul 2024 22:24:44 +0200
Subject: [PATCH] Use extra byte to avoid bogus SOC clock-stretching problem
---
controller/python/controller.py | 63 ++++++++++++++++++++++++---------
i2c2-slave/src/lib.rs | 7 +++-
2 files changed, 52 insertions(+), 18 deletions(-)
diff --git a/controller/python/controller.py b/controller/python/controller.py
index baa0fd5..1d55bea 100644
--- a/controller/python/controller.py
+++ b/controller/python/controller.py
@@ -1,43 +1,67 @@
+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))
+ )
diff --git a/i2c2-slave/src/lib.rs b/i2c2-slave/src/lib.rs
index 2b280b4..3b19b53 100644
--- a/i2c2-slave/src/lib.rs
+++ b/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() {
--
GitLab