sensor_angle: Add support for bulk querying of spi angle sensors

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2021-07-09 22:04:10 -04:00
parent 91ba9c00e3
commit 74937326d3
7 changed files with 524 additions and 1 deletions

View File

@ -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 "header" field in the initial query response is used to describe
the fields found in later "data" responses. 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 ### pause_resume/cancel
This endpoint is similar to running the "PRINT_CANCEL" G-Code command. This endpoint is similar to running the "PRINT_CANCEL" G-Code command.

View File

@ -4001,6 +4001,33 @@ serial:
# Auto cancel print when ping varation is above this threshold # 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 bus parameters
### Common SPI settings ### Common SPI settings

188
klippy/extras/angle.py Normal file
View File

@ -0,0 +1,188 @@
# Support for reading SPI magnetic angle sensors
#
# Copyright (C) 2021,2022 Kevin O'Connor <kevin@koconnor.net>
#
# 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<<time_shift)
ptime = round(clock_to_print_time(mclock) - static_delay, 6)
samples[count] = (ptime, last_angle)
count += 1
self.last_sequence = last_sequence
self.last_angle = last_angle
del samples[count:]
return samples, error_count
# API interface
def _api_update(self, eventtime):
with self.lock:
raw_samples = self.raw_samples
self.raw_samples = []
if not raw_samples:
return {}
samples, error_count = self._extract_samples(raw_samples)
if not samples:
return {}
return {'data': samples, 'errors': error_count}
def _start_measurements(self):
if self.is_measuring():
return
logging.info("Starting angle '%s' measurements", self.name)
self.sensor_helper.start()
# Start bulk reading
with self.lock:
self.raw_samples = []
self.last_sequence = 0
systime = self.printer.get_reactor().monotonic()
print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
self.start_clock = reqclock = self.mcu.print_time_to_clock(print_time)
rest_ticks = self.mcu.seconds_to_clock(self.sample_period)
self.sample_ticks = rest_ticks
self.query_spi_angle_cmd.send([self.oid, reqclock, rest_ticks,
self.time_shift], reqclock=reqclock)
def _finish_measurements(self):
if not self.is_measuring():
return
# Halt bulk reading
params = self.query_spi_angle_end_cmd.send([self.oid, 0, 0, 0])
self.start_clock = 0
with self.lock:
self.raw_samples = []
logging.info("Stopped angle '%s' measurements", self.name)
def _api_startstop(self, is_start):
if is_start:
self._start_measurements()
else:
self._finish_measurements()
def _handle_dump_angle(self, web_request):
self.api_dump.add_client(web_request)
hdr = ('time', 'angle')
web_request.send({'header': hdr})
def load_config_prefix(config):
return Angle(config)

View File

@ -7,6 +7,6 @@ src-$(CONFIG_HAVE_GPIO_ADC) += adccmds.c
src-$(CONFIG_HAVE_GPIO_SPI) += spicmds.c thermocouple.c src-$(CONFIG_HAVE_GPIO_SPI) += spicmds.c thermocouple.c
src-$(CONFIG_HAVE_GPIO_I2C) += i2ccmds.c src-$(CONFIG_HAVE_GPIO_I2C) += i2ccmds.c
src-$(CONFIG_HAVE_GPIO_HARD_PWM) += pwmcmds.c src-$(CONFIG_HAVE_GPIO_HARD_PWM) += pwmcmds.c
bb-src-$(CONFIG_HAVE_GPIO_SPI) := spi_software.c sensor_adxl345.c bb-src-$(CONFIG_HAVE_GPIO_SPI) := spi_software.c sensor_adxl345.c sensor_angle.c
src-$(CONFIG_HAVE_GPIO_BITBANGING) += $(bb-src-y) lcd_st7920.c lcd_hd44780.c \ src-$(CONFIG_HAVE_GPIO_BITBANGING) += $(bb-src-y) lcd_st7920.c lcd_hd44780.c \
buttons.c tmcuart.c neopixel.c pulse_counter.c buttons.c tmcuart.c neopixel.c pulse_counter.c

276
src/sensor_angle.c Normal file
View File

@ -0,0 +1,276 @@
// Support for querying magnetic angle sensors via SPI
//
// Copyright (C) 2021 Kevin O'Connor <kevin@koconnor.net>
//
// 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);

View File

@ -70,6 +70,18 @@ spidev_set_software_bus(struct spidev_s *spi, struct spi_software *ss)
spi->flags |= SF_SOFTWARE; 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 void
spidev_transfer(struct spidev_s *spi, uint8_t receive_data spidev_transfer(struct spidev_s *spi, uint8_t receive_data
, uint8_t data_len, uint8_t *data) , uint8_t data_len, uint8_t *data)

View File

@ -6,6 +6,8 @@
struct spidev_s *spidev_oid_lookup(uint8_t oid); struct spidev_s *spidev_oid_lookup(uint8_t oid);
struct spi_software; struct spi_software;
void spidev_set_software_bus(struct spidev_s *spi, struct spi_software *ss); 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 void spidev_transfer(struct spidev_s *spi, uint8_t receive_data
, uint8_t data_len, uint8_t *data); , uint8_t data_len, uint8_t *data);