adxl345: Verify the register writes, added debug adxl345 commands

Signed-off-by: Dmitry Butyugin <dmbutyugin@google.com>

docs/G-Codes.md

@@ -717,6 +717,13 @@ The following commands are available when an
   is used. This command is useful to test the connection to the
   ADXL345 accelerometer: one of the returned values should be a
   free-fall acceleration (+/- some noise of the chip).
+- `ADXL345_DEBUG_READ [CHIP=<config_name>] REG=<register>`: queries
+  ADXL345 register <register> (e.g. 44 or 0x2C). Can be useful for
+  debugging purposes.
+- `ADXL345_DEBUG_WRITE [CHIP=<config_name>] REG=<reg> VAL=<value>`:
+  writes raw <value> into a register <register>. Both <value> and
+  <register> can be a decimal or a hexadecimal integer. Use with care,
+  and refer to ADXL345 data sheet for the reference.
 
 ## Resonance Testing Commands
 

klippy/extras/adxl345.py

@@ -20,7 +20,10 @@ QUERY_RATES = {
     800: 0xd, 1600: 0xe, 3200: 0xf,
 }
 
-SCALE = 0.004 * 9.80665 * 1000. # 4mg/LSB * Earth gravity in mm/s**2
+ADXL345_DEV_ID = 0xe5
+
+FREEFALL_ACCEL = 9.80665 * 1000.
+SCALE = 0.0039 * FREEFALL_ACCEL # 3.9mg/LSB * Earth gravity in mm/s**2
 
 Accel_Measurement = collections.namedtuple(
     'Accel_Measurement', ('time', 'accel_x', 'accel_y', 'accel_z'))
@@ -93,7 +96,7 @@ class ADXL345Results:
         write_proc.daemon = True
         write_proc.start()
 
-# Printer class that controls measurments
+# Printer class that controls ADXL345 chip
 class ADXL345:
     def __init__(self, config):
         self.printer = config.get_printer()
@@ -135,11 +138,36 @@ class ADXL345:
         gcode.register_mux_command("ACCELEROMETER_QUERY", "CHIP", self.name,
                                    self.cmd_ACCELEROMETER_QUERY,
                                    desc=self.cmd_ACCELEROMETER_QUERY_help)
+        gcode.register_mux_command("ADXL345_DEBUG_READ", "CHIP", self.name,
+                                   self.cmd_ADXL345_DEBUG_READ,
+                                   desc=self.cmd_ADXL345_DEBUG_READ_help)
+        gcode.register_mux_command("ADXL345_DEBUG_WRITE", "CHIP", self.name,
+                                   self.cmd_ADXL345_DEBUG_WRITE,
+                                   desc=self.cmd_ADXL345_DEBUG_WRITE_help)
         if self.name == "default":
             gcode.register_mux_command("ACCELEROMETER_MEASURE", "CHIP", None,
                                        self.cmd_ACCELEROMETER_MEASURE)
             gcode.register_mux_command("ACCELEROMETER_QUERY", "CHIP", None,
                                        self.cmd_ACCELEROMETER_QUERY)
+            gcode.register_mux_command("ADXL345_DEBUG_READ", "CHIP", None,
+                                       self.cmd_ADXL345_DEBUG_READ,
+                                       desc=self.cmd_ADXL345_DEBUG_READ_help)
+            gcode.register_mux_command("ADXL345_DEBUG_WRITE", "CHIP", None,
+                                       self.cmd_ADXL345_DEBUG_WRITE,
+                                       desc=self.cmd_ADXL345_DEBUG_WRITE_help)
+    def is_initialized(self):
+        # In case of miswiring, testing ADXL345 device ID prevents treating
+        # noise or wrong signal as a correctly initialized device
+        return (self.read_reg(REG_DEVID) == ADXL345_DEV_ID and
+                (self.read_reg(REG_DATA_FORMAT) & 0xB) != 0)
+    def initialize(self):
+        # Setup ADXL345 parameters and verify chip connectivity
+        self.set_reg(REG_POWER_CTL, 0x00)
+        dev_id = self.read_reg(REG_DEVID)
+        if dev_id != ADXL345_DEV_ID:
+            raise self.printer.command_error("Invalid adxl345 id (got %x vs %x)"
+                                             % (dev_id, ADXL345_DEV_ID))
+        self.set_reg(REG_DATA_FORMAT, 0x0B)
     def _build_config(self):
         self.query_adxl345_cmd = self.mcu.lookup_command(
             "query_adxl345 oid=%c clock=%u rest_ticks=%u",
@@ -170,23 +198,35 @@ class ADXL345:
         if sequence < self.last_sequence:
             sequence += 0x10000
         return sequence
-    def start_measurements(self, rate=None):
+    def read_reg(self, reg):
-        rate = rate or self.data_rate
+        params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00])
-        # Verify chip connectivity
-        params = self.spi.spi_transfer([REG_DEVID | REG_MOD_READ, 0x00])
         response = bytearray(params['response'])
-        if response[1] != 0xe5:
+        return response[1]
-            raise self.printer.command_error("Invalid adxl345 id (got %x vs %x)"
+    def set_reg(self, reg, val, minclock=0):
-                                             % (response[1], 0xe5))
+        self.spi.spi_send([reg, val & 0xFF], minclock=minclock)
+        stored_val = self.read_reg(reg)
+        if stored_val != val:
+            raise self.printer.command_error(
+                    "Failed to set ADXL345 register [0x%x] to 0x%x: got 0x%x. "
+                    "This is generally indicative of connection problems "
+                    "(e.g. faulty wiring) or a faulty adxl345 chip." % (
+                        reg, val, stored_val))
+    def is_measuring(self):
+        return self.query_rate > 0
+    def start_measurements(self, rate=None):
+        if self.is_measuring():
+            return
+        rate = rate or self.data_rate
+        if not self.is_initialized():
+            self.initialize()
         # Setup chip in requested query rate
         clock = 0
         if self.last_tx_time:
             clock = self.mcu.print_time_to_clock(self.last_tx_time)
-        self.spi.spi_send([REG_POWER_CTL, 0x00], minclock=clock)
+        self.set_reg(REG_POWER_CTL, 0x00, minclock=clock)
-        self.spi.spi_send([REG_FIFO_CTL, 0x00])
+        self.set_reg(REG_FIFO_CTL, 0x00)
-        self.spi.spi_send([REG_DATA_FORMAT, 0x0B])
+        self.set_reg(REG_BW_RATE, QUERY_RATES[rate])
-        self.spi.spi_send([REG_BW_RATE, QUERY_RATES[rate]])
+        self.set_reg(REG_FIFO_CTL, 0x80)
-        self.spi.spi_send([REG_FIFO_CTL, 0x80])
         # Setup samples
         print_time = self.printer.lookup_object('toolhead').get_last_move_time()
         self.raw_samples = []
@@ -200,8 +240,7 @@ class ADXL345:
         self.query_adxl345_cmd.send([self.oid, reqclock, rest_ticks],
                                     reqclock=reqclock)
     def finish_measurements(self):
-        query_rate = self.query_rate
+        if not self.is_measuring():
-        if not query_rate:
             return ADXL345Results()
         # Halt bulk reading
         print_time = self.printer.lookup_object('toolhead').get_last_move_time()
@@ -224,8 +263,8 @@ class ADXL345:
         logging.info("ADXL345 finished %d measurements: %s",
                      res.total_count, res.get_stats())
         return res
-    def end_query(self, name):
+    def end_query(self, name, gcmd):
-        if not self.query_rate:
+        if not self.is_measuring():
             return
         res = self.finish_measurements()
         # Write data to file
@@ -234,13 +273,15 @@ class ADXL345:
         else:
             filename = "/tmp/adxl345-%s-%s.csv" % (self.name, name,)
         res.write_to_file(filename)
+        gcmd.respond_info(
+                "Writing raw accelerometer data to %s file" % (filename,))
     cmd_ACCELEROMETER_MEASURE_help = "Start/stop accelerometer"
     def cmd_ACCELEROMETER_MEASURE(self, gcmd):
-        if self.query_rate:
+        if self.is_measuring():
             name = gcmd.get("NAME", time.strftime("%Y%m%d_%H%M%S"))
             if not name.replace('-', '').replace('_', '').isalnum():
                 raise gcmd.error("Invalid adxl345 NAME parameter")
-            self.end_query(name)
+            self.end_query(name, gcmd)
             gcmd.respond_info("adxl345 measurements stopped")
         else:
             rate = gcmd.get_int("RATE", self.data_rate)
@@ -250,7 +291,7 @@ class ADXL345:
             gcmd.respond_info("adxl345 measurements started")
     cmd_ACCELEROMETER_QUERY_help = "Query accelerometer for the current values"
     def cmd_ACCELEROMETER_QUERY(self, gcmd):
-        if self.query_rate:
+        if self.is_measuring():
             raise gcmd.error("adxl345 measurements in progress")
         self.start_measurements()
         reactor = self.printer.get_reactor()
@@ -266,6 +307,20 @@ class ADXL345:
         _, accel_x, accel_y, accel_z = values[-1]
         gcmd.respond_info("adxl345 values (x, y, z): %.6f, %.6f, %.6f" % (
             accel_x, accel_y, accel_z))
+    cmd_ADXL345_DEBUG_READ_help = "Query accelerometer register (for debugging)"
+    def cmd_ADXL345_DEBUG_READ(self, gcmd):
+        if self.is_measuring():
+            raise gcmd.error("adxl345 measurements in progress")
+        reg = gcmd.get("REG", minval=29, maxval=57, parser=lambda x: int(x, 0))
+        val = self.read_reg(reg)
+        gcmd.respond_info("ADXL345 REG[0x%x] = 0x%x" % (reg, val))
+    cmd_ADXL345_DEBUG_WRITE_help = "Set accelerometer register (for debugging)"
+    def cmd_ADXL345_DEBUG_WRITE(self, gcmd):
+        if self.is_measuring():
+            raise gcmd.error("adxl345 measurements in progress")
+        reg = gcmd.get("REG", minval=29, maxval=57, parser=lambda x: int(x, 0))
+        val = gcmd.get("VAL", minval=0, maxval=255, parser=lambda x: int(x, 0))
+        self.set_reg(reg, val)
 
 def load_config(config):
     return ADXL345(config)