2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								# Support for reading acceleration data from an adxl345 chip
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								#
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 13:01:34 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								# Copyright (C) 2020-2023  Kevin O'Connor <kevin@koconnor.net>
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								#
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								# This file may be distributed under the terms of the GNU GPLv3 license.
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 11:30:51 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								import logging, time, collections, multiprocessing, os
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 14:07:06 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								from . import bus, bulk_sensor
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								# ADXL345 registers
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								REG_DEVID = 0x00
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								REG_BW_RATE = 0x2C
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								REG_POWER_CTL = 0x2D
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								REG_DATA_FORMAT = 0x31
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								REG_FIFO_CTL = 0x38
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								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,
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								}
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							
								
									
										
										
										
											2021-03-26 19:32:32 +01:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								ADXL345_DEV_ID = 0xe5
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-22 16:24:16 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								SET_FIFO_CTL = 0x90
							 | 
						
					
						
							
								
									
										
										
										
											2021-03-26 19:32:32 +01:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								FREEFALL_ACCEL = 9.80665 * 1000.
							 | 
						
					
						
							
								
									
										
										
										
											2022-10-17 19:28:41 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								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
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								Accel_Measurement = collections.namedtuple(
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								    'Accel_Measurement', ('time', 'accel_x', 'accel_y', 'accel_z'))
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 23:09:49 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								# Helper class to obtain measurements
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:36:14 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								class AccelQueryHelper:
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 00:15:55 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def __init__(self, printer):
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 23:09:49 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.printer = printer
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 00:15:55 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.is_finished = False
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        print_time = printer.lookup_object('toolhead').get_last_move_time()
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.request_start_time = self.request_end_time = print_time
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 00:15:55 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.msgs = []
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.samples = []
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 23:09:49 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def finish_measurements(self):
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        toolhead = self.printer.lookup_object('toolhead')
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.request_end_time = toolhead.get_last_move_time()
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        toolhead.wait_moves()
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 00:15:55 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.is_finished = True
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								    def handle_batch(self, msg):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        if self.is_finished:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            return False
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        if len(self.msgs) >= 10000:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            # Avoid filling up memory with too many samples
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            return False
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.msgs.append(msg)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        return True
							 | 
						
					
						
							
								
									
										
										
										
											2021-09-26 01:54:50 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def has_valid_samples(self):
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 00:15:55 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        for msg in self.msgs:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            data = msg['data']
							 | 
						
					
						
							
								
									
										
										
										
											2021-09-26 01:54:50 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            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):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                continue
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            # The time intervals [first_sample_time, last_sample_time]
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            # and [request_start_time, request_end_time] have non-zero
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            # intersection. It is still theoretically possible that none
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 00:15:55 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            # of the samples from msgs fall into the time interval
							 | 
						
					
						
							
								
									
										
										
										
											2021-09-26 01:54:50 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            # [request_start_time, request_end_time] if it is too narrow
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            # or on very heavy data losses. In practice, that interval
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            # is at least 1 second, so this possibility is negligible.
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            return True
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        return False
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								    def get_samples(self):
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 00:15:55 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        if not self.msgs:
							 | 
						
					
						
							
								
									
										
										
										
											2020-10-15 02:08:10 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            return self.samples
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 00:15:55 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        total = sum([len(m['data']) for m in self.msgs])
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        count = 0
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.samples = samples = [None] * total
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 00:15:55 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        for msg in self.msgs:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            for samp_time, x, y, z in msg['data']:
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								                if samp_time < self.request_start_time:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                    continue
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                if samp_time > self.request_end_time:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                    break
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                samples[count] = Accel_Measurement(samp_time, x, y, z)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                count += 1
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        del samples[count:]
							 | 
						
					
						
							
								
									
										
										
										
											2020-10-15 02:08:10 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        return self.samples
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								    def write_to_file(self, filename):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        def write_impl():
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            try:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                # Try to re-nice writing process
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                os.nice(20)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            except:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                pass
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            f = open(filename, "w")
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            f.write("#time,accel_x,accel_y,accel_z\n")
							 | 
						
					
						
							
								
									
										
										
										
											2021-09-26 01:54:50 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            samples = self.samples or self.get_samples()
							 | 
						
					
						
							
								
									
										
										
										
											2020-10-15 02:08:10 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            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.close()
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        write_proc = multiprocessing.Process(target=write_impl)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        write_proc.daemon = True
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        write_proc.start()
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								# Helper class for G-Code commands
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								class AccelCommandHelper:
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def __init__(self, config, chip):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.printer = config.get_printer()
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.chip = chip
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 23:09:49 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.bg_client = None
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        name_parts = config.get_name().split()
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.base_name = name_parts[0]
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.name = name_parts[-1]
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.register_commands(self.name)
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        if len(name_parts) == 1:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            if self.name == "adxl345" or not config.has_section("adxl345"):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                self.register_commands(None)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    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)
							 | 
						
					
						
							
								
									
										
										
										
											2021-09-26 01:05:30 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        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)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    cmd_ACCELEROMETER_MEASURE_help = "Start/stop accelerometer"
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								    def cmd_ACCELEROMETER_MEASURE(self, gcmd):
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-23 14:05:07 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        if self.bg_client is None:
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            # Start measurements
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 23:09:49 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            self.bg_client = self.chip.start_internal_client()
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            gcmd.respond_info("accelerometer measurements started")
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            return
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        # End measurements
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        name = gcmd.get("NAME", time.strftime("%Y%m%d_%H%M%S"))
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        if not name.replace('-', '').replace('_', '').isalnum():
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            raise gcmd.error("Invalid NAME parameter")
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 23:09:49 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        bg_client = self.bg_client
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.bg_client = None
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        bg_client.finish_measurements()
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        # Write data to file
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        if self.base_name == self.name:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            filename = "/tmp/%s-%s.csv" % (self.base_name, name)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        else:
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            filename = "/tmp/%s-%s-%s.csv" % (self.base_name, self.name, name)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 23:09:49 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        bg_client.write_to_file(filename)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        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):
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 23:09:49 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        aclient = self.chip.start_internal_client()
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.printer.lookup_object('toolhead').dwell(1.)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 23:09:49 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        aclient.finish_measurements()
							 | 
						
					
						
							
								
									
										
										
										
											2021-09-26 01:54:50 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        values = aclient.get_samples()
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        if not values:
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            raise gcmd.error("No accelerometer measurements found")
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        _, accel_x, accel_y, accel_z = values[-1]
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        gcmd.respond_info("accelerometer values (x, y, z): %.6f, %.6f, %.6f"
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								                          % (accel_x, accel_y, accel_z))
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    cmd_ACCELEROMETER_DEBUG_READ_help = "Query register (for debugging)"
							 | 
						
					
						
							
								
									
										
										
										
											2021-09-26 01:05:30 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def cmd_ACCELEROMETER_DEBUG_READ(self, gcmd):
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        reg = gcmd.get("REG", minval=0, maxval=126, parser=lambda x: int(x, 0))
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        val = self.chip.read_reg(reg)
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        gcmd.respond_info("Accelerometer REG[0x%x] = 0x%x" % (reg, val))
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								    cmd_ACCELEROMETER_DEBUG_WRITE_help = "Set register (for debugging)"
							 | 
						
					
						
							
								
									
										
										
										
											2021-09-26 01:05:30 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def cmd_ACCELEROMETER_DEBUG_WRITE(self, gcmd):
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        reg = gcmd.get("REG", minval=0, maxval=126, parser=lambda x: int(x, 0))
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        val = gcmd.get("VAL", minval=0, maxval=255, parser=lambda x: int(x, 0))
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.chip.set_reg(reg, val)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 02:10:43 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								# Helper to read the axes_map parameter from the config
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								def read_axes_map(config):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								    am = {'x': (0, SCALE_XY), 'y': (1, SCALE_XY), 'z': (2, SCALE_Z),
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								          '-x': (0, -SCALE_XY), '-y': (1, -SCALE_XY), '-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]
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								MIN_MSG_TIME = 0.100
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-22 16:24:16 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								BYTES_PER_SAMPLE = 5
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								SAMPLES_PER_BLOCK = 10
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 23:26:42 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								BATCH_UPDATES = 0.100
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 11:15:23 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							
								
									
										
										
										
											2021-03-26 19:32:32 +01:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								# Printer class that controls ADXL345 chip
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								class ADXL345:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								    def __init__(self, config):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.printer = config.get_printer()
							 | 
						
					
						
							
								
									
										
										
										
											2022-06-16 11:54:31 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        AccelCommandHelper(config, self)
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 02:10:43 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.axes_map = read_axes_map(config)
							 | 
						
					
						
							
								
									
										
										
										
											2020-10-15 02:08:10 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        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,))
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        # Setup mcu sensor_adxl345 bulk query code
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.mcu = mcu = self.spi.get_mcu()
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.oid = oid = mcu.create_oid()
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.query_adxl345_cmd = self.query_adxl345_end_cmd = None
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.query_adxl345_status_cmd = None
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        mcu.add_config_cmd("config_adxl345 oid=%d spi_oid=%d"
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                           % (oid, self.spi.get_oid()))
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        mcu.add_config_cmd("query_adxl345 oid=%d clock=0 rest_ticks=0"
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                           % (oid,), on_restart=True)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        mcu.register_config_callback(self._build_config)
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 11:30:51 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.bulk_queue = bulk_sensor.BulkDataQueue(mcu, "adxl345_data", oid)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        # Clock tracking
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 23:26:42 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        chip_smooth = self.data_rate * BATCH_UPDATES * 2
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 11:15:23 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth)
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 13:01:34 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.clock_updater = bulk_sensor.ChipClockUpdater(self.clock_sync,
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                                                          BYTES_PER_SAMPLE)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.last_error_count = 0
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 23:26:42 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        # Process messages in batches
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.batch_bulk = bulk_sensor.BatchBulkHelper(
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            self.printer, self._process_batch,
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            self._start_measurements, self._finish_measurements, BATCH_UPDATES)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-23 13:56:01 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.name = config.get_name().split()[-1]
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 14:31:32 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        hdr = ('time', 'x_acceleration', 'y_acceleration', 'z_acceleration')
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 23:26:42 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.batch_bulk.add_mux_endpoint("adxl345/dump_adxl345", "sensor",
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                                         self.name, {'header': hdr})
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								    def _build_config(self):
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        cmdqueue = self.spi.get_command_queue()
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.query_adxl345_cmd = self.mcu.lookup_command(
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            "query_adxl345 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue)
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.query_adxl345_end_cmd = self.mcu.lookup_query_command(
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            "query_adxl345 oid=%c clock=%u rest_ticks=%u",
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            "adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.query_adxl345_status_cmd = self.mcu.lookup_query_command(
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            "query_adxl345_status oid=%c",
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            "adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
							 | 
						
					
						
							
								
									
										
										
										
											2021-03-26 19:32:32 +01:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def read_reg(self, reg):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00])
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        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))
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 23:26:42 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def start_internal_client(self):
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-17 00:15:55 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        aqh = AccelQueryHelper(self.printer)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.batch_bulk.add_client(aqh.handle_batch)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        return aqh
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 23:26:42 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    # Measurement decoding
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def _extract_samples(self, raw_samples):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        # Load variables to optimize inner loop below
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 13:01:34 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        last_sequence = self.clock_updater.get_last_sequence()
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        # Process every message in raw_samples
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        count = seq = 0
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-22 16:24:16 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        for params in raw_samples:
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-04 14:13:37 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            seq_diff = (params['sequence'] - last_sequence) & 0xffff
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            seq_diff -= (seq_diff & 0x8000) << 1
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-04 14:13:37 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            seq = last_sequence + seq_diff
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            d = bytearray(params['data'])
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-22 16:24:16 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            for i in range(len(d) // BYTES_PER_SAMPLE):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                d_xyz = d[i*BYTES_PER_SAMPLE:(i+1)*BYTES_PER_SAMPLE]
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                xlow, ylow, zlow, xzhigh, yzhigh = d_xyz
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                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))
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                raw_xyz = (rx, ry, rz)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                x = round(raw_xyz[x_pos] * x_scale, 6)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                y = round(raw_xyz[y_pos] * y_scale, 6)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                z = round(raw_xyz[z_pos] * z_scale, 6)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								                ptime = round(time_base + (msg_cdiff + i) * inv_freq, 6)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                samples[count] = (ptime, x, y, z)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								                count += 1
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-22 16:24:16 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        del samples[count:]
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        return samples
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def _update_clock(self, minclock=0):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        # Query current state
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        for retry in range(5):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            params = self.query_adxl345_status_cmd.send([self.oid],
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                                                        minclock=minclock)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            fifo = params['fifo'] & 0x7f
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            if fifo <= 32:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                break
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        else:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            raise self.printer.command_error("Unable to query adxl345 fifo")
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 13:01:34 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.clock_updater.update_clock(params)
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 23:26:42 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    # Start, stop, and process message batches
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 23:09:49 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def _start_measurements(self):
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 22:16:03 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        # In case of miswiring, testing ADXL345 device ID prevents treating
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        # noise or wrong signal as a correctly initialized device
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        dev_id = self.read_reg(REG_DEVID)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        if dev_id != ADXL345_DEV_ID:
							 | 
						
					
						
							
								
									
										
										
										
											2021-09-26 01:57:10 +02:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								            raise self.printer.command_error(
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                "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))
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        # Setup chip in requested query rate
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.set_reg(REG_POWER_CTL, 0x00)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 22:16:03 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.set_reg(REG_DATA_FORMAT, 0x0B)
							 | 
						
					
						
							
								
									
										
										
										
											2021-03-26 19:32:32 +01:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.set_reg(REG_FIFO_CTL, 0x00)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate])
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-22 16:24:16 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.set_reg(REG_FIFO_CTL, SET_FIFO_CTL)
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        # Start bulk reading
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 11:30:51 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.bulk_queue.clear_samples()
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        systime = self.printer.get_reactor().monotonic()
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        reqclock = self.mcu.print_time_to_clock(print_time)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 21:54:19 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate)
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.query_adxl345_cmd.send([self.oid, reqclock, rest_ticks],
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								                                    reqclock=reqclock)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        logging.info("ADXL345 starting '%s' measurements", self.name)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        # Initialize clock tracking
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 13:01:34 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.clock_updater.note_start(reqclock)
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self._update_clock(minclock=reqclock)
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 13:01:34 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.clock_updater.clear_duration_filter()
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        self.last_error_count = 0
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-11 23:09:49 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def _finish_measurements(self):
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        # Halt bulk reading
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        params = self.query_adxl345_end_cmd.send([self.oid, 0, 0])
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 11:30:51 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self.bulk_queue.clear_samples()
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        logging.info("ADXL345 finished '%s' measurements", self.name)
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 23:26:42 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								    def _process_batch(self, eventtime):
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-14 13:41:12 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        self._update_clock()
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 11:30:51 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        raw_samples = self.bulk_queue.pull_samples()
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-12 01:05:09 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        if not raw_samples:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            return {}
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        samples = self._extract_samples(raw_samples)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								        if not samples:
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								            return {}
							 | 
						
					
						
							
								
									
										
										
										
											2021-08-22 16:24:16 -04:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								        return {'data': samples, 'errors': self.last_error_count,
							 | 
						
					
						
							
								
									
										
										
										
											2023-12-16 13:01:34 -05:00
										 
									 
								 
							 | 
							
								
									
										
									
								
							 | 
							
								
							 | 
							
							
								                'overflows': self.clock_updater.get_last_limit_count()}
							 | 
						
					
						
							
								
									
										
										
										
											2020-07-30 01:32:17 -04:00
										 
									 
								 
							 | 
							
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								def load_config(config):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								    return ADXL345(config)
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								def load_config_prefix(config):
							 | 
						
					
						
							| 
								
							 | 
							
								
							 | 
							
								
							 | 
							
							
								    return ADXL345(config)
							 |