icm20948: Add support for ICM20948 accelerometer (#6756)

Signed-off-by: Paul Hansel <github@paulhansel.com>

1 files changed, 184 insertions, 0 deletions

diff --git a/src/sensor_icm20948.c b/src/sensor_icm20948.c
new file mode 100644
index 00000000..eb73a52c
--- /dev/null
+++ b/src/sensor_icm20948.c
@@ -0,0 +1,184 @@
+// Support for gathering acceleration data from icm20948 chip
+//
+// Copyright (C) 2024 Paul Hansel <github@paulhansel.com>
+// Copyright (C) 2023 Matthew Swabey <matthew@swabey.org>
+// Copyright (C) 2022 Harry Beyel <harry3b9@gmail.com>
+// Copyright (C) 2020-2023 Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <string.h> // memcpy
+#include "board/irq.h" // irq_disable
+#include "board/misc.h" // timer_read_time
+#include "basecmd.h" // oid_alloc
+#include "command.h" // DECL_COMMAND
+#include "sched.h" // DECL_TASK
+#include "sensor_bulk.h" // sensor_bulk_report
+#include "i2ccmds.h" // i2cdev_oid_lookup
+
+// Chip registers
+#define AR_FIFO_COUNT_H 0x70
+#define AR_FIFO         0x72
+#define AR_INT_STATUS   0x1B // INT_STATUS_2
+
+#define FIFO_OVERFLOW_INT 0x0F // from datasheet
+
+#define BYTES_PER_FIFO_ENTRY 6
+#define BYTES_PER_BLOCK 48
+
+struct icm20948 {
+    struct timer timer;
+    uint32_t rest_ticks;
+    struct i2cdev_s *i2c;
+    uint16_t fifo_max, fifo_pkts_bytes;
+    uint8_t flags;
+    struct sensor_bulk sb;
+};
+
+enum {
+    AX_PENDING = 1<<0,
+};
+
+static struct task_wake icm20948_wake;
+
+// Event handler that wakes icm20948_task() periodically
+static uint_fast8_t
+icm20948_event(struct timer *timer)
+{
+    struct icm20948 *ax = container_of(timer, struct icm20948, timer);
+    ax->flags |= AX_PENDING;
+    sched_wake_task(&icm20948_wake);
+    return SF_DONE;
+}
+
+void
+command_config_icm20948(uint32_t *args)
+{
+    struct icm20948 *ic = oid_alloc(args[0], command_config_icm20948
+                                   , sizeof(*ic));
+    ic->timer.func = icm20948_event;
+    ic->i2c = i2cdev_oid_lookup(args[1]);
+}
+DECL_COMMAND(command_config_icm20948, "config_icm20948 oid=%c i2c_oid=%c");
+
+// Helper code to reschedule the icm20948_event() timer
+static void
+ic20948_reschedule_timer(struct icm20948 *ic)
+{
+    irq_disable();
+    ic->timer.waketime = timer_read_time() + ic->rest_ticks;
+    sched_add_timer(&ic->timer);
+    irq_enable();
+}
+
+static void
+read_mpu(struct i2cdev_s *i2c, uint8_t reg_len, uint8_t *reg
+         , uint8_t read_len, uint8_t *read)
+{
+    int ret = i2c_dev_read(i2c, reg_len, reg, read_len, read);
+    i2c_shutdown_on_err(ret);
+}
+
+// Reads the fifo byte count from the device.
+static uint16_t
+get_fifo_status(struct icm20948 *ic)
+{
+    uint8_t reg[] = {AR_FIFO_COUNT_H};
+    uint8_t msg[2];
+    read_mpu(ic->i2c, sizeof(reg), reg, sizeof(msg), msg);
+    uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];
+    if (fifo_bytes > ic->fifo_max)
+        ic->fifo_max = fifo_bytes;
+    return fifo_bytes;
+}
+
+// Query accelerometer data
+static void
+ic20948_query(struct icm20948 *ic, uint8_t oid)
+{
+    // If not enough bytes to fill report read MPU FIFO's fill
+    if (ic->fifo_pkts_bytes < BYTES_PER_BLOCK)
+        ic->fifo_pkts_bytes = get_fifo_status(ic);
+
+    // If we have enough bytes to fill the buffer do it and send report
+    if (ic->fifo_pkts_bytes >= BYTES_PER_BLOCK) {
+        uint8_t reg = AR_FIFO;
+        read_mpu(ic->i2c, sizeof(reg), &reg, BYTES_PER_BLOCK, &ic->sb.data[0]);
+        ic->sb.data_count = BYTES_PER_BLOCK;
+        ic->fifo_pkts_bytes -= BYTES_PER_BLOCK;
+        sensor_bulk_report(&ic->sb, oid);
+    }
+
+    // If we have enough bytes remaining to fill another report wake again
+    //  otherwise schedule timed wakeup
+    if (ic->fifo_pkts_bytes >= BYTES_PER_BLOCK) {
+        sched_wake_task(&icm20948_wake);
+    } else {
+        ic->flags &= ~AX_PENDING;
+        ic20948_reschedule_timer(ic);
+    }
+}
+
+void
+command_query_icm20948(uint32_t *args)
+{
+    struct icm20948 *ic = oid_lookup(args[0], command_config_icm20948);
+
+    sched_del_timer(&ic->timer);
+    ic->flags = 0;
+    if (!args[1]) {
+        // End measurements
+
+        // Uncomment and rebuilt to check for FIFO overruns when tuning
+        //output("mpu9240 fifo_max=%u", ic->fifo_max);
+        return;
+    }
+
+    // Start new measurements query
+    ic->rest_ticks = args[1];
+    sensor_bulk_reset(&ic->sb);
+    ic->fifo_max = 0;
+    ic->fifo_pkts_bytes = 0;
+    ic20948_reschedule_timer(ic);
+}
+DECL_COMMAND(command_query_icm20948, "query_icm20948 oid=%c rest_ticks=%u");
+
+void
+command_query_icm20948_status(uint32_t *args)
+{
+    struct icm20948 *ic = oid_lookup(args[0], command_config_icm20948);
+
+    // Detect if a FIFO overrun occurred
+    uint8_t int_reg[] = {AR_INT_STATUS};
+    uint8_t int_msg;
+    read_mpu(ic->i2c, sizeof(int_reg), int_reg, sizeof(int_msg), &int_msg);
+    if (int_msg & FIFO_OVERFLOW_INT)
+        ic->sb.possible_overflows++;
+
+    // Read latest FIFO count (with precise timing)
+    uint8_t reg[] = {AR_FIFO_COUNT_H};
+    uint8_t msg[2];
+    uint32_t time1 = timer_read_time();
+    read_mpu(ic->i2c, sizeof(reg), reg, sizeof(msg), msg);
+    uint32_t time2 = timer_read_time();
+    uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];
+
+    // Report status
+    sensor_bulk_status(&ic->sb, args[0], time1, time2-time1, fifo_bytes);
+}
+DECL_COMMAND(command_query_icm20948_status, "query_icm20948_status oid=%c");
+
+void
+icm20948_task(void)
+{
+    if (!sched_check_wake(&icm20948_wake))
+        return;
+    uint8_t oid;
+    struct icm20948 *ic;
+    foreach_oid(oid, ic, command_config_icm20948) {
+        uint_fast8_t flags = ic->flags;
+        if (flags & AX_PENDING)
+            ic20948_query(ic, oid);
+    }
+}
+DECL_TASK(icm20948_task);