mirror of https://github.com/Desuuuu/klipper.git
adxl345: Implement timing via new adxl345_status messages
Query the adxl345 message counter every 100ms so that accurate timing can be obtained during measurements. This allows the adxl345 data to be exported with timestamps while captures are running. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
dd95f80d9d
commit
e34137582d
|
@ -30,53 +30,26 @@ Accel_Measurement = collections.namedtuple(
|
||||||
|
|
||||||
# Helper class to obtain measurements
|
# Helper class to obtain measurements
|
||||||
class ADXL345QueryHelper:
|
class ADXL345QueryHelper:
|
||||||
def __init__(self, printer, chip, cconn):
|
def __init__(self, printer, cconn):
|
||||||
self.printer = printer
|
self.printer = printer
|
||||||
self.chip = chip
|
|
||||||
self.cconn = cconn
|
self.cconn = cconn
|
||||||
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.request_start_time = self.request_end_time = print_time
|
||||||
self.raw_samples = None
|
|
||||||
self.samples = []
|
self.samples = []
|
||||||
self.drops = self.overflows = 0
|
|
||||||
self.start2_time = 0.
|
|
||||||
self.time_per_sample = self.start_range = self.end_range = 0.
|
|
||||||
def finish_measurements(self):
|
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()
|
self.request_end_time = toolhead.get_last_move_time()
|
||||||
toolhead.wait_moves()
|
toolhead.wait_moves()
|
||||||
self.cconn.finalize()
|
self.cconn.finalize()
|
||||||
toolhead.dwell(0.200)
|
|
||||||
toolhead.wait_moves()
|
|
||||||
self._setup_data(*self.chip.final_results) # XXX
|
|
||||||
def get_stats(self):
|
|
||||||
return ("drops=%d,overflows=%d"
|
|
||||||
",time_per_sample=%.9f,start_range=%.6f,end_range=%.6f"
|
|
||||||
% (self.drops, self.overflows,
|
|
||||||
self.time_per_sample, self.start_range, self.end_range))
|
|
||||||
def _setup_data(self, end_sequence, overflows,
|
|
||||||
start1_time, start2_time, end1_time, end2_time):
|
|
||||||
raw_samples = self.cconn.get_messages()
|
|
||||||
if not raw_samples or not end_sequence:
|
|
||||||
return
|
|
||||||
self.raw_samples = raw_samples
|
|
||||||
self.overflows = overflows
|
|
||||||
self.start2_time = start2_time
|
|
||||||
self.start_range = start2_time - start1_time
|
|
||||||
self.end_range = end2_time - end1_time
|
|
||||||
self.total_count = raw_samples[-1]['params']['data'][-1][0] + 1
|
|
||||||
total_time = end2_time - start2_time
|
|
||||||
self.time_per_sample = total_time / self.total_count
|
|
||||||
actual_count = sum([len(m['params']['data']) for m in raw_samples])
|
|
||||||
self.drops = self.total_count - actual_count
|
|
||||||
def decode_samples(self):
|
def decode_samples(self):
|
||||||
if not self.raw_samples:
|
raw_samples = self.cconn.get_messages()
|
||||||
|
if not raw_samples:
|
||||||
return self.samples
|
return self.samples
|
||||||
|
total = sum([len(m['params']['data']) for m in raw_samples])
|
||||||
count = 0
|
count = 0
|
||||||
self.samples = samples = [None] * self.total_count
|
self.samples = samples = [None] * total
|
||||||
for msg in self.raw_samples:
|
for msg in raw_samples:
|
||||||
for seq, x, y, z in msg['params']['data']:
|
for samp_time, x, y, z in msg['params']['data']:
|
||||||
samp_time = self.start2_time + seq * self.time_per_sample
|
|
||||||
if samp_time < self.request_start_time:
|
if samp_time < self.request_start_time:
|
||||||
continue
|
continue
|
||||||
if samp_time > self.request_end_time:
|
if samp_time > self.request_end_time:
|
||||||
|
@ -93,8 +66,7 @@ class ADXL345QueryHelper:
|
||||||
except:
|
except:
|
||||||
pass
|
pass
|
||||||
f = open(filename, "w")
|
f = open(filename, "w")
|
||||||
f.write("##%s\n#time,accel_x,accel_y,accel_z\n" % (
|
f.write("#time,accel_x,accel_y,accel_z\n")
|
||||||
self.get_stats(),))
|
|
||||||
samples = self.samples or self.decode_samples()
|
samples = self.samples or self.decode_samples()
|
||||||
for t, accel_x, accel_y, accel_z in samples:
|
for t, accel_x, accel_y, accel_z in samples:
|
||||||
f.write("%.6f,%.6f,%.6f,%.6f\n" % (
|
f.write("%.6f,%.6f,%.6f,%.6f\n" % (
|
||||||
|
@ -183,6 +155,54 @@ class ADXLCommandHelper:
|
||||||
val = gcmd.get("VAL", minval=0, maxval=255, 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)
|
self.chip.set_reg(reg, val)
|
||||||
|
|
||||||
|
# Helper class for chip clock synchronization via linear regression
|
||||||
|
class ClockSyncRegression:
|
||||||
|
def __init__(self, mcu, chip_clock_smooth, decay = 1. / 20.):
|
||||||
|
self.mcu = mcu
|
||||||
|
self.chip_clock_smooth = chip_clock_smooth
|
||||||
|
self.decay = decay
|
||||||
|
self.last_chip_clock = self.last_exp_mcu_clock = 0.
|
||||||
|
self.mcu_clock_avg = self.mcu_clock_variance = 0.
|
||||||
|
self.chip_clock_avg = self.chip_clock_covariance = 0.
|
||||||
|
def reset(self, mcu_clock, chip_clock):
|
||||||
|
self.mcu_clock_avg = self.last_mcu_clock = mcu_clock
|
||||||
|
self.chip_clock_avg = chip_clock
|
||||||
|
self.mcu_clock_variance = self.chip_clock_covariance = 0.
|
||||||
|
self.last_chip_clock = self.last_exp_mcu_clock = 0.
|
||||||
|
def update(self, mcu_clock, chip_clock):
|
||||||
|
# Update linear regression
|
||||||
|
decay = self.decay
|
||||||
|
diff_mcu_clock = mcu_clock - self.mcu_clock_avg
|
||||||
|
self.mcu_clock_avg += decay * diff_mcu_clock
|
||||||
|
self.mcu_clock_variance = (1. - decay) * (
|
||||||
|
self.mcu_clock_variance + diff_mcu_clock**2 * decay)
|
||||||
|
diff_chip_clock = chip_clock - self.chip_clock_avg
|
||||||
|
self.chip_clock_avg += decay * diff_chip_clock
|
||||||
|
self.chip_clock_covariance = (1. - decay) * (
|
||||||
|
self.chip_clock_covariance + diff_mcu_clock*diff_chip_clock*decay)
|
||||||
|
def set_last_chip_clock(self, chip_clock):
|
||||||
|
base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
|
||||||
|
self.last_chip_clock = chip_clock
|
||||||
|
self.last_exp_mcu_clock = base_mcu + (chip_clock-base_chip) * inv_cfreq
|
||||||
|
def get_clock_translation(self):
|
||||||
|
inv_chip_freq = self.mcu_clock_variance / self.chip_clock_covariance
|
||||||
|
if not self.last_chip_clock:
|
||||||
|
return self.mcu_clock_avg, self.chip_clock_avg, inv_chip_freq
|
||||||
|
# Find mcu clock associated with future chip_clock
|
||||||
|
s_chip_clock = self.last_chip_clock + self.chip_clock_smooth
|
||||||
|
scdiff = s_chip_clock - self.chip_clock_avg
|
||||||
|
s_mcu_clock = self.mcu_clock_avg + scdiff * inv_chip_freq
|
||||||
|
# Calculate frequency to converge at future point
|
||||||
|
mdiff = s_mcu_clock - self.last_exp_mcu_clock
|
||||||
|
s_inv_chip_freq = mdiff / self.chip_clock_smooth
|
||||||
|
return self.last_exp_mcu_clock, self.last_chip_clock, s_inv_chip_freq
|
||||||
|
def get_time_translation(self):
|
||||||
|
base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
|
||||||
|
clock_to_print_time = self.mcu.clock_to_print_time
|
||||||
|
base_time = clock_to_print_time(base_mcu)
|
||||||
|
inv_freq = clock_to_print_time(base_mcu + inv_cfreq) - base_time
|
||||||
|
return base_time, base_chip, inv_freq
|
||||||
|
|
||||||
MIN_MSG_TIME = 0.100
|
MIN_MSG_TIME = 0.100
|
||||||
|
|
||||||
# Printer class that controls ADXL345 chip
|
# Printer class that controls ADXL345 chip
|
||||||
|
@ -191,7 +211,6 @@ class ADXL345:
|
||||||
self.printer = config.get_printer()
|
self.printer = config.get_printer()
|
||||||
ADXLCommandHelper(config, self)
|
ADXLCommandHelper(config, self)
|
||||||
self.query_rate = 0
|
self.query_rate = 0
|
||||||
self.last_sequence = 0
|
|
||||||
am = {'x': (0, SCALE), 'y': (1, SCALE), 'z': (2, SCALE),
|
am = {'x': (0, SCALE), 'y': (1, SCALE), 'z': (2, SCALE),
|
||||||
'-x': (0, -SCALE), '-y': (1, -SCALE), '-z': (2, -SCALE)}
|
'-x': (0, -SCALE), '-y': (1, -SCALE), '-z': (2, -SCALE)}
|
||||||
axes_map = config.getlist('axes_map', ('x','y','z'), count=3)
|
axes_map = config.getlist('axes_map', ('x','y','z'), count=3)
|
||||||
|
@ -204,19 +223,21 @@ class ADXL345:
|
||||||
# Measurement storage (accessed from background thread)
|
# Measurement storage (accessed from background thread)
|
||||||
self.lock = threading.Lock()
|
self.lock = threading.Lock()
|
||||||
self.raw_samples = []
|
self.raw_samples = []
|
||||||
self.samples_start1 = self.samples_start2 = 0.
|
|
||||||
# Setup mcu sensor_adxl345 bulk query code
|
# Setup mcu sensor_adxl345 bulk query code
|
||||||
self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000)
|
self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000)
|
||||||
self.mcu = mcu = self.spi.get_mcu()
|
self.mcu = mcu = self.spi.get_mcu()
|
||||||
self.oid = oid = mcu.create_oid()
|
self.oid = oid = mcu.create_oid()
|
||||||
self.query_adxl345_cmd = self.query_adxl345_end_cmd =None
|
self.query_adxl345_cmd = self.query_adxl345_end_cmd = None
|
||||||
|
self.query_adxl345_status_cmd = None
|
||||||
mcu.add_config_cmd("config_adxl345 oid=%d spi_oid=%d"
|
mcu.add_config_cmd("config_adxl345 oid=%d spi_oid=%d"
|
||||||
% (oid, self.spi.get_oid()))
|
% (oid, self.spi.get_oid()))
|
||||||
mcu.add_config_cmd("query_adxl345 oid=%d clock=0 rest_ticks=0"
|
mcu.add_config_cmd("query_adxl345 oid=%d clock=0 rest_ticks=0"
|
||||||
% (oid,), on_restart=True)
|
% (oid,), on_restart=True)
|
||||||
mcu.register_config_callback(self._build_config)
|
mcu.register_config_callback(self._build_config)
|
||||||
mcu.register_response(self._handle_adxl345_start, "adxl345_start", oid)
|
|
||||||
mcu.register_response(self._handle_adxl345_data, "adxl345_data", oid)
|
mcu.register_response(self._handle_adxl345_data, "adxl345_data", oid)
|
||||||
|
# Clock tracking
|
||||||
|
self.last_sequence = self.last_limit_count = self.max_query_duration = 0
|
||||||
|
self.clock_sync = ClockSyncRegression(self.mcu, 640)
|
||||||
# API server endpoints
|
# API server endpoints
|
||||||
self.api_dump = motion_report.APIDumpHelper(
|
self.api_dump = motion_report.APIDumpHelper(
|
||||||
self.printer, self._api_update, self._api_startstop, 0.100)
|
self.printer, self._api_update, self._api_startstop, 0.100)
|
||||||
|
@ -227,21 +248,17 @@ class ADXL345:
|
||||||
wh.register_mux_endpoint("adxl345/dump_adxl345", "sensor", self.name,
|
wh.register_mux_endpoint("adxl345/dump_adxl345", "sensor", self.name,
|
||||||
self._handle_dump_adxl345)
|
self._handle_dump_adxl345)
|
||||||
def _build_config(self):
|
def _build_config(self):
|
||||||
|
cmdqueue = self.spi.get_command_queue()
|
||||||
self.query_adxl345_cmd = self.mcu.lookup_command(
|
self.query_adxl345_cmd = self.mcu.lookup_command(
|
||||||
"query_adxl345 oid=%c clock=%u rest_ticks=%u",
|
"query_adxl345 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue)
|
||||||
cq=self.spi.get_command_queue())
|
|
||||||
self.query_adxl345_end_cmd = self.mcu.lookup_query_command(
|
self.query_adxl345_end_cmd = self.mcu.lookup_query_command(
|
||||||
"query_adxl345 oid=%c clock=%u rest_ticks=%u",
|
"query_adxl345 oid=%c clock=%u rest_ticks=%u",
|
||||||
"adxl345_end oid=%c end1_clock=%u end2_clock=%u"
|
"adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
|
||||||
" limit_count=%hu sequence=%hu",
|
" buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
|
||||||
oid=self.oid, cq=self.spi.get_command_queue())
|
self.query_adxl345_status_cmd = self.mcu.lookup_query_command(
|
||||||
def _clock_to_print_time(self, clock):
|
"query_adxl345_status oid=%c",
|
||||||
return self.mcu.clock_to_print_time(self.mcu.clock32_to_clock64(clock))
|
"adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
|
||||||
def _convert_sequence(self, sequence):
|
" buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
|
||||||
sequence = (self.last_sequence & ~0xffff) | sequence
|
|
||||||
if sequence < self.last_sequence:
|
|
||||||
sequence += 0x10000
|
|
||||||
return sequence
|
|
||||||
def read_reg(self, reg):
|
def read_reg(self, reg):
|
||||||
params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00])
|
params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00])
|
||||||
response = bytearray(params['response'])
|
response = bytearray(params['response'])
|
||||||
|
@ -258,9 +275,6 @@ class ADXL345:
|
||||||
# Measurement collection
|
# Measurement collection
|
||||||
def is_measuring(self):
|
def is_measuring(self):
|
||||||
return self.query_rate > 0
|
return self.query_rate > 0
|
||||||
def _handle_adxl345_start(self, params):
|
|
||||||
self.samples_start1 = self._clock_to_print_time(params['start1_clock'])
|
|
||||||
self.samples_start2 = self._clock_to_print_time(params['start2_clock'])
|
|
||||||
def _handle_adxl345_data(self, params):
|
def _handle_adxl345_data(self, params):
|
||||||
with self.lock:
|
with self.lock:
|
||||||
self.raw_samples.append(params)
|
self.raw_samples.append(params)
|
||||||
|
@ -268,27 +282,62 @@ class ADXL345:
|
||||||
# Load variables to optimize inner loop below
|
# Load variables to optimize inner loop below
|
||||||
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
|
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
|
||||||
last_sequence = self.last_sequence
|
last_sequence = self.last_sequence
|
||||||
|
time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
|
||||||
# Process every message in raw_samples
|
# Process every message in raw_samples
|
||||||
count = 0
|
count = seq = 0
|
||||||
samples = [None] * (len(raw_samples) * 8)
|
samples = [None] * (len(raw_samples) * 8)
|
||||||
for params in raw_samples:
|
for params in raw_samples:
|
||||||
seq = (last_sequence & ~0xffff) | params['sequence']
|
seq_diff = (last_sequence - params['sequence']) & 0xffff
|
||||||
if seq < last_sequence:
|
seq_diff -= (seq_diff & 0x8000) << 1
|
||||||
seq += 0x10000
|
seq = last_sequence - seq_diff
|
||||||
last_sequence = seq
|
|
||||||
d = bytearray(params['data'])
|
d = bytearray(params['data'])
|
||||||
len_d = len(d)
|
len_d = len(d)
|
||||||
sdata = [(d[i] | (d[i+1] << 8)) - ((d[i+1] & 0x80) << 9)
|
sdata = [(d[i] | ((d[i+1] & 0x1f) << 8)) - ((d[i+1] & 0x10) << 9)
|
||||||
for i in range(0, len_d-1, 2)]
|
for i in range(0, len_d-1, 2)]
|
||||||
|
msg_cdiff = seq * 8 - chip_base
|
||||||
for i in range(len_d // 6):
|
for i in range(len_d // 6):
|
||||||
x = round(sdata[i*3 + x_pos] * x_scale, 6)
|
x = round(sdata[i*3 + x_pos] * x_scale, 6)
|
||||||
y = round(sdata[i*3 + y_pos] * y_scale, 6)
|
y = round(sdata[i*3 + y_pos] * y_scale, 6)
|
||||||
z = round(sdata[i*3 + z_pos] * z_scale, 6)
|
z = round(sdata[i*3 + z_pos] * z_scale, 6)
|
||||||
samples[count] = (seq * 8 + i, x, y, z)
|
ptime = round(time_base + (msg_cdiff + i) * inv_freq, 6)
|
||||||
|
samples[count] = (ptime, x, y, z)
|
||||||
count += 1
|
count += 1
|
||||||
self.last_sequence = last_sequence
|
self.clock_sync.set_last_chip_clock(seq * 8 + i)
|
||||||
del samples[count:]
|
del samples[count:]
|
||||||
return samples
|
return samples
|
||||||
|
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")
|
||||||
|
mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
|
||||||
|
sequence = (self.last_sequence & ~0xffff) | params['next_sequence']
|
||||||
|
if sequence < self.last_sequence:
|
||||||
|
sequence += 0x10000
|
||||||
|
self.last_sequence = sequence
|
||||||
|
buffered = params['buffered']
|
||||||
|
limit_count = (self.last_limit_count & ~0xffff) | params['limit_count']
|
||||||
|
if limit_count < self.last_limit_count:
|
||||||
|
limit_count += 0x10000
|
||||||
|
self.last_limit_count = limit_count
|
||||||
|
duration = params['query_ticks']
|
||||||
|
if duration > self.max_query_duration:
|
||||||
|
# Skip measurement as a high query time could skew clock tracking
|
||||||
|
self.max_query_duration = max(2 * self.max_query_duration,
|
||||||
|
self.mcu.seconds_to_clock(.000005))
|
||||||
|
return
|
||||||
|
self.max_query_duration = 2 * duration
|
||||||
|
msg_count = sequence * 8 + buffered // 6 + fifo
|
||||||
|
# The "chip clock" is the message counter plus .5 for average
|
||||||
|
# inaccuracy of query responses and plus .5 for assumed offset
|
||||||
|
# of adxl345 hw processing time.
|
||||||
|
chip_clock = msg_count + 1
|
||||||
|
self.clock_sync.update(mcu_clock + duration // 2, chip_clock)
|
||||||
def _start_measurements(self):
|
def _start_measurements(self):
|
||||||
if self.is_measuring():
|
if self.is_measuring():
|
||||||
return
|
return
|
||||||
|
@ -305,19 +354,24 @@ class ADXL345:
|
||||||
self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate])
|
self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate])
|
||||||
self.set_reg(REG_FIFO_CTL, 0x80)
|
self.set_reg(REG_FIFO_CTL, 0x80)
|
||||||
# Setup samples
|
# Setup samples
|
||||||
systime = self.printer.get_reactor().monotonic()
|
|
||||||
print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
|
|
||||||
self.samples_start1 = self.samples_start2 = print_time
|
|
||||||
self.last_sequence = 0
|
|
||||||
with self.lock:
|
with self.lock:
|
||||||
self.raw_samples = []
|
self.raw_samples = []
|
||||||
# Start bulk reading
|
# Start bulk reading
|
||||||
|
systime = self.printer.get_reactor().monotonic()
|
||||||
|
print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
|
||||||
reqclock = self.mcu.print_time_to_clock(print_time)
|
reqclock = self.mcu.print_time_to_clock(print_time)
|
||||||
rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate)
|
rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate)
|
||||||
self.query_rate = self.data_rate
|
self.query_rate = self.data_rate
|
||||||
self.query_adxl345_cmd.send([self.oid, reqclock, rest_ticks],
|
self.query_adxl345_cmd.send([self.oid, reqclock, rest_ticks],
|
||||||
reqclock=reqclock)
|
reqclock=reqclock)
|
||||||
logging.info("ADXL345 starting '%s' measurements", self.name)
|
logging.info("ADXL345 starting '%s' measurements", self.name)
|
||||||
|
# Initialize clock tracking
|
||||||
|
self.last_sequence = 0
|
||||||
|
self.last_limit_count = 0
|
||||||
|
self.clock_sync.reset(reqclock, 0)
|
||||||
|
self.max_query_duration = 1 << 31
|
||||||
|
self._update_clock(minclock=reqclock)
|
||||||
|
self.max_query_duration = 1 << 31
|
||||||
def _finish_measurements(self):
|
def _finish_measurements(self):
|
||||||
if not self.is_measuring():
|
if not self.is_measuring():
|
||||||
return
|
return
|
||||||
|
@ -326,17 +380,10 @@ class ADXL345:
|
||||||
self.query_rate = 0
|
self.query_rate = 0
|
||||||
with self.lock:
|
with self.lock:
|
||||||
self.raw_samples = []
|
self.raw_samples = []
|
||||||
# Generate results
|
|
||||||
end1_time = self._clock_to_print_time(params['end1_clock'])
|
|
||||||
end2_time = self._clock_to_print_time(params['end2_clock'])
|
|
||||||
end_sequence = self._convert_sequence(params['sequence'])
|
|
||||||
overflows = params['limit_count']
|
|
||||||
logging.info("ADXL345 finished '%s' measurements", self.name)
|
logging.info("ADXL345 finished '%s' measurements", self.name)
|
||||||
self.final_results = (end_sequence, overflows,
|
|
||||||
self.samples_start1, self.samples_start2,
|
|
||||||
end1_time, end2_time) # XXX
|
|
||||||
# API interface
|
# API interface
|
||||||
def _api_update(self, eventtime):
|
def _api_update(self, eventtime):
|
||||||
|
self._update_clock()
|
||||||
with self.lock:
|
with self.lock:
|
||||||
raw_samples = self.raw_samples
|
raw_samples = self.raw_samples
|
||||||
self.raw_samples = []
|
self.raw_samples = []
|
||||||
|
@ -345,7 +392,7 @@ class ADXL345:
|
||||||
samples = self._extract_samples(raw_samples)
|
samples = self._extract_samples(raw_samples)
|
||||||
if not samples:
|
if not samples:
|
||||||
return {}
|
return {}
|
||||||
return {'data': samples}
|
return {'data': samples, 'overflows': self.last_limit_count}
|
||||||
def _api_startstop(self, is_start):
|
def _api_startstop(self, is_start):
|
||||||
if is_start:
|
if is_start:
|
||||||
self._start_measurements()
|
self._start_measurements()
|
||||||
|
@ -353,14 +400,14 @@ class ADXL345:
|
||||||
self._finish_measurements()
|
self._finish_measurements()
|
||||||
def _handle_dump_adxl345(self, web_request):
|
def _handle_dump_adxl345(self, web_request):
|
||||||
self.api_dump.add_client(web_request)
|
self.api_dump.add_client(web_request)
|
||||||
hdr = ('sequence', 'x_acceleration', 'y_acceleration', 'z_acceleration')
|
hdr = ('time', 'x_acceleration', 'y_acceleration', 'z_acceleration')
|
||||||
web_request.send({'header': hdr})
|
web_request.send({'header': hdr})
|
||||||
def start_internal_client(self):
|
def start_internal_client(self):
|
||||||
if self.is_measuring():
|
if self.is_measuring():
|
||||||
raise self.printer.command_error(
|
raise self.printer.command_error(
|
||||||
"ADXL345 measurement already in progress")
|
"ADXL345 measurement already in progress")
|
||||||
cconn = self.api_dump.add_internal_client()
|
cconn = self.api_dump.add_internal_client()
|
||||||
return ADXL345QueryHelper(self.printer, self, cconn)
|
return ADXL345QueryHelper(self.printer, cconn)
|
||||||
|
|
||||||
def load_config(config):
|
def load_config(config):
|
||||||
return ADXL345(config)
|
return ADXL345(config)
|
||||||
|
|
|
@ -181,8 +181,6 @@ class ResonanceTester:
|
||||||
gcmd.respond_info(
|
gcmd.respond_info(
|
||||||
"Writing raw accelerometer data to "
|
"Writing raw accelerometer data to "
|
||||||
"%s file" % (raw_name,))
|
"%s file" % (raw_name,))
|
||||||
gcmd.respond_info("%s-axis accelerometer stats: %s" % (
|
|
||||||
chip_axis, aclient.get_stats(),))
|
|
||||||
if helper is None:
|
if helper is None:
|
||||||
continue
|
continue
|
||||||
for chip_axis, chip_values in raw_values:
|
for chip_axis, chip_values in raw_values:
|
||||||
|
|
|
@ -27,6 +27,7 @@ enum {
|
||||||
|
|
||||||
static struct task_wake adxl345_wake;
|
static struct task_wake adxl345_wake;
|
||||||
|
|
||||||
|
// Event handler that wakes adxl345_task() periodically
|
||||||
static uint_fast8_t
|
static uint_fast8_t
|
||||||
adxl345_event(struct timer *timer)
|
adxl345_event(struct timer *timer)
|
||||||
{
|
{
|
||||||
|
@ -56,6 +57,27 @@ adxl_report(struct adxl345 *ax, uint8_t oid)
|
||||||
ax->sequence++;
|
ax->sequence++;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Report buffer and fifo status
|
||||||
|
static void
|
||||||
|
adxl_status(struct adxl345 *ax, uint_fast8_t oid
|
||||||
|
, uint32_t time1, uint32_t time2, uint_fast8_t fifo)
|
||||||
|
{
|
||||||
|
sendf("adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
|
||||||
|
" buffered=%c fifo=%c limit_count=%hu"
|
||||||
|
, oid, time1, time2-time1, ax->sequence
|
||||||
|
, ax->data_count, fifo, ax->limit_count);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Helper code to reschedule the adxl345_event() timer
|
||||||
|
static void
|
||||||
|
adxl_reschedule_timer(struct adxl345 *ax)
|
||||||
|
{
|
||||||
|
irq_disable();
|
||||||
|
ax->timer.waketime = timer_read_time() + ax->rest_ticks;
|
||||||
|
sched_add_timer(&ax->timer);
|
||||||
|
irq_enable();
|
||||||
|
}
|
||||||
|
|
||||||
// Chip registers
|
// Chip registers
|
||||||
#define AR_POWER_CTL 0x2D
|
#define AR_POWER_CTL 0x2D
|
||||||
#define AR_DATAX0 0x32
|
#define AR_DATAX0 0x32
|
||||||
|
@ -74,7 +96,7 @@ adxl_query(struct adxl345 *ax, uint8_t oid)
|
||||||
if (ax->data_count + 6 > ARRAY_SIZE(ax->data))
|
if (ax->data_count + 6 > ARRAY_SIZE(ax->data))
|
||||||
adxl_report(ax, oid);
|
adxl_report(ax, oid);
|
||||||
uint_fast8_t fifo_status = msg[8] & ~0x80; // Ignore trigger bit
|
uint_fast8_t fifo_status = msg[8] & ~0x80; // Ignore trigger bit
|
||||||
if (fifo_status >= 31 && ax->limit_count != 0xffff)
|
if (fifo_status >= 31)
|
||||||
ax->limit_count++;
|
ax->limit_count++;
|
||||||
if (fifo_status > 1 && fifo_status <= 32) {
|
if (fifo_status > 1 && fifo_status <= 32) {
|
||||||
// More data in fifo - wake this task again
|
// More data in fifo - wake this task again
|
||||||
|
@ -83,10 +105,7 @@ adxl_query(struct adxl345 *ax, uint8_t oid)
|
||||||
// Sleep until next check time
|
// Sleep until next check time
|
||||||
sched_del_timer(&ax->timer);
|
sched_del_timer(&ax->timer);
|
||||||
ax->flags &= ~AX_PENDING;
|
ax->flags &= ~AX_PENDING;
|
||||||
irq_disable();
|
adxl_reschedule_timer(ax);
|
||||||
ax->timer.waketime = timer_read_time() + ax->rest_ticks;
|
|
||||||
sched_add_timer(&ax->timer);
|
|
||||||
irq_enable();
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -97,15 +116,8 @@ adxl_start(struct adxl345 *ax, uint8_t oid)
|
||||||
sched_del_timer(&ax->timer);
|
sched_del_timer(&ax->timer);
|
||||||
ax->flags = AX_RUNNING;
|
ax->flags = AX_RUNNING;
|
||||||
uint8_t msg[2] = { AR_POWER_CTL, 0x08 };
|
uint8_t msg[2] = { AR_POWER_CTL, 0x08 };
|
||||||
uint32_t start1_time = timer_read_time();
|
|
||||||
spidev_transfer(ax->spi, 0, sizeof(msg), msg);
|
spidev_transfer(ax->spi, 0, sizeof(msg), msg);
|
||||||
irq_disable();
|
adxl_reschedule_timer(ax);
|
||||||
uint32_t start2_time = timer_read_time();
|
|
||||||
ax->timer.waketime = start2_time + ax->rest_ticks;
|
|
||||||
sched_add_timer(&ax->timer);
|
|
||||||
irq_enable();
|
|
||||||
sendf("adxl345_start oid=%c start1_clock=%u start2_clock=%u"
|
|
||||||
, oid, start1_time, start2_time);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// End measurements
|
// End measurements
|
||||||
|
@ -123,18 +135,18 @@ adxl_stop(struct adxl345 *ax, uint8_t oid)
|
||||||
uint_fast8_t i;
|
uint_fast8_t i;
|
||||||
for (i=0; i<33; i++) {
|
for (i=0; i<33; i++) {
|
||||||
msg[0] = AR_FIFO_STATUS | AM_READ;
|
msg[0] = AR_FIFO_STATUS | AM_READ;
|
||||||
msg[1] = 0;
|
msg[1] = 0x00;
|
||||||
spidev_transfer(ax->spi, 1, sizeof(msg), msg);
|
spidev_transfer(ax->spi, 1, sizeof(msg), msg);
|
||||||
if (!(msg[1] & 0x3f))
|
uint_fast8_t fifo_status = msg[1] & ~0x80;
|
||||||
|
if (!fifo_status)
|
||||||
break;
|
break;
|
||||||
adxl_query(ax, oid);
|
if (fifo_status <= 32)
|
||||||
|
adxl_query(ax, oid);
|
||||||
}
|
}
|
||||||
// Report final data
|
// Report final data
|
||||||
if (ax->data_count)
|
if (ax->data_count)
|
||||||
adxl_report(ax, oid);
|
adxl_report(ax, oid);
|
||||||
sendf("adxl345_end oid=%c end1_clock=%u end2_clock=%u"
|
adxl_status(ax, oid, end1_time, end2_time, msg[1]);
|
||||||
" limit_count=%hu sequence=%hu"
|
|
||||||
, oid, end1_time, end2_time, ax->limit_count, ax->sequence);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -159,6 +171,18 @@ command_query_adxl345(uint32_t *args)
|
||||||
DECL_COMMAND(command_query_adxl345,
|
DECL_COMMAND(command_query_adxl345,
|
||||||
"query_adxl345 oid=%c clock=%u rest_ticks=%u");
|
"query_adxl345 oid=%c clock=%u rest_ticks=%u");
|
||||||
|
|
||||||
|
void
|
||||||
|
command_query_adxl345_status(uint32_t *args)
|
||||||
|
{
|
||||||
|
struct adxl345 *ax = oid_lookup(args[0], command_config_adxl345);
|
||||||
|
uint8_t msg[2] = { AR_FIFO_STATUS | AM_READ, 0x00 };
|
||||||
|
uint32_t time1 = timer_read_time();
|
||||||
|
spidev_transfer(ax->spi, 1, sizeof(msg), msg);
|
||||||
|
uint32_t time2 = timer_read_time();
|
||||||
|
adxl_status(ax, args[0], time1, time2, msg[1]);
|
||||||
|
}
|
||||||
|
DECL_COMMAND(command_query_adxl345_status, "query_adxl345_status oid=%c");
|
||||||
|
|
||||||
void
|
void
|
||||||
adxl345_task(void)
|
adxl345_task(void)
|
||||||
{
|
{
|
||||||
|
|
Loading…
Reference in New Issue