diff options
Diffstat (limited to 'klippy/extras/adxl345.py')
| -rw-r--r-- | klippy/extras/adxl345.py | 204 |
1 files changed, 140 insertions, 64 deletions
diff --git a/klippy/extras/adxl345.py b/klippy/extras/adxl345.py index c2d64a41..7870bd4a 100644 --- a/klippy/extras/adxl345.py +++ b/klippy/extras/adxl345.py @@ -16,34 +16,44 @@ REG_MOD_READ = 0x80 REG_MOD_MULTI = 0x40 QUERY_RATES = { - 25: 0x8, 50: 0x9, 100: 0xa, 200: 0xb, 400: 0xc, - 800: 0xd, 1600: 0xe, 3200: 0xf, + 25: 0x8, + 50: 0x9, + 100: 0xA, + 200: 0xB, + 400: 0xC, + 800: 0xD, + 1600: 0xE, + 3200: 0xF, } -ADXL345_DEV_ID = 0xe5 +ADXL345_DEV_ID = 0xE5 SET_FIFO_CTL = 0x90 -FREEFALL_ACCEL = 9.80665 * 1000. -SCALE_XY = 0.003774 * FREEFALL_ACCEL # 1 / 265 (at 3.3V) mg/LSB -SCALE_Z = 0.003906 * FREEFALL_ACCEL # 1 / 256 (at 3.3V) mg/LSB +FREEFALL_ACCEL = 9.80665 * 1000.0 +SCALE_XY = 0.003774 * FREEFALL_ACCEL # 1 / 265 (at 3.3V) mg/LSB +SCALE_Z = 0.003906 * FREEFALL_ACCEL # 1 / 256 (at 3.3V) mg/LSB Accel_Measurement = collections.namedtuple( - 'Accel_Measurement', ('time', 'accel_x', 'accel_y', 'accel_z')) + "Accel_Measurement", ("time", "accel_x", "accel_y", "accel_z") +) + # Helper class to obtain measurements class AccelQueryHelper: def __init__(self, printer): self.printer = printer self.is_finished = False - print_time = printer.lookup_object('toolhead').get_last_move_time() + print_time = printer.lookup_object("toolhead").get_last_move_time() self.request_start_time = self.request_end_time = print_time self.msgs = [] self.samples = [] + def finish_measurements(self): - toolhead = self.printer.lookup_object('toolhead') + toolhead = self.printer.lookup_object("toolhead") self.request_end_time = toolhead.get_last_move_time() toolhead.wait_moves() self.is_finished = True + def handle_batch(self, msg): if self.is_finished: return False @@ -52,13 +62,16 @@ class AccelQueryHelper: return False self.msgs.append(msg) return True + def has_valid_samples(self): for msg in self.msgs: - data = msg['data'] + data = msg["data"] first_sample_time = data[0][0] last_sample_time = data[-1][0] - if (first_sample_time > self.request_end_time - or last_sample_time < self.request_start_time): + if ( + first_sample_time > self.request_end_time + or last_sample_time < self.request_start_time + ): continue # The time intervals [first_sample_time, last_sample_time] # and [request_start_time, request_end_time] have non-zero @@ -69,14 +82,15 @@ class AccelQueryHelper: # is at least 1 second, so this possibility is negligible. return True return False + def get_samples(self): if not self.msgs: return self.samples - total = sum([len(m['data']) for m in self.msgs]) + total = sum([len(m["data"]) for m in self.msgs]) count = 0 self.samples = samples = [None] * total for msg in self.msgs: - for samp_time, x, y, z in msg['data']: + for samp_time, x, y, z in msg["data"]: if samp_time < self.request_start_time: continue if samp_time > self.request_end_time: @@ -85,6 +99,7 @@ class AccelQueryHelper: count += 1 del samples[count:] return self.samples + def write_to_file(self, filename): def write_impl(): try: @@ -96,13 +111,14 @@ class AccelQueryHelper: f.write("#time,accel_x,accel_y,accel_z\n") samples = self.samples or self.get_samples() for t, accel_x, accel_y, accel_z in samples: - f.write("%.6f,%.6f,%.6f,%.6f\n" % ( - t, accel_x, accel_y, accel_z)) + f.write("%.6f,%.6f,%.6f,%.6f\n" % (t, accel_x, accel_y, accel_z)) f.close() + write_proc = multiprocessing.Process(target=write_impl) write_proc.daemon = True write_proc.start() + # Helper class for G-Code commands class AccelCommandHelper: def __init__(self, config, chip): @@ -116,22 +132,41 @@ class AccelCommandHelper: if len(name_parts) == 1: if self.name == "adxl345" or not config.has_section("adxl345"): self.register_commands(None) + def register_commands(self, name): # Register commands - gcode = self.printer.lookup_object('gcode') - gcode.register_mux_command("ACCELEROMETER_MEASURE", "CHIP", name, - self.cmd_ACCELEROMETER_MEASURE, - desc=self.cmd_ACCELEROMETER_MEASURE_help) - gcode.register_mux_command("ACCELEROMETER_QUERY", "CHIP", name, - self.cmd_ACCELEROMETER_QUERY, - desc=self.cmd_ACCELEROMETER_QUERY_help) - gcode.register_mux_command("ACCELEROMETER_DEBUG_READ", "CHIP", name, - self.cmd_ACCELEROMETER_DEBUG_READ, - desc=self.cmd_ACCELEROMETER_DEBUG_READ_help) - gcode.register_mux_command("ACCELEROMETER_DEBUG_WRITE", "CHIP", name, - self.cmd_ACCELEROMETER_DEBUG_WRITE, - desc=self.cmd_ACCELEROMETER_DEBUG_WRITE_help) + gcode = self.printer.lookup_object("gcode") + gcode.register_mux_command( + "ACCELEROMETER_MEASURE", + "CHIP", + name, + self.cmd_ACCELEROMETER_MEASURE, + desc=self.cmd_ACCELEROMETER_MEASURE_help, + ) + gcode.register_mux_command( + "ACCELEROMETER_QUERY", + "CHIP", + name, + self.cmd_ACCELEROMETER_QUERY, + desc=self.cmd_ACCELEROMETER_QUERY_help, + ) + gcode.register_mux_command( + "ACCELEROMETER_DEBUG_READ", + "CHIP", + name, + self.cmd_ACCELEROMETER_DEBUG_READ, + desc=self.cmd_ACCELEROMETER_DEBUG_READ_help, + ) + gcode.register_mux_command( + "ACCELEROMETER_DEBUG_WRITE", + "CHIP", + name, + self.cmd_ACCELEROMETER_DEBUG_WRITE, + desc=self.cmd_ACCELEROMETER_DEBUG_WRITE_help, + ) + cmd_ACCELEROMETER_MEASURE_help = "Start/stop accelerometer" + def cmd_ACCELEROMETER_MEASURE(self, gcmd): if self.bg_client is None: # Start measurements @@ -140,7 +175,7 @@ class AccelCommandHelper: return # End measurements name = gcmd.get("NAME", time.strftime("%Y%m%d_%H%M%S")) - if not name.replace('-', '').replace('_', '').isalnum(): + if not name.replace("-", "").replace("_", "").isalnum(): raise gcmd.error("Invalid NAME parameter") bg_client = self.bg_client self.bg_client = None @@ -151,48 +186,64 @@ class AccelCommandHelper: else: filename = "/tmp/%s-%s-%s.csv" % (self.base_name, self.name, name) bg_client.write_to_file(filename) - gcmd.respond_info("Writing raw accelerometer data to %s file" - % (filename,)) + gcmd.respond_info("Writing raw accelerometer data to %s file" % (filename,)) + cmd_ACCELEROMETER_QUERY_help = "Query accelerometer for the current values" + def cmd_ACCELEROMETER_QUERY(self, gcmd): aclient = self.chip.start_internal_client() - self.printer.lookup_object('toolhead').dwell(1.) + self.printer.lookup_object("toolhead").dwell(1.0) aclient.finish_measurements() values = aclient.get_samples() if not values: raise gcmd.error("No accelerometer measurements found") _, accel_x, accel_y, accel_z = values[-1] - gcmd.respond_info("accelerometer values (x, y, z): %.6f, %.6f, %.6f" - % (accel_x, accel_y, accel_z)) + gcmd.respond_info( + "accelerometer values (x, y, z): %.6f, %.6f, %.6f" + % (accel_x, accel_y, accel_z) + ) + cmd_ACCELEROMETER_DEBUG_READ_help = "Query register (for debugging)" + def cmd_ACCELEROMETER_DEBUG_READ(self, gcmd): reg = gcmd.get("REG", minval=0, maxval=127, parser=lambda x: int(x, 0)) val = self.chip.read_reg(reg) gcmd.respond_info("Accelerometer REG[0x%x] = 0x%x" % (reg, val)) + cmd_ACCELEROMETER_DEBUG_WRITE_help = "Set register (for debugging)" + def cmd_ACCELEROMETER_DEBUG_WRITE(self, gcmd): reg = gcmd.get("REG", minval=0, maxval=127, parser=lambda x: int(x, 0)) val = gcmd.get("VAL", minval=0, maxval=255, parser=lambda x: int(x, 0)) self.chip.set_reg(reg, val) + # Helper to read the axes_map parameter from the config def read_axes_map(config, scale_x, scale_y, scale_z): - am = {'x': (0, scale_x), 'y': (1, scale_y), 'z': (2, scale_z), - '-x': (0, -scale_x), '-y': (1, -scale_y), '-z': (2, -scale_z)} - axes_map = config.getlist('axes_map', ('x','y','z'), count=3) + am = { + "x": (0, scale_x), + "y": (1, scale_y), + "z": (2, scale_z), + "-x": (0, -scale_x), + "-y": (1, -scale_y), + "-z": (2, -scale_z), + } + axes_map = config.getlist("axes_map", ("x", "y", "z"), count=3) if any([a not in am for a in axes_map]): raise config.error("Invalid axes_map parameter") return [am[a.strip()] for a in axes_map] + BATCH_UPDATES = 0.100 + # Printer class that controls ADXL345 chip class ADXL345: def __init__(self, config): self.printer = config.get_printer() AccelCommandHelper(config, self) self.axes_map = read_axes_map(config, SCALE_XY, SCALE_XY, SCALE_Z) - self.data_rate = config.getint('rate', 3200) + self.data_rate = config.getint("rate", 3200) if self.data_rate not in QUERY_RATES: raise config.error("Invalid rate parameter: %d" % (self.data_rate,)) # Setup mcu sensor_adxl345 bulk query code @@ -200,10 +251,12 @@ class ADXL345: self.mcu = mcu = self.spi.get_mcu() self.oid = oid = mcu.create_oid() self.query_adxl345_cmd = None - mcu.add_config_cmd("config_adxl345 oid=%d spi_oid=%d" - % (oid, self.spi.get_oid())) - mcu.add_config_cmd("query_adxl345 oid=%d rest_ticks=0" - % (oid,), on_restart=True) + mcu.add_config_cmd( + "config_adxl345 oid=%d spi_oid=%d" % (oid, self.spi.get_oid()) + ) + mcu.add_config_cmd( + "query_adxl345 oid=%d rest_ticks=0" % (oid,), on_restart=True + ) mcu.register_config_callback(self._build_config) # Bulk sample message reading chip_smooth = self.data_rate * BATCH_UPDATES * 2 @@ -211,35 +264,48 @@ class ADXL345: self.last_error_count = 0 # Process messages in batches self.batch_bulk = bulk_sensor.BatchBulkHelper( - self.printer, self._process_batch, - self._start_measurements, self._finish_measurements, BATCH_UPDATES) + self.printer, + self._process_batch, + self._start_measurements, + self._finish_measurements, + BATCH_UPDATES, + ) self.name = config.get_name().split()[-1] - hdr = ('time', 'x_acceleration', 'y_acceleration', 'z_acceleration') - self.batch_bulk.add_mux_endpoint("adxl345/dump_adxl345", "sensor", - self.name, {'header': hdr}) + hdr = ("time", "x_acceleration", "y_acceleration", "z_acceleration") + self.batch_bulk.add_mux_endpoint( + "adxl345/dump_adxl345", "sensor", self.name, {"header": hdr} + ) + def _build_config(self): cmdqueue = self.spi.get_command_queue() self.query_adxl345_cmd = self.mcu.lookup_command( - "query_adxl345 oid=%c rest_ticks=%u", cq=cmdqueue) - self.ffreader.setup_query_command("query_adxl345_status oid=%c", - oid=self.oid, cq=cmdqueue) + "query_adxl345 oid=%c rest_ticks=%u", cq=cmdqueue + ) + self.ffreader.setup_query_command( + "query_adxl345_status oid=%c", oid=self.oid, cq=cmdqueue + ) + def read_reg(self, reg): params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00]) - response = bytearray(params['response']) + response = bytearray(params["response"]) return response[1] + def set_reg(self, reg, val, minclock=0): 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)) + "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 start_internal_client(self): aqh = AccelQueryHelper(self.printer) self.batch_bulk.add_client(aqh.handle_batch) return aqh + # Measurement decoding def _convert_samples(self, samples): (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map @@ -248,10 +314,11 @@ class ADXL345: if yzhigh & 0x80: self.last_error_count += 1 continue - rx = (xlow | ((xzhigh & 0x1f) << 8)) - ((xzhigh & 0x10) << 9) - ry = (ylow | ((yzhigh & 0x1f) << 8)) - ((yzhigh & 0x10) << 9) - rz = ((zlow | ((xzhigh & 0xe0) << 3) | ((yzhigh & 0xe0) << 6)) - - ((yzhigh & 0x40) << 7)) + rx = (xlow | ((xzhigh & 0x1F) << 8)) - ((xzhigh & 0x10) << 9) + ry = (ylow | ((yzhigh & 0x1F) << 8)) - ((yzhigh & 0x10) << 9) + rz = (zlow | ((xzhigh & 0xE0) << 3) | ((yzhigh & 0xE0) << 6)) - ( + (yzhigh & 0x40) << 7 + ) raw_xyz = (rx, ry, rz) x = round(raw_xyz[x_pos] * x_scale, 6) y = round(raw_xyz[y_pos] * y_scale, 6) @@ -259,6 +326,7 @@ class ADXL345: samples[count] = (round(ptime, 6), x, y, z) count += 1 del samples[count:] + # Start, stop, and process message batches def _start_measurements(self): # In case of miswiring, testing ADXL345 device ID prevents treating @@ -269,7 +337,8 @@ class ADXL345: "Invalid adxl345 id (got %x vs %x).\n" "This is generally indicative of connection problems\n" "(e.g. faulty wiring) or a faulty adxl345 chip." - % (dev_id, ADXL345_DEV_ID)) + % (dev_id, ADXL345_DEV_ID) + ) # Setup chip in requested query rate self.set_reg(REG_POWER_CTL, 0x00) self.set_reg(REG_DATA_FORMAT, 0x0B) @@ -277,29 +346,36 @@ class ADXL345: self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate]) self.set_reg(REG_FIFO_CTL, SET_FIFO_CTL) # Start bulk reading - rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate) + rest_ticks = self.mcu.seconds_to_clock(4.0 / self.data_rate) self.query_adxl345_cmd.send([self.oid, rest_ticks]) self.set_reg(REG_POWER_CTL, 0x08) logging.info("ADXL345 starting '%s' measurements", self.name) # Initialize clock tracking self.ffreader.note_start() self.last_error_count = 0 + def _finish_measurements(self): # Halt bulk reading self.set_reg(REG_POWER_CTL, 0x00) self.query_adxl345_cmd.send_wait_ack([self.oid, 0]) self.ffreader.note_end() logging.info("ADXL345 finished '%s' measurements", self.name) + def _process_batch(self, eventtime): samples = self.ffreader.pull_samples() self._convert_samples(samples) if not samples: return {} - return {'data': samples, 'errors': self.last_error_count, - 'overflows': self.ffreader.get_last_overflows()} + return { + "data": samples, + "errors": self.last_error_count, + "overflows": self.ffreader.get_last_overflows(), + } + def load_config(config): return ADXL345(config) + def load_config_prefix(config): return ADXL345(config) |