From 74937326d313ae741c42207035ebcf8c09aa9e01 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Fri, 9 Jul 2021 22:04:10 -0400 Subject: [PATCH] sensor_angle: Add support for bulk querying of spi angle sensors Signed-off-by: Kevin O'Connor --- docs/API_Server.md | 18 +++ docs/Config_Reference.md | 27 ++++ klippy/extras/angle.py | 188 ++++++++++++++++++++++++++ src/Makefile | 2 +- src/sensor_angle.c | 276 +++++++++++++++++++++++++++++++++++++++ src/spicmds.c | 12 ++ src/spicmds.h | 2 + 7 files changed, 524 insertions(+), 1 deletion(-) create mode 100644 klippy/extras/angle.py create mode 100644 src/sensor_angle.c diff --git a/docs/API_Server.md b/docs/API_Server.md index 562ad57e..8c3e966e 100644 --- a/docs/API_Server.md +++ b/docs/API_Server.md @@ -341,6 +341,24 @@ and might later produce asynchronous messages such as: The "header" field in the initial query response is used to describe the fields found in later "data" responses. +### angle/dump_angle + +This endpoint is used to subscribe to +[angle sensor data](Config_Reference.md#angle). Obtaining these +low-level motion updates may be useful for diagnostic and debugging +purposes. Using this endpoint may increase Klipper's system load. + +A request may look like: +`{"id": 123, "method":"angle/dump_angle", +"params": {"sensor": "my_angle_sensor", "response_template": {}}}` +and might return: +`{"id": 123,"result":{"header":["time","angle"]}}` +and might later produce asynchronous messages such as: +`{"params":{"errors":0,"data":[[1290.951905,-5063],[1290.952321,-5065]]}}` + +The "header" field in the initial query response is used to describe +the fields found in later "data" responses. + ### pause_resume/cancel This endpoint is similar to running the "PRINT_CANCEL" G-Code command. diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index a0eecf01..0add9aed 100644 --- a/docs/Config_Reference.md +++ b/docs/Config_Reference.md @@ -4001,6 +4001,33 @@ serial: # Auto cancel print when ping varation is above this threshold ``` +### [angle] + +Magnetic hall angle sensor support for reading stepper motor angle +shaft measurements using a1333, as5047d, or tle5012b SPI chips. The +measurements are available via the [API Server](API_Server.md) and +[motion analysis tool](Debugging.md#motion-analysis-and-data-logging). + +``` +[angle my_angle_sensor] +sensor_type: +# The type of the magnetic hall sensor chip. Available choices are +# "a1333", "as5047d", and "tle5012b". This parameter must be +# specified. +#sample_period: 0.000400 +# The query period (in seconds) to use during measurements. The +# default is 0.000400 (which is 2500 samples per second). +cs_pin: +# The SPI enable pin for the sensor. This parameter must be provided. +#spi_speed: +#spi_bus: +#spi_software_sclk_pin: +#spi_software_mosi_pin: +#spi_software_miso_pin: +# See the "common SPI settings" section for a description of the +# above parameters. +``` + ## Common bus parameters ### Common SPI settings diff --git a/klippy/extras/angle.py b/klippy/extras/angle.py new file mode 100644 index 00000000..43d4fd1d --- /dev/null +++ b/klippy/extras/angle.py @@ -0,0 +1,188 @@ +# Support for reading SPI magnetic angle sensors +# +# Copyright (C) 2021,2022 Kevin O'Connor +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import logging, threading +from . import bus, motion_report + +MIN_MSG_TIME = 0.100 +TCODE_ERROR = 0xff + +class HelperA1333: + SPI_MODE = 3 + SPI_SPEED = 10000000 + def __init__(self, config, spi, oid): + self.spi = spi + def get_static_delay(self): + return .000001 + def start(self): + # Setup for angle query + self.spi.spi_transfer([0x32, 0x00]) + +class HelperAS5047D: + SPI_MODE = 1 + SPI_SPEED = int(1. / .000000350) + def __init__(self, config, spi, oid): + self.spi = spi + def get_static_delay(self): + return .000100 + def start(self): + # Clear any errors from device + self.spi.spi_transfer([0xff, 0xfc]) # Read DIAAGC + self.spi.spi_transfer([0x40, 0x01]) # Read ERRFL + self.spi.spi_transfer([0xc0, 0x00]) # Read NOP + +class HelperTLE5012B: + SPI_MODE = 1 + SPI_SPEED = 4000000 + def __init__(self, config, spi, oid): + self.spi = spi + def get_static_delay(self): + return .000042700 * 2.5 + def start(self): + # Clear any errors from device + self.spi.spi_transfer([0x80, 0x01, 0x00, 0x00, 0x00, 0x00]) # Read STAT + +SAMPLE_PERIOD = 0.000400 + +class Angle: + def __init__(self, config): + self.printer = config.get_printer() + self.sample_period = config.getfloat('sample_period', SAMPLE_PERIOD, + above=0.) + # Measurement conversion + self.start_clock = self.time_shift = self.sample_ticks = 0 + self.last_sequence = self.last_angle = 0 + # Measurement storage (accessed from background thread) + self.lock = threading.Lock() + self.raw_samples = [] + # Sensor type + sensors = { "a1333": HelperA1333, "as5047d": HelperAS5047D, + "tle5012b": HelperTLE5012B } + sensor_type = config.getchoice('sensor_type', {s: s for s in sensors}) + sensor_class = sensors[sensor_type] + self.spi = bus.MCU_SPI_from_config(config, sensor_class.SPI_MODE, + default_speed=sensor_class.SPI_SPEED) + self.mcu = mcu = self.spi.get_mcu() + self.oid = oid = mcu.create_oid() + self.sensor_helper = sensor_class(config, self.spi, oid) + # Setup mcu sensor_spi_angle bulk query code + self.query_spi_angle_cmd = self.query_spi_angle_end_cmd = None + mcu.add_config_cmd( + "config_spi_angle oid=%d spi_oid=%d spi_angle_type=%s" + % (oid, self.spi.get_oid(), sensor_type)) + mcu.add_config_cmd( + "query_spi_angle oid=%d clock=0 rest_ticks=0 time_shift=0" + % (oid,), on_restart=True) + mcu.register_config_callback(self._build_config) + mcu.register_response(self._handle_spi_angle_data, + "spi_angle_data", oid) + # API server endpoints + self.api_dump = motion_report.APIDumpHelper( + self.printer, self._api_update, self._api_startstop, 0.100) + self.name = config.get_name().split()[1] + wh = self.printer.lookup_object('webhooks') + wh.register_mux_endpoint("angle/dump_angle", "sensor", self.name, + self._handle_dump_angle) + def _build_config(self): + freq = self.mcu.seconds_to_clock(1.) + while float(TCODE_ERROR << self.time_shift) / freq < 0.002: + self.time_shift += 1 + cmdqueue = self.spi.get_command_queue() + self.query_spi_angle_cmd = self.mcu.lookup_command( + "query_spi_angle oid=%c clock=%u rest_ticks=%u time_shift=%c", + cq=cmdqueue) + self.query_spi_angle_end_cmd = self.mcu.lookup_query_command( + "query_spi_angle oid=%c clock=%u rest_ticks=%u time_shift=%c", + "spi_angle_end oid=%c sequence=%hu", oid=self.oid, cq=cmdqueue) + # Measurement collection + def is_measuring(self): + return self.start_clock != 0 + def _handle_spi_angle_data(self, params): + with self.lock: + self.raw_samples.append(params) + def _extract_samples(self, raw_samples): + # Load variables to optimize inner loop below + sample_ticks = self.sample_ticks + start_clock = self.start_clock + clock_to_print_time = self.mcu.clock_to_print_time + last_sequence = self.last_sequence + last_angle = self.last_angle + time_shift = self.time_shift + static_delay = self.sensor_helper.get_static_delay() + # Process every message in raw_samples + count = error_count = 0 + samples = [None] * (len(raw_samples) * 16) + for params in raw_samples: + seq = (last_sequence & ~0xffff) | params['sequence'] + if seq < last_sequence: + seq += 0x10000 + last_sequence = seq + d = bytearray(params['data']) + msg_mclock = start_clock + seq*16*sample_ticks + for i in range(len(d) // 3): + tcode = d[i*3] + if tcode == TCODE_ERROR: + error_count += 1 + continue + raw_angle = d[i*3 + 1] | (d[i*3 + 2] << 8) + angle_diff = (last_angle - raw_angle) & 0xffff + angle_diff -= (angle_diff & 0x8000) << 1 + last_angle -= angle_diff + mclock = msg_mclock + i*sample_ticks + (tcode< +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "basecmd.h" // oid_alloc +#include "board/misc.h" // timer_read_time +#include "board/gpio.h" // gpio_out_write +#include "board/irq.h" // irq_disable +#include "command.h" // DECL_COMMAND +#include "sched.h" // DECL_TASK +#include "spicmds.h" // spidev_transfer + +enum { SA_CHIP_A1333, SA_CHIP_AS5047D, SA_CHIP_TLE5012B, SA_CHIP_MAX }; + +DECL_ENUMERATION("spi_angle_type", "a1333", SA_CHIP_A1333); +DECL_ENUMERATION("spi_angle_type", "as5047d", SA_CHIP_AS5047D); +DECL_ENUMERATION("spi_angle_type", "tle5012b", SA_CHIP_TLE5012B); + +enum { TCODE_ERROR = 0xff }; +enum { + SE_OVERFLOW, SE_SCHEDULE, SE_SPI_TIME, SE_CRC, SE_DUP, SE_NO_ANGLE +}; + +#define MAX_SPI_READ_TIME timer_from_us(50) + +struct spi_angle { + struct timer timer; + uint32_t rest_ticks; + struct spidev_s *spi; + uint16_t sequence; + uint8_t flags, chip_type, data_count, time_shift, overflow; + uint8_t data[48]; +}; + +enum { + SA_PENDING = 1<<2, +}; + +static struct task_wake angle_wake; + +// Event handler that wakes spi_angle_task() periodically +static uint_fast8_t +angle_event(struct timer *timer) +{ + struct spi_angle *sa = container_of(timer, struct spi_angle, timer); + uint8_t flags = sa->flags; + if (sa->flags & SA_PENDING) + sa->overflow++; + else + sa->flags = flags | SA_PENDING; + sched_wake_task(&angle_wake); + sa->timer.waketime += sa->rest_ticks; + return SF_RESCHEDULE; +} + +void +command_config_spi_angle(uint32_t *args) +{ + uint8_t chip_type = args[2]; + if (chip_type > SA_CHIP_MAX) + shutdown("Invalid spi_angle chip type"); + struct spi_angle *sa = oid_alloc(args[0], command_config_spi_angle + , sizeof(*sa)); + sa->timer.func = angle_event; + sa->spi = spidev_oid_lookup(args[1]); + if (!spidev_have_cs_pin(sa->spi)) + shutdown("angle sensor requires cs pin"); + sa->chip_type = chip_type; +} +DECL_COMMAND(command_config_spi_angle, + "config_spi_angle oid=%c spi_oid=%c spi_angle_type=%c"); + +// Report local measurement buffer +static void +angle_report(struct spi_angle *sa, uint8_t oid) +{ + sendf("spi_angle_data oid=%c sequence=%hu data=%*s" + , oid, sa->sequence, sa->data_count, sa->data); + sa->data_count = 0; + sa->sequence++; +} + +// Send spi_angle_data message if buffer is full +static void +angle_check_report(struct spi_angle *sa, uint8_t oid) +{ + if (sa->data_count + 3 > ARRAY_SIZE(sa->data)) + angle_report(sa, oid); +} + +// Add an error indicator to the measurement buffer +static void +angle_add_error(struct spi_angle *sa, uint_fast8_t error_code) +{ + sa->data[sa->data_count] = TCODE_ERROR; + sa->data[sa->data_count + 1] = error_code; + sa->data[sa->data_count + 2] = 0; + sa->data_count += 3; +} + +// Add a measurement to the buffer +static void +angle_add_data(struct spi_angle *sa, uint32_t stime, uint32_t mtime + , uint_fast16_t angle) +{ + uint32_t tdiff = mtime - stime; + if (sa->time_shift) + tdiff = (tdiff + (1<<(sa->time_shift - 1))) >> sa->time_shift; + if (tdiff >= TCODE_ERROR) { + angle_add_error(sa, SE_SCHEDULE); + return; + } + sa->data[sa->data_count] = tdiff; + sa->data[sa->data_count + 1] = angle; + sa->data[sa->data_count + 2] = angle >> 8; + sa->data_count += 3; +} + +// a1333 sensor query +static void +a1333_query(struct spi_angle *sa, uint32_t stime) +{ + uint8_t msg[2] = { 0x32, 0x00 }; + uint32_t mtime1 = timer_read_time(); + spidev_transfer(sa->spi, 1, sizeof(msg), msg); + uint32_t mtime2 = timer_read_time(); + // Data is latched on first sclk edge of response + if (mtime2 - mtime1 > MAX_SPI_READ_TIME) + angle_add_error(sa, SE_SPI_TIME); + else if (msg[0] & 0x80) + angle_add_error(sa, SE_CRC); + else + angle_add_data(sa, stime, mtime1, (msg[0] << 9) | (msg[1] << 1)); +} + +// as5047d sensor query +static void +as5047d_query(struct spi_angle *sa, uint32_t stime) +{ + uint8_t msg[2] = { 0x7F, 0xFE }; + uint32_t mtime1 = timer_read_time(); + spidev_transfer(sa->spi, 0, sizeof(msg), msg); + uint32_t mtime2 = timer_read_time(); + // Data is latched on CS pin rising after query request + if (mtime2 - mtime1 > MAX_SPI_READ_TIME) { + angle_add_error(sa, SE_SPI_TIME); + return; + } + msg[0] = 0xC0; + msg[1] = 0x00; + spidev_transfer(sa->spi, 1, sizeof(msg), msg); + uint_fast8_t parity = msg[0] ^ msg[1]; + parity ^= parity >> 4; + parity ^= parity >> 2; + parity ^= parity >> 1; + if (parity & 1) + angle_add_error(sa, SE_CRC); + else if (msg[0] & 0x40) + angle_add_error(sa, SE_NO_ANGLE); + else + angle_add_data(sa, stime, mtime2, (msg[0] << 10) | (msg[1] << 2)); +} + +#define TLE_READ 0x80 +#define TLE_READ_LATCH (TLE_READ | 0x04) +#define TLE_REG_AVAL 0x02 + +// crc8 "J1850" calculation for tle5012b messages +static uint8_t +crc8(uint8_t crc, uint8_t data) +{ + crc ^= data; + int i; + for (i=0; i<8; i++) + crc = crc & 0x80 ? (crc << 1) ^ 0x1d : crc << 1; + return crc; +} + +// microsecond delay helper +static inline void +udelay(uint32_t usecs) +{ + uint32_t end = timer_read_time() + timer_from_us(usecs); + while (!timer_is_before(end, timer_read_time())) + irq_poll(); +} + +// tle5012b sensor query +static void +tle5012b_query(struct spi_angle *sa, uint32_t stime) +{ + struct gpio_out cs_pin = spidev_get_cs_pin(sa->spi); + // Latch data (data is latched on rising CS of a NULL message) + gpio_out_write(cs_pin, 0); + udelay(1); + irq_disable(); + gpio_out_write(cs_pin, 1); + uint32_t mtime = timer_read_time(); + irq_enable(); + + uint8_t msg[6] = { TLE_READ_LATCH, (TLE_REG_AVAL << 4) | 0x01, 0, 0, 0, 0 }; + uint8_t start_crc = 0x3f; // 0x3f == crc8(crc8(0xff, msg[0]), msg[1]) + spidev_transfer(sa->spi, 1, sizeof(msg), msg); + uint8_t crc = ~crc8(crc8(start_crc, msg[2]), msg[3]); + if (crc != msg[5]) + angle_add_error(sa, SE_CRC); + else if (!(msg[4] & (1<<4))) + angle_add_error(sa, SE_NO_ANGLE); + else if (!(msg[2] & 0x80)) + angle_add_error(sa, SE_DUP); + else + angle_add_data(sa, stime, mtime, (msg[2] << 9) | (msg[3] << 1)); +} + +void +command_query_spi_angle(uint32_t *args) +{ + uint8_t oid = args[0]; + struct spi_angle *sa = oid_lookup(oid, command_config_spi_angle); + + sched_del_timer(&sa->timer); + sa->flags = 0; + if (!args[2]) { + // End measurements + if (sa->data_count) + angle_report(sa, oid); + sendf("spi_angle_end oid=%c sequence=%hu", oid, sa->sequence); + return; + } + // Start new measurements query + sa->timer.waketime = args[1]; + sa->rest_ticks = args[2]; + sa->sequence = 0; + sa->data_count = 0; + sa->time_shift = args[3]; + sched_add_timer(&sa->timer); +} +DECL_COMMAND(command_query_spi_angle, + "query_spi_angle oid=%c clock=%u rest_ticks=%u time_shift=%c"); + +// Background task that performs measurements +void +spi_angle_task(void) +{ + if (!sched_check_wake(&angle_wake)) + return; + uint8_t oid; + struct spi_angle *sa; + foreach_oid(oid, sa, command_config_spi_angle) { + uint_fast8_t flags = sa->flags; + if (!(flags & SA_PENDING)) + continue; + irq_disable(); + uint32_t stime = sa->timer.waketime; + uint_fast8_t overflow = sa->overflow; + sa->flags = 0; + sa->overflow = 0; + irq_enable(); + stime -= sa->rest_ticks; + while (overflow--) { + angle_add_error(sa, SE_OVERFLOW); + angle_check_report(sa, oid); + } + uint_fast8_t chip = sa->chip_type; + if (chip == SA_CHIP_A1333) + a1333_query(sa, stime); + else if (chip == SA_CHIP_AS5047D) + as5047d_query(sa, stime); + else if (chip == SA_CHIP_TLE5012B) + tle5012b_query(sa, stime); + angle_check_report(sa, oid); + } +} +DECL_TASK(spi_angle_task); diff --git a/src/spicmds.c b/src/spicmds.c index f24bfca3..8bdefbbf 100644 --- a/src/spicmds.c +++ b/src/spicmds.c @@ -70,6 +70,18 @@ spidev_set_software_bus(struct spidev_s *spi, struct spi_software *ss) spi->flags |= SF_SOFTWARE; } +int +spidev_have_cs_pin(struct spidev_s *spi) +{ + return spi->flags & SF_HAVE_PIN; +} + +struct gpio_out +spidev_get_cs_pin(struct spidev_s *spi) +{ + return spi->pin; +} + void spidev_transfer(struct spidev_s *spi, uint8_t receive_data , uint8_t data_len, uint8_t *data) diff --git a/src/spicmds.h b/src/spicmds.h index 959fddb9..f7cf83fd 100644 --- a/src/spicmds.h +++ b/src/spicmds.h @@ -6,6 +6,8 @@ struct spidev_s *spidev_oid_lookup(uint8_t oid); struct spi_software; void spidev_set_software_bus(struct spidev_s *spi, struct spi_software *ss); +int spidev_have_cs_pin(struct spidev_s *spi); +struct gpio_out spidev_get_cs_pin(struct spidev_s *spi); void spidev_transfer(struct spidev_s *spi, uint8_t receive_data , uint8_t data_len, uint8_t *data);