mirror of https://github.com/Desuuuu/klipper.git
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:
parent
91ba9c00e3
commit
74937326d3
|
@ -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.
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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)
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
|
@ -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)
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue