sensor_angle: Add support for bulk querying of spi angle sensors

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 276 insertions, 0 deletions

diff --git a/src/sensor_angle.c b/src/sensor_angle.c
new file mode 100644
index 00000000..c03cc957
--- /dev/null
+++ b/src/sensor_angle.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);