2016-05-25 17:37:40 +02:00
|
|
|
# Multi-processor safe interface to micro-controller
|
|
|
|
#
|
|
|
|
# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net>
|
|
|
|
#
|
|
|
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
2017-02-06 19:31:34 +01:00
|
|
|
import sys, zlib, logging, math
|
2016-05-25 17:37:40 +02:00
|
|
|
import serialhdl, pins, chelper
|
|
|
|
|
2016-11-30 00:27:48 +01:00
|
|
|
class error(Exception):
|
|
|
|
pass
|
2016-11-18 19:03:40 +01:00
|
|
|
|
2016-05-25 17:37:40 +02:00
|
|
|
def parse_pin_extras(pin, can_pullup=False):
|
|
|
|
pullup = invert = 0
|
|
|
|
if can_pullup and pin.startswith('^'):
|
2016-11-08 15:29:38 +01:00
|
|
|
pullup = 1
|
2016-05-25 17:37:40 +02:00
|
|
|
pin = pin[1:].strip()
|
|
|
|
if pin.startswith('!'):
|
2016-11-08 15:29:38 +01:00
|
|
|
invert = 1
|
2016-05-25 17:37:40 +02:00
|
|
|
pin = pin[1:].strip()
|
|
|
|
return pin, pullup, invert
|
|
|
|
|
2017-02-06 17:37:03 +01:00
|
|
|
STEPCOMPRESS_ERROR_RET = -989898989
|
|
|
|
|
2016-05-25 17:37:40 +02:00
|
|
|
class MCU_stepper:
|
|
|
|
def __init__(self, mcu, step_pin, dir_pin, min_stop_interval, max_error):
|
|
|
|
self._mcu = mcu
|
|
|
|
self._oid = mcu.create_oid()
|
|
|
|
step_pin, pullup, invert_step = parse_pin_extras(step_pin)
|
|
|
|
dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin)
|
2016-08-24 22:42:25 +02:00
|
|
|
self._mcu_freq = mcu.get_mcu_freq()
|
|
|
|
min_stop_interval = int(min_stop_interval * self._mcu_freq)
|
|
|
|
max_error = int(max_error * self._mcu_freq)
|
2016-11-19 17:34:42 +01:00
|
|
|
self.commanded_position = 0
|
2016-11-28 17:22:01 +01:00
|
|
|
self._mcu_position_offset = 0
|
2016-05-25 17:37:40 +02:00
|
|
|
mcu.add_config_cmd(
|
|
|
|
"config_stepper oid=%d step_pin=%s dir_pin=%s"
|
|
|
|
" min_stop_interval=%d invert_step=%d" % (
|
|
|
|
self._oid, step_pin, dir_pin, min_stop_interval, invert_step))
|
|
|
|
mcu.register_stepper(self)
|
|
|
|
self._step_cmd = mcu.lookup_command(
|
|
|
|
"queue_step oid=%c interval=%u count=%hu add=%hi")
|
|
|
|
self._dir_cmd = mcu.lookup_command(
|
|
|
|
"set_next_step_dir oid=%c dir=%c")
|
|
|
|
self._reset_cmd = mcu.lookup_command(
|
|
|
|
"reset_step_clock oid=%c clock=%u")
|
|
|
|
ffi_main, self.ffi_lib = chelper.get_ffi()
|
2016-11-30 07:58:45 +01:00
|
|
|
self._stepqueue = ffi_main.gc(self.ffi_lib.stepcompress_alloc(
|
2016-11-10 18:44:04 +01:00
|
|
|
max_error, self._step_cmd.msgid
|
2016-11-30 07:58:45 +01:00
|
|
|
, self._dir_cmd.msgid, self._invert_dir, self._oid),
|
|
|
|
self.ffi_lib.stepcompress_free)
|
2016-08-24 22:42:25 +02:00
|
|
|
self.print_to_mcu_time = mcu.print_to_mcu_time
|
2016-05-25 17:37:40 +02:00
|
|
|
def get_oid(self):
|
|
|
|
return self._oid
|
2016-07-27 04:06:14 +02:00
|
|
|
def get_invert_dir(self):
|
|
|
|
return self._invert_dir
|
2016-11-19 17:34:42 +01:00
|
|
|
def set_position(self, pos):
|
2016-11-28 17:22:01 +01:00
|
|
|
self._mcu_position_offset += self.commanded_position - pos
|
2016-11-19 17:34:42 +01:00
|
|
|
self.commanded_position = pos
|
2016-11-28 17:22:01 +01:00
|
|
|
def set_mcu_position(self, pos):
|
|
|
|
self._mcu_position_offset = pos - self.commanded_position
|
|
|
|
def get_mcu_position(self):
|
|
|
|
return self.commanded_position + self._mcu_position_offset
|
2016-12-30 23:02:28 +01:00
|
|
|
def note_homing_start(self, homing_clock):
|
2017-02-06 17:37:03 +01:00
|
|
|
ret = self.ffi_lib.stepcompress_set_homing(self._stepqueue, homing_clock)
|
|
|
|
if ret:
|
|
|
|
raise error("Internal error in stepcompress")
|
2016-12-30 23:02:28 +01:00
|
|
|
def note_homing_finalized(self):
|
2017-02-06 17:37:03 +01:00
|
|
|
ret = self.ffi_lib.stepcompress_set_homing(self._stepqueue, 0)
|
|
|
|
if ret:
|
|
|
|
raise error("Internal error in stepcompress")
|
|
|
|
ret = self.ffi_lib.stepcompress_reset(self._stepqueue, 0)
|
|
|
|
if ret:
|
|
|
|
raise error("Internal error in stepcompress")
|
2016-11-14 19:10:14 +01:00
|
|
|
def reset_step_clock(self, mcu_time):
|
2016-11-10 18:44:04 +01:00
|
|
|
clock = int(mcu_time * self._mcu_freq)
|
2017-02-06 17:37:03 +01:00
|
|
|
ret = self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
|
|
|
|
if ret:
|
|
|
|
raise error("Internal error in stepcompress")
|
2016-05-25 17:37:40 +02:00
|
|
|
data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff)
|
2017-02-06 17:37:03 +01:00
|
|
|
ret = self.ffi_lib.stepcompress_queue_msg(
|
|
|
|
self._stepqueue, data, len(data))
|
|
|
|
if ret:
|
|
|
|
raise error("Internal error in stepcompress")
|
2016-11-10 18:44:04 +01:00
|
|
|
def step(self, mcu_time, sdir):
|
2016-08-24 22:42:25 +02:00
|
|
|
clock = mcu_time * self._mcu_freq
|
2017-02-06 17:37:03 +01:00
|
|
|
ret = self.ffi_lib.stepcompress_push(self._stepqueue, clock, sdir)
|
|
|
|
if ret:
|
|
|
|
raise error("Internal error in stepcompress")
|
2016-11-19 17:34:42 +01:00
|
|
|
if sdir:
|
|
|
|
self.commanded_position += 1
|
|
|
|
else:
|
|
|
|
self.commanded_position -= 1
|
2016-08-24 22:42:25 +02:00
|
|
|
def step_sqrt(self, mcu_time, steps, step_offset, sqrt_offset, factor):
|
|
|
|
clock = mcu_time * self._mcu_freq
|
|
|
|
mcu_freq2 = self._mcu_freq**2
|
2016-11-19 17:34:42 +01:00
|
|
|
count = self.ffi_lib.stepcompress_push_sqrt(
|
2016-08-24 22:42:25 +02:00
|
|
|
self._stepqueue, steps, step_offset, clock
|
|
|
|
, sqrt_offset * mcu_freq2, factor * mcu_freq2)
|
2017-02-06 17:37:03 +01:00
|
|
|
if count == STEPCOMPRESS_ERROR_RET:
|
|
|
|
raise error("Internal error in stepcompress")
|
2016-11-19 17:34:42 +01:00
|
|
|
self.commanded_position += count
|
|
|
|
return count
|
2016-08-24 22:42:25 +02:00
|
|
|
def step_factor(self, mcu_time, steps, step_offset, factor):
|
|
|
|
clock = mcu_time * self._mcu_freq
|
2016-11-19 17:34:42 +01:00
|
|
|
count = self.ffi_lib.stepcompress_push_factor(
|
2016-08-24 22:42:25 +02:00
|
|
|
self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq)
|
2017-02-06 17:37:03 +01:00
|
|
|
if count == STEPCOMPRESS_ERROR_RET:
|
|
|
|
raise error("Internal error in stepcompress")
|
2016-11-19 17:34:42 +01:00
|
|
|
self.commanded_position += count
|
|
|
|
return count
|
2016-12-05 01:30:35 +01:00
|
|
|
def step_delta_const(self, mcu_time, dist, start_pos
|
|
|
|
, inv_velocity, step_dist
|
|
|
|
, height, closestxy_d, closest_height2, movez_r):
|
2016-09-15 18:20:49 +02:00
|
|
|
clock = mcu_time * self._mcu_freq
|
2016-11-19 17:34:42 +01:00
|
|
|
count = self.ffi_lib.stepcompress_push_delta_const(
|
2016-12-05 01:30:35 +01:00
|
|
|
self._stepqueue, clock, dist, start_pos
|
|
|
|
, inv_velocity * self._mcu_freq, step_dist
|
|
|
|
, height, closestxy_d, closest_height2, movez_r)
|
2017-02-06 17:37:03 +01:00
|
|
|
if count == STEPCOMPRESS_ERROR_RET:
|
|
|
|
raise error("Internal error in stepcompress")
|
2016-11-19 17:34:42 +01:00
|
|
|
self.commanded_position += count
|
|
|
|
return count
|
2016-12-05 01:30:35 +01:00
|
|
|
def step_delta_accel(self, mcu_time, dist, start_pos
|
|
|
|
, accel_multiplier, step_dist
|
|
|
|
, height, closestxy_d, closest_height2, movez_r):
|
2016-09-15 18:20:49 +02:00
|
|
|
clock = mcu_time * self._mcu_freq
|
|
|
|
mcu_freq2 = self._mcu_freq**2
|
2016-11-19 17:34:42 +01:00
|
|
|
count = self.ffi_lib.stepcompress_push_delta_accel(
|
2016-12-05 01:30:35 +01:00
|
|
|
self._stepqueue, clock, dist, start_pos
|
|
|
|
, accel_multiplier * mcu_freq2, step_dist
|
|
|
|
, height, closestxy_d, closest_height2, movez_r)
|
2017-02-06 17:37:03 +01:00
|
|
|
if count == STEPCOMPRESS_ERROR_RET:
|
|
|
|
raise error("Internal error in stepcompress")
|
2016-11-19 17:34:42 +01:00
|
|
|
self.commanded_position += count
|
|
|
|
return count
|
2016-05-25 17:37:40 +02:00
|
|
|
|
|
|
|
class MCU_endstop:
|
2016-12-09 00:12:20 +01:00
|
|
|
error = error
|
2016-05-25 17:37:40 +02:00
|
|
|
RETRY_QUERY = 1.000
|
|
|
|
def __init__(self, mcu, pin, stepper):
|
|
|
|
self._mcu = mcu
|
|
|
|
self._oid = mcu.create_oid()
|
|
|
|
self._stepper = stepper
|
|
|
|
stepper_oid = stepper.get_oid()
|
|
|
|
pin, pullup, self._invert = parse_pin_extras(pin, can_pullup=True)
|
|
|
|
self._cmd_queue = mcu.alloc_command_queue()
|
|
|
|
mcu.add_config_cmd(
|
|
|
|
"config_end_stop oid=%d pin=%s pull_up=%d stepper_oid=%d" % (
|
|
|
|
self._oid, pin, pullup, stepper_oid))
|
|
|
|
self._home_cmd = mcu.lookup_command(
|
|
|
|
"end_stop_home oid=%c clock=%u rest_ticks=%u pin_value=%c")
|
|
|
|
mcu.register_msg(self._handle_end_stop_state, "end_stop_state"
|
|
|
|
, self._oid)
|
|
|
|
self._query_cmd = mcu.lookup_command("end_stop_query oid=%c")
|
|
|
|
self._homing = False
|
2016-10-11 05:41:40 +02:00
|
|
|
self._min_query_time = 0.
|
2016-11-18 19:03:40 +01:00
|
|
|
self._next_query_clock = self._home_timeout_clock = 0
|
2016-08-24 22:42:25 +02:00
|
|
|
self._mcu_freq = mcu.get_mcu_freq()
|
2016-10-11 05:41:40 +02:00
|
|
|
self._retry_query_ticks = int(self._mcu_freq * self.RETRY_QUERY)
|
|
|
|
self._last_state = {}
|
2016-08-24 22:42:25 +02:00
|
|
|
self.print_to_mcu_time = mcu.print_to_mcu_time
|
2016-12-09 00:12:20 +01:00
|
|
|
def home_start(self, mcu_time, rest_time):
|
2016-08-24 22:42:25 +02:00
|
|
|
clock = int(mcu_time * self._mcu_freq)
|
|
|
|
rest_ticks = int(rest_time * self._mcu_freq)
|
2016-05-25 17:37:40 +02:00
|
|
|
self._homing = True
|
2017-02-06 19:31:34 +01:00
|
|
|
self._min_query_time = self._mcu.monotonic()
|
2016-10-11 05:41:40 +02:00
|
|
|
self._next_query_clock = clock + self._retry_query_ticks
|
2016-05-25 17:37:40 +02:00
|
|
|
msg = self._home_cmd.encode(
|
|
|
|
self._oid, clock, rest_ticks, 1 ^ self._invert)
|
|
|
|
self._mcu.send(msg, reqclock=clock, cq=self._cmd_queue)
|
2016-12-30 23:02:28 +01:00
|
|
|
self._stepper.note_homing_start(clock)
|
2016-11-18 19:03:40 +01:00
|
|
|
def home_finalize(self, mcu_time):
|
2016-12-30 23:02:28 +01:00
|
|
|
self._stepper.note_homing_finalized()
|
2016-11-18 19:03:40 +01:00
|
|
|
self._home_timeout_clock = int(mcu_time * self._mcu_freq)
|
2016-12-09 00:12:20 +01:00
|
|
|
def home_wait(self):
|
2017-02-06 19:31:34 +01:00
|
|
|
eventtime = self._mcu.monotonic()
|
2016-12-09 00:12:20 +01:00
|
|
|
while self._check_busy(eventtime):
|
|
|
|
eventtime = self._mcu.pause(eventtime + 0.1)
|
2016-05-25 17:37:40 +02:00
|
|
|
def _handle_end_stop_state(self, params):
|
|
|
|
logging.debug("end_stop_state %s" % (params,))
|
2016-09-22 17:09:20 +02:00
|
|
|
self._last_state = params
|
2016-12-09 00:12:20 +01:00
|
|
|
def _check_busy(self, eventtime):
|
2016-09-22 17:09:20 +02:00
|
|
|
# Check if need to send an end_stop_query command
|
2016-11-23 22:18:08 +01:00
|
|
|
if self._mcu.is_fileoutput():
|
2016-05-25 17:37:40 +02:00
|
|
|
return False
|
2016-11-18 19:03:40 +01:00
|
|
|
last_sent_time = self._last_state.get('#sent_time', -1.)
|
|
|
|
if last_sent_time >= self._min_query_time:
|
2016-11-28 17:22:01 +01:00
|
|
|
if not self._homing:
|
|
|
|
return False
|
|
|
|
if not self._last_state.get('homing', 0):
|
2016-12-09 00:12:20 +01:00
|
|
|
pos = self._last_state.get('pos', 0)
|
|
|
|
if self._stepper.get_invert_dir():
|
|
|
|
pos = -pos
|
|
|
|
self._stepper.set_mcu_position(pos)
|
2016-11-28 17:22:01 +01:00
|
|
|
self._homing = False
|
2016-09-22 17:09:20 +02:00
|
|
|
return False
|
2016-11-18 19:03:40 +01:00
|
|
|
if (self._mcu.serial.get_clock(last_sent_time)
|
|
|
|
> self._home_timeout_clock):
|
|
|
|
# Timeout - disable endstop checking
|
|
|
|
msg = self._home_cmd.encode(self._oid, 0, 0, 0)
|
|
|
|
self._mcu.send(msg, reqclock=0, cq=self._cmd_queue)
|
2016-11-30 00:27:48 +01:00
|
|
|
raise error("Timeout during endstop homing")
|
2016-11-18 19:03:40 +01:00
|
|
|
if self._mcu.is_shutdown:
|
2016-11-30 00:27:48 +01:00
|
|
|
raise error("MCU is shutdown")
|
2016-12-01 01:31:46 +01:00
|
|
|
last_clock, last_clock_time = self._mcu.get_last_clock()
|
2016-10-11 05:41:40 +02:00
|
|
|
if last_clock >= self._next_query_clock:
|
|
|
|
self._next_query_clock = last_clock + self._retry_query_ticks
|
2016-05-25 17:37:40 +02:00
|
|
|
msg = self._query_cmd.encode(self._oid)
|
|
|
|
self._mcu.send(msg, cq=self._cmd_queue)
|
2016-09-22 17:09:20 +02:00
|
|
|
return True
|
2016-11-17 23:24:03 +01:00
|
|
|
def query_endstop(self, mcu_time):
|
|
|
|
clock = int(mcu_time * self._mcu_freq)
|
2016-09-22 17:09:20 +02:00
|
|
|
self._homing = False
|
2017-02-06 19:31:34 +01:00
|
|
|
self._min_query_time = self._mcu.monotonic()
|
2016-11-17 23:24:03 +01:00
|
|
|
self._next_query_clock = clock
|
2016-12-08 18:42:12 +01:00
|
|
|
def query_endstop_wait(self):
|
2017-02-06 19:31:34 +01:00
|
|
|
eventtime = self._mcu.monotonic()
|
2016-12-09 00:12:20 +01:00
|
|
|
while self._check_busy(eventtime):
|
2016-12-08 18:42:12 +01:00
|
|
|
eventtime = self._mcu.pause(eventtime + 0.1)
|
2016-09-22 17:09:20 +02:00
|
|
|
return self._last_state.get('pin', self._invert) ^ self._invert
|
2016-05-25 17:37:40 +02:00
|
|
|
|
|
|
|
class MCU_digital_out:
|
|
|
|
def __init__(self, mcu, pin, max_duration):
|
|
|
|
self._mcu = mcu
|
|
|
|
self._oid = mcu.create_oid()
|
|
|
|
pin, pullup, self._invert = parse_pin_extras(pin)
|
|
|
|
self._last_clock = 0
|
|
|
|
self._last_value = None
|
2016-08-24 21:16:02 +02:00
|
|
|
self._mcu_freq = mcu.get_mcu_freq()
|
2016-05-25 17:37:40 +02:00
|
|
|
self._cmd_queue = mcu.alloc_command_queue()
|
|
|
|
mcu.add_config_cmd(
|
|
|
|
"config_digital_out oid=%d pin=%s default_value=%d"
|
|
|
|
" max_duration=%d" % (self._oid, pin, self._invert, max_duration))
|
|
|
|
self._set_cmd = mcu.lookup_command(
|
|
|
|
"schedule_digital_out oid=%c clock=%u value=%c")
|
2016-08-24 22:42:25 +02:00
|
|
|
self.print_to_mcu_time = mcu.print_to_mcu_time
|
|
|
|
def set_digital(self, mcu_time, value):
|
|
|
|
clock = int(mcu_time * self._mcu_freq)
|
2016-05-25 17:37:40 +02:00
|
|
|
msg = self._set_cmd.encode(self._oid, clock, value ^ self._invert)
|
|
|
|
self._mcu.send(msg, minclock=self._last_clock, reqclock=clock
|
|
|
|
, cq=self._cmd_queue)
|
|
|
|
self._last_clock = clock
|
|
|
|
self._last_value = value
|
|
|
|
def get_last_setting(self):
|
|
|
|
return self._last_value
|
2016-08-24 21:16:02 +02:00
|
|
|
def set_pwm(self, mcu_time, value):
|
2016-05-25 17:37:40 +02:00
|
|
|
dval = 0
|
2016-08-25 17:24:18 +02:00
|
|
|
if value >= 0.5:
|
2016-05-25 17:37:40 +02:00
|
|
|
dval = 1
|
2016-08-24 22:42:25 +02:00
|
|
|
self.set_digital(mcu_time, dval)
|
2016-05-25 17:37:40 +02:00
|
|
|
|
|
|
|
class MCU_pwm:
|
2016-08-25 17:24:18 +02:00
|
|
|
PWM_MAX = 255.
|
2017-03-09 02:00:27 +01:00
|
|
|
def __init__(self, mcu, pin, cycle_ticks, max_duration, hard_pwm=False):
|
2016-05-25 17:37:40 +02:00
|
|
|
self._mcu = mcu
|
|
|
|
self._oid = mcu.create_oid()
|
2017-03-09 06:44:25 +01:00
|
|
|
pin, pullup, self._invert = parse_pin_extras(pin)
|
2016-05-25 17:37:40 +02:00
|
|
|
self._last_clock = 0
|
2016-08-24 21:16:02 +02:00
|
|
|
self._mcu_freq = mcu.get_mcu_freq()
|
2016-05-25 17:37:40 +02:00
|
|
|
self._cmd_queue = mcu.alloc_command_queue()
|
|
|
|
if hard_pwm:
|
|
|
|
mcu.add_config_cmd(
|
2017-03-09 06:44:25 +01:00
|
|
|
"config_pwm_out oid=%d pin=%s cycle_ticks=%d default_value=%d"
|
|
|
|
" max_duration=%d" % (
|
|
|
|
self._oid, pin, cycle_ticks, self._invert, max_duration))
|
2016-05-25 17:37:40 +02:00
|
|
|
self._set_cmd = mcu.lookup_command(
|
|
|
|
"schedule_pwm_out oid=%c clock=%u value=%c")
|
|
|
|
else:
|
|
|
|
mcu.add_config_cmd(
|
|
|
|
"config_soft_pwm_out oid=%d pin=%s cycle_ticks=%d"
|
2017-03-09 06:44:25 +01:00
|
|
|
" default_value=%d max_duration=%d" % (
|
|
|
|
self._oid, pin, cycle_ticks, self._invert, max_duration))
|
2016-05-25 17:37:40 +02:00
|
|
|
self._set_cmd = mcu.lookup_command(
|
|
|
|
"schedule_soft_pwm_out oid=%c clock=%u value=%c")
|
2016-08-24 21:16:02 +02:00
|
|
|
self.print_to_mcu_time = mcu.print_to_mcu_time
|
|
|
|
def set_pwm(self, mcu_time, value):
|
|
|
|
clock = int(mcu_time * self._mcu_freq)
|
2017-03-09 06:44:25 +01:00
|
|
|
if self._invert:
|
|
|
|
value = 1. - value
|
2016-08-25 17:24:18 +02:00
|
|
|
value = int(value * self.PWM_MAX + 0.5)
|
2016-05-25 17:37:40 +02:00
|
|
|
msg = self._set_cmd.encode(self._oid, clock, value)
|
|
|
|
self._mcu.send(msg, minclock=self._last_clock, reqclock=clock
|
|
|
|
, cq=self._cmd_queue)
|
|
|
|
self._last_clock = clock
|
|
|
|
|
|
|
|
class MCU_adc:
|
|
|
|
def __init__(self, mcu, pin):
|
|
|
|
self._mcu = mcu
|
|
|
|
self._oid = mcu.create_oid()
|
|
|
|
self._min_sample = 0
|
|
|
|
self._max_sample = 0xffff
|
|
|
|
self._sample_ticks = 0
|
|
|
|
self._sample_count = 1
|
|
|
|
self._report_clock = 0
|
|
|
|
self._callback = None
|
2016-08-24 21:16:02 +02:00
|
|
|
self._inv_max_adc = 0.
|
|
|
|
self._mcu_freq = mcu.get_mcu_freq()
|
2016-05-25 17:37:40 +02:00
|
|
|
self._cmd_queue = mcu.alloc_command_queue()
|
|
|
|
mcu.add_config_cmd("config_analog_in oid=%d pin=%s" % (self._oid, pin))
|
2016-11-22 18:37:37 +01:00
|
|
|
mcu.add_init_callback(self._init_callback)
|
2016-05-25 17:37:40 +02:00
|
|
|
mcu.register_msg(self._handle_analog_in_state, "analog_in_state"
|
|
|
|
, self._oid)
|
|
|
|
self._query_cmd = mcu.lookup_command(
|
|
|
|
"query_analog_in oid=%c clock=%u sample_ticks=%u sample_count=%c"
|
|
|
|
" rest_ticks=%u min_value=%hu max_value=%hu")
|
2016-08-24 21:16:02 +02:00
|
|
|
def set_minmax(self, sample_time, sample_count, minval=None, maxval=None):
|
|
|
|
self._sample_ticks = int(sample_time * self._mcu_freq)
|
|
|
|
self._sample_count = sample_count
|
2016-05-25 17:37:40 +02:00
|
|
|
if minval is None:
|
|
|
|
minval = 0
|
|
|
|
if maxval is None:
|
|
|
|
maxval = 0xffff
|
2017-01-10 05:50:13 +01:00
|
|
|
mcu_adc_max = self._mcu.serial.msgparser.get_constant_float("ADC_MAX")
|
2016-12-23 05:58:51 +01:00
|
|
|
max_adc = sample_count * mcu_adc_max
|
2016-05-25 17:37:40 +02:00
|
|
|
self._min_sample = int(minval * max_adc)
|
|
|
|
self._max_sample = min(0xffff, int(math.ceil(maxval * max_adc)))
|
2016-08-24 21:16:02 +02:00
|
|
|
self._inv_max_adc = 1.0 / max_adc
|
2016-11-22 18:37:37 +01:00
|
|
|
def _init_callback(self):
|
2016-12-01 01:31:46 +01:00
|
|
|
last_clock, last_clock_time = self._mcu.get_last_clock()
|
|
|
|
clock = last_clock + int(self._mcu_freq * (1.0 + self._oid * 0.01)) # XXX
|
2016-05-25 17:37:40 +02:00
|
|
|
msg = self._query_cmd.encode(
|
|
|
|
self._oid, clock, self._sample_ticks, self._sample_count
|
2016-08-24 21:16:02 +02:00
|
|
|
, self._report_clock, self._min_sample, self._max_sample)
|
2016-05-25 17:37:40 +02:00
|
|
|
self._mcu.send(msg, reqclock=clock, cq=self._cmd_queue)
|
|
|
|
def _handle_analog_in_state(self, params):
|
2016-08-24 21:16:02 +02:00
|
|
|
last_value = params['value'] * self._inv_max_adc
|
2016-05-25 17:37:40 +02:00
|
|
|
next_clock = self._mcu.serial.translate_clock(params['next_clock'])
|
2016-08-24 21:16:02 +02:00
|
|
|
last_read_time = (next_clock - self._report_clock) / self._mcu_freq
|
2016-05-25 17:37:40 +02:00
|
|
|
if self._callback is not None:
|
2016-08-24 21:16:02 +02:00
|
|
|
self._callback(last_read_time, last_value)
|
2016-11-22 18:37:37 +01:00
|
|
|
def set_adc_callback(self, report_time, callback):
|
|
|
|
self._report_clock = int(report_time * self._mcu_freq)
|
|
|
|
self._callback = callback
|
2016-05-25 17:37:40 +02:00
|
|
|
|
|
|
|
class MCU:
|
2016-12-08 18:42:12 +01:00
|
|
|
error = error
|
2016-12-01 01:31:46 +01:00
|
|
|
COMM_TIMEOUT = 3.5
|
2016-05-25 17:37:40 +02:00
|
|
|
def __init__(self, printer, config):
|
|
|
|
self._printer = printer
|
|
|
|
self._config = config
|
|
|
|
# Serial port
|
2017-01-02 18:22:43 +01:00
|
|
|
baud = config.getint('baud', 250000)
|
2017-03-09 04:26:10 +01:00
|
|
|
self._serialport = config.get('serial', '/dev/ttyS0')
|
|
|
|
self.serial = serialhdl.SerialReader(
|
|
|
|
printer.reactor, self._serialport, baud)
|
2016-05-25 17:37:40 +02:00
|
|
|
self.is_shutdown = False
|
2017-03-09 03:42:51 +01:00
|
|
|
self._shutdown_msg = ""
|
2016-11-23 22:18:08 +01:00
|
|
|
self._is_fileoutput = False
|
2016-12-01 01:31:46 +01:00
|
|
|
self._timeout_timer = printer.reactor.register_timer(
|
|
|
|
self.timeout_handler)
|
2016-05-25 17:37:40 +02:00
|
|
|
# Config building
|
2016-12-01 06:08:35 +01:00
|
|
|
self._emergency_stop_cmd = self._clear_shutdown_cmd = None
|
2016-05-25 17:37:40 +02:00
|
|
|
self._num_oids = 0
|
|
|
|
self._config_cmds = []
|
|
|
|
self._config_crc = None
|
2016-11-22 18:37:37 +01:00
|
|
|
self._init_callbacks = []
|
2016-05-25 17:37:40 +02:00
|
|
|
# Move command queuing
|
|
|
|
ffi_main, self.ffi_lib = chelper.get_ffi()
|
|
|
|
self._steppers = []
|
|
|
|
self._steppersync = None
|
|
|
|
# Print time to clock epoch calculations
|
2016-08-25 18:07:08 +02:00
|
|
|
self._print_start_time = 0.
|
2016-08-25 18:09:37 +02:00
|
|
|
self._mcu_freq = 0.
|
2016-05-25 17:37:40 +02:00
|
|
|
# Stats
|
2017-01-10 05:08:23 +01:00
|
|
|
self._stats_sumsq_base = 0.
|
2016-05-25 17:37:40 +02:00
|
|
|
self._mcu_tick_avg = 0.
|
|
|
|
self._mcu_tick_stddev = 0.
|
|
|
|
def handle_mcu_stats(self, params):
|
|
|
|
logging.debug("mcu stats: %s" % (params,))
|
|
|
|
count = params['count']
|
|
|
|
tick_sum = params['sum']
|
2016-08-25 18:09:37 +02:00
|
|
|
c = 1.0 / (count * self._mcu_freq)
|
2016-05-25 17:37:40 +02:00
|
|
|
self._mcu_tick_avg = tick_sum * c
|
2017-01-10 05:08:23 +01:00
|
|
|
tick_sumsq = params['sumsq'] * self._stats_sumsq_base
|
|
|
|
self._mcu_tick_stddev = c * math.sqrt(count*tick_sumsq - tick_sum**2)
|
2016-05-25 17:37:40 +02:00
|
|
|
def handle_shutdown(self, params):
|
|
|
|
if self.is_shutdown:
|
|
|
|
return
|
|
|
|
self.is_shutdown = True
|
2017-03-09 03:42:51 +01:00
|
|
|
self._shutdown_msg = params['#msg']
|
|
|
|
logging.info("%s: %s" % (params['#name'], self._shutdown_msg))
|
2016-05-25 17:37:40 +02:00
|
|
|
self.serial.dump_debug()
|
2017-03-09 03:42:51 +01:00
|
|
|
self._printer.note_shutdown(self._shutdown_msg)
|
2016-05-25 17:37:40 +02:00
|
|
|
# Connection phase
|
|
|
|
def connect(self):
|
2016-11-27 23:45:58 +01:00
|
|
|
if not self._is_fileoutput:
|
|
|
|
self.serial.connect()
|
2016-12-01 01:31:46 +01:00
|
|
|
self._printer.reactor.update_timer(
|
2017-02-06 19:31:34 +01:00
|
|
|
self._timeout_timer, self.monotonic() + self.COMM_TIMEOUT)
|
2017-01-10 05:50:13 +01:00
|
|
|
self._mcu_freq = self.serial.msgparser.get_constant_float('CLOCK_FREQ')
|
|
|
|
self._stats_sumsq_base = self.serial.msgparser.get_constant_float(
|
|
|
|
'STATS_SUMSQ_BASE')
|
2016-12-01 01:31:46 +01:00
|
|
|
self._emergency_stop_cmd = self.lookup_command("emergency_stop")
|
2016-12-01 06:08:35 +01:00
|
|
|
self._clear_shutdown_cmd = self.lookup_command("clear_shutdown")
|
2016-05-25 17:37:40 +02:00
|
|
|
self.register_msg(self.handle_shutdown, 'shutdown')
|
|
|
|
self.register_msg(self.handle_shutdown, 'is_shutdown')
|
|
|
|
self.register_msg(self.handle_mcu_stats, 'stats')
|
|
|
|
def connect_file(self, debugoutput, dictionary, pace=False):
|
2016-11-23 22:18:08 +01:00
|
|
|
self._is_fileoutput = True
|
2016-05-25 17:37:40 +02:00
|
|
|
self.serial.connect_file(debugoutput, dictionary)
|
|
|
|
if not pace:
|
|
|
|
def dummy_set_print_start_time(eventtime):
|
|
|
|
pass
|
|
|
|
def dummy_get_print_buffer_time(eventtime, last_move_end):
|
2017-02-11 20:43:36 +01:00
|
|
|
return 1.250
|
2016-05-25 17:37:40 +02:00
|
|
|
self.set_print_start_time = dummy_set_print_start_time
|
|
|
|
self.get_print_buffer_time = dummy_get_print_buffer_time
|
2016-12-01 01:31:46 +01:00
|
|
|
def timeout_handler(self, eventtime):
|
|
|
|
last_clock, last_clock_time = self.serial.get_last_clock()
|
|
|
|
timeout = last_clock_time + self.COMM_TIMEOUT
|
|
|
|
if eventtime < timeout:
|
|
|
|
return timeout
|
|
|
|
logging.info("Timeout with firmware (eventtime=%f last_status=%f)" % (
|
|
|
|
eventtime, last_clock_time))
|
|
|
|
self._printer.note_mcu_error("Lost communication with firmware")
|
|
|
|
return self._printer.reactor.NEVER
|
2016-05-25 17:37:40 +02:00
|
|
|
def disconnect(self):
|
|
|
|
self.serial.disconnect()
|
2016-11-30 07:58:45 +01:00
|
|
|
if self._steppersync is not None:
|
|
|
|
self.ffi_lib.steppersync_free(self._steppersync)
|
|
|
|
self._steppersync = None
|
2016-05-25 17:37:40 +02:00
|
|
|
def stats(self, eventtime):
|
2017-02-06 17:37:03 +01:00
|
|
|
return "%s mcu_task_avg=%.06f mcu_task_stddev=%.06f" % (
|
|
|
|
self.serial.stats(eventtime),
|
2016-05-25 17:37:40 +02:00
|
|
|
self._mcu_tick_avg, self._mcu_tick_stddev)
|
2016-09-19 19:18:55 +02:00
|
|
|
def force_shutdown(self):
|
|
|
|
self.send(self._emergency_stop_cmd.encode())
|
2016-12-01 06:08:35 +01:00
|
|
|
def clear_shutdown(self):
|
|
|
|
logging.info("Sending clear_shutdown command")
|
|
|
|
self.send(self._clear_shutdown_cmd.encode())
|
2017-03-09 04:26:10 +01:00
|
|
|
def microcontroller_restart(self):
|
|
|
|
logging.info("Attempting a microcontroller reset")
|
|
|
|
self.disconnect()
|
|
|
|
serialhdl.arduino_reset(self._serialport, self._printer.reactor)
|
2016-11-23 22:18:08 +01:00
|
|
|
def is_fileoutput(self):
|
|
|
|
return self._is_fileoutput
|
2016-05-25 17:37:40 +02:00
|
|
|
# Configuration phase
|
|
|
|
def _add_custom(self):
|
|
|
|
data = self._config.get('custom', '')
|
|
|
|
for line in data.split('\n'):
|
|
|
|
line = line.strip()
|
|
|
|
cpos = line.find('#')
|
|
|
|
if cpos >= 0:
|
|
|
|
line = line[:cpos].strip()
|
|
|
|
if not line:
|
|
|
|
continue
|
|
|
|
self.add_config_cmd(line)
|
|
|
|
def build_config(self):
|
|
|
|
# Build config commands
|
|
|
|
self._add_custom()
|
|
|
|
self._config_cmds.insert(0, "allocate_oids count=%d" % (
|
|
|
|
self._num_oids,))
|
|
|
|
|
|
|
|
# Resolve pin names
|
2017-01-10 05:50:13 +01:00
|
|
|
mcu = self.serial.msgparser.get_constant('MCU')
|
2016-11-30 21:39:36 +01:00
|
|
|
pin_map = self._config.get('pin_map', None)
|
2016-05-25 17:37:40 +02:00
|
|
|
if pin_map is None:
|
|
|
|
pnames = pins.mcu_to_pins(mcu)
|
|
|
|
else:
|
|
|
|
pnames = pins.map_pins(pin_map, mcu)
|
2016-11-30 21:05:26 +01:00
|
|
|
updated_cmds = []
|
|
|
|
for cmd in self._config_cmds:
|
|
|
|
try:
|
|
|
|
updated_cmds.append(pins.update_command(cmd, pnames))
|
|
|
|
except:
|
|
|
|
raise self._config.error("Unable to translate pin name: %s" % (
|
|
|
|
cmd,))
|
|
|
|
self._config_cmds = updated_cmds
|
2016-05-25 17:37:40 +02:00
|
|
|
|
|
|
|
# Calculate config CRC
|
|
|
|
self._config_crc = zlib.crc32('\n'.join(self._config_cmds)) & 0xffffffff
|
|
|
|
self.add_config_cmd("finalize_config crc=%d" % (self._config_crc,))
|
|
|
|
|
|
|
|
self._send_config()
|
|
|
|
def _send_config(self):
|
|
|
|
msg = self.create_command("get_config")
|
2016-11-28 20:52:22 +01:00
|
|
|
if self._is_fileoutput:
|
|
|
|
config_params = {
|
|
|
|
'is_config': 0, 'move_count': 500, 'crc': self._config_crc}
|
|
|
|
else:
|
|
|
|
config_params = self.serial.send_with_response(msg, 'config')
|
|
|
|
if not config_params['is_config']:
|
|
|
|
# Send config commands
|
2017-03-09 04:26:10 +01:00
|
|
|
logging.info("Sending printer configuration...")
|
2016-11-28 20:52:22 +01:00
|
|
|
for c in self._config_cmds:
|
|
|
|
self.send(self.create_command(c))
|
|
|
|
if not self._is_fileoutput:
|
|
|
|
config_params = self.serial.send_with_response(msg, 'config')
|
2017-03-09 03:42:51 +01:00
|
|
|
if not config_params['is_config']:
|
|
|
|
if self.is_shutdown:
|
|
|
|
raise error("Firmware error during config: %s" % (
|
|
|
|
self._shutdown_msg,))
|
|
|
|
raise error("Unable to configure printer")
|
2016-11-28 20:52:22 +01:00
|
|
|
if self._config_crc != config_params['crc']:
|
2017-03-09 04:26:10 +01:00
|
|
|
if self._printer.get_startup_state() != 'firmware_restart':
|
|
|
|
# Attempt a firmware restart to fix the CRC error
|
|
|
|
logging.info(
|
|
|
|
"Printer CRC mismatch - attempting firmware restart")
|
|
|
|
self._printer.request_exit('firmware_restart')
|
|
|
|
self._printer.reactor.pause(0.100)
|
2016-11-30 20:30:45 +01:00
|
|
|
raise error("Printer CRC does not match config")
|
2016-12-24 18:33:56 +01:00
|
|
|
move_count = config_params['move_count']
|
|
|
|
logging.info("Configured (%d moves)" % (move_count,))
|
2016-11-28 20:52:22 +01:00
|
|
|
stepqueues = tuple(s._stepqueue for s in self._steppers)
|
|
|
|
self._steppersync = self.ffi_lib.steppersync_alloc(
|
2016-12-24 18:33:56 +01:00
|
|
|
self.serial.serialqueue, stepqueues, len(stepqueues), move_count)
|
2016-11-22 18:37:37 +01:00
|
|
|
for cb in self._init_callbacks:
|
|
|
|
cb()
|
2016-05-25 17:37:40 +02:00
|
|
|
# Config creation helpers
|
|
|
|
def create_oid(self):
|
|
|
|
oid = self._num_oids
|
|
|
|
self._num_oids += 1
|
|
|
|
return oid
|
|
|
|
def add_config_cmd(self, cmd):
|
|
|
|
self._config_cmds.append(cmd)
|
2016-11-22 18:37:37 +01:00
|
|
|
def add_init_callback(self, callback):
|
|
|
|
self._init_callbacks.append(callback)
|
2016-05-25 17:37:40 +02:00
|
|
|
def register_msg(self, cb, msg, oid=None):
|
|
|
|
self.serial.register_callback(cb, msg, oid)
|
|
|
|
def register_stepper(self, stepper):
|
|
|
|
self._steppers.append(stepper)
|
|
|
|
def alloc_command_queue(self):
|
|
|
|
return self.serial.alloc_command_queue()
|
|
|
|
def lookup_command(self, msgformat):
|
|
|
|
return self.serial.msgparser.lookup_command(msgformat)
|
|
|
|
def create_command(self, msg):
|
|
|
|
return self.serial.msgparser.create_command(msg)
|
|
|
|
# Wrappers for mcu object creation
|
|
|
|
def create_stepper(self, step_pin, dir_pin, min_stop_interval, max_error):
|
|
|
|
return MCU_stepper(self, step_pin, dir_pin, min_stop_interval, max_error)
|
|
|
|
def create_endstop(self, pin, stepper):
|
|
|
|
return MCU_endstop(self, pin, stepper)
|
|
|
|
def create_digital_out(self, pin, max_duration=2.):
|
2016-08-25 18:09:37 +02:00
|
|
|
max_duration = int(max_duration * self._mcu_freq)
|
2016-05-25 17:37:40 +02:00
|
|
|
return MCU_digital_out(self, pin, max_duration)
|
2017-03-09 02:00:27 +01:00
|
|
|
def create_pwm(self, pin, cycle_time, hard_cycle_ticks=0, max_duration=2.):
|
2016-08-25 18:09:37 +02:00
|
|
|
max_duration = int(max_duration * self._mcu_freq)
|
2016-05-25 17:37:40 +02:00
|
|
|
if hard_cycle_ticks:
|
2017-03-09 02:00:27 +01:00
|
|
|
return MCU_pwm(self, pin, hard_cycle_ticks, max_duration, True)
|
2016-05-25 17:37:40 +02:00
|
|
|
if hard_cycle_ticks < 0:
|
|
|
|
return MCU_digital_out(self, pin, max_duration)
|
2017-03-09 02:00:27 +01:00
|
|
|
cycle_ticks = int(cycle_time * self._mcu_freq)
|
|
|
|
return MCU_pwm(self, pin, cycle_ticks, max_duration, False)
|
2016-05-25 17:37:40 +02:00
|
|
|
def create_adc(self, pin):
|
|
|
|
return MCU_adc(self, pin)
|
|
|
|
# Clock syncing
|
|
|
|
def set_print_start_time(self, eventtime):
|
2016-08-25 18:09:37 +02:00
|
|
|
est_mcu_time = self.serial.get_clock(eventtime) / self._mcu_freq
|
2016-08-25 18:07:08 +02:00
|
|
|
self._print_start_time = est_mcu_time
|
|
|
|
def get_print_buffer_time(self, eventtime, print_time):
|
2016-12-09 15:41:46 +01:00
|
|
|
if self.is_shutdown:
|
|
|
|
return 0.
|
2016-08-25 18:07:08 +02:00
|
|
|
mcu_time = print_time + self._print_start_time
|
2016-08-25 18:09:37 +02:00
|
|
|
est_mcu_time = self.serial.get_clock(eventtime) / self._mcu_freq
|
2016-08-25 18:07:08 +02:00
|
|
|
return mcu_time - est_mcu_time
|
2016-08-24 21:16:02 +02:00
|
|
|
def print_to_mcu_time(self, print_time):
|
2016-08-25 18:07:08 +02:00
|
|
|
return print_time + self._print_start_time
|
2016-05-25 17:37:40 +02:00
|
|
|
def get_mcu_freq(self):
|
2016-08-25 18:09:37 +02:00
|
|
|
return self._mcu_freq
|
2016-05-25 17:37:40 +02:00
|
|
|
def get_last_clock(self):
|
|
|
|
return self.serial.get_last_clock()
|
|
|
|
# Move command queuing
|
|
|
|
def send(self, cmd, minclock=0, reqclock=0, cq=None):
|
|
|
|
self.serial.send(cmd, minclock, reqclock, cq=cq)
|
|
|
|
def flush_moves(self, print_time):
|
2016-12-29 04:17:49 +01:00
|
|
|
if self._steppersync is None:
|
|
|
|
return
|
2016-08-25 18:07:08 +02:00
|
|
|
mcu_time = print_time + self._print_start_time
|
2016-08-25 18:09:37 +02:00
|
|
|
clock = int(mcu_time * self._mcu_freq)
|
2017-02-06 17:37:03 +01:00
|
|
|
ret = self.ffi_lib.steppersync_flush(self._steppersync, clock)
|
|
|
|
if ret:
|
|
|
|
raise error("Internal error in stepcompress")
|
2016-12-08 18:42:12 +01:00
|
|
|
def pause(self, waketime):
|
|
|
|
return self._printer.reactor.pause(waketime)
|
2017-02-06 19:31:34 +01:00
|
|
|
def monotonic(self):
|
|
|
|
return self._printer.reactor.monotonic()
|
2016-11-30 07:58:45 +01:00
|
|
|
def __del__(self):
|
|
|
|
self.disconnect()
|