Hack in more I2C retries

src/klipper_sgp40/__init__.py

@@ -8,9 +8,11 @@
 import logging
 import math
 import re
+from logging import ERROR, WARNING
 from struct import unpack_from
 
-from .. import bus  # type:ignore
+from ...serialhdl import error  # type: ignore
+from .. import bus  # type: ignore
 from .gia import GasIndexAlgorithm
 
 SGP40_CHIP_ADDR = 0x59
@@ -21,6 +23,27 @@
 MEASURE_RAW_CMD_PREFIX = [0x26, 0x0F]
 
 
+# Hack i2c to attempt more retries.
+def _get_response(self, cmds, cmd_queue, minclock=0, reqclock=0):
+    retries = 15
+    retry_delay = 0.010
+    while 1:
+        for cmd in cmds[:-1]:
+            self.serial.raw_send(cmd, minclock, reqclock, cmd_queue)
+        self.serial.raw_send_wait_ack(cmds[-1], minclock, reqclock, cmd_queue)
+        params = self.last_params
+        if params is not None:
+            self.serial.register_response(None, self.name, self.oid)
+            return params
+        if retries <= 0:
+            self.serial.register_response(None, self.name, self.oid)
+            raise error("Unable to obtain '%s' response" % (self.name,))
+        reactor = self.serial.reactor
+        reactor.pause(reactor.monotonic() + retry_delay)
+        retries -= 1
+        retry_delay *= 2.0
+
+
 def _generate_crc(data):
     # From SGP40 data sheet
     crc = 0xFF
@@ -183,6 +206,8 @@ def _handle_connect(self):
 
         self._init_sgp40()
 
+        self.i2c.i2c_read_cmd._xmit_helper.get_response = _get_response
+
         self.reactor.update_timer(self.step_timer, self.reactor.NOW)
 
     def _handle_ready(self):
@@ -213,7 +238,7 @@ def _init_sgp40(self):
         self._wait_ms(500)
         response = self._read()
         if response[0] != 0xD400:
-            logging.error(self._log_message("Self test error"))
+            self._log(ERROR, "Self test error")
 
         self.step_timer = self.reactor.register_timer(self._handle_step)
 
@@ -286,13 +311,13 @@ def _read(self, len=1):
             if not _check_crc8(
                 response[i : i + SGP40_WORD_LEN], response[i + SGP40_WORD_LEN]
             ):
-                logging.warning(self._log_message("Checksum error on read!"))
+                self._log(WARNING, "Checksum error on read!")
             data.append(unpack_from(">H", response[i : i + SGP40_WORD_LEN])[0])
 
         return data
 
-    def _log_message(self, message):
-        return "SGP40 %s: %s" % (self.name, message)
+    def _log(self, level, msg):
+        logging.log(level, "SGP40 %s: %s" % (self.name, msg))
 
     def get_status(self, eventtime):
         return {