mpu9250: Adding support for MPU-9250 (and MPU-6050) accelerometer

Add support for mpu9250 accelerometer over I2C bus.

Signed-off-by: Harry Beyel <harry3b9@gmail.com>

1 files changed, 277 insertions, 0 deletions

diff --git a/src/sensor_mpu9250.c b/src/sensor_mpu9250.c
new file mode 100644
index 00000000..d7f30928
--- /dev/null
+++ b/src/sensor_mpu9250.c
@@ -0,0 +1,277 @@
+// Support for gathering acceleration data from mpu9250 chip
+//
+// Copyright (C) 2022 Harry Beyel <harry3b9@gmail.com>
+// Copyright (C) 2020-2021 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 "board/gpio.h" // i2c_read
+#include "i2ccmds.h" // i2cdev_oid_lookup
+
+// Chip registers
+#define AR_FIFO_SIZE 512
+
+#define AR_PWR_MGMT_1   0x6B
+#define AR_PWR_MGMT_2   0x6C
+#define AR_FIFO_EN      0x23
+#define AR_ACCEL_OUT_XH 0x3B
+#define AR_USER_CTRL    0x6A
+#define AR_FIFO_COUNT_H 0x72
+#define AR_FIFO         0x74
+
+#define SET_ENABLE_FIFO 0x08
+#define SET_DISABLE_FIFO 0x00
+#define SET_USER_FIFO_RESET 0x04
+#define SET_USER_FIFO_EN 0x40
+
+#define SET_PWR_SLEEP   0x40
+#define SET_PWR_WAKE    0x00
+#define SET_PWR_2_ACCEL 0x07 // only enable accelerometers
+#define SET_PWR_2_NONE  0x3F // disable all sensors
+
+#define BYTES_PER_FIFO_ENTRY 6
+
+struct mpu9250 {
+    struct timer timer;
+    uint32_t rest_ticks;
+    struct i2cdev_s *i2c;
+    uint16_t sequence, limit_count;
+    uint8_t flags, data_count;
+    // data size must be <= 255 due to i2c api
+    // = SAMPLES_PER_BLOCK (from mpu9250.py) * BYTES_PER_FIFO_ENTRY + 1
+    uint8_t data[48];
+};
+
+enum {
+    AX_HAVE_START = 1<<0, AX_RUNNING = 1<<1, AX_PENDING = 1<<2,
+};
+
+static struct task_wake mpu9250_wake;
+
+// Reads the fifo byte count from the device.
+uint16_t
+get_fifo_status (struct mpu9250 *mp)
+{
+    uint8_t regs[] = {AR_FIFO_COUNT_H};
+    uint8_t msg[2];
+    i2c_read(mp->i2c->i2c_config, sizeof(regs), regs, 2, msg);
+    msg[0] = 0x1F & msg[0]; // discard 3 MSB per datasheet
+    return (((uint16_t)msg[0]) << 8 | msg[1]);
+}
+
+// Event handler that wakes mpu9250_task() periodically
+static uint_fast8_t
+mpu9250_event(struct timer *timer)
+{
+    struct mpu9250 *ax = container_of(timer, struct mpu9250, timer);
+    ax->flags |= AX_PENDING;
+    sched_wake_task(&mpu9250_wake);
+    return SF_DONE;
+}
+
+void
+command_config_mpu9250(uint32_t *args)
+{
+    struct mpu9250 *mp = oid_alloc(args[0], command_config_mpu9250
+                                   , sizeof(*mp));
+    mp->timer.func = mpu9250_event;
+    mp->i2c = i2cdev_oid_lookup(args[1]);
+}
+DECL_COMMAND(command_config_mpu9250, "config_mpu9250 oid=%c i2c_oid=%c");
+
+// Report local measurement buffer
+static void
+mp9250_report(struct mpu9250 *mp, uint8_t oid)
+{
+    sendf("mpu9250_data oid=%c sequence=%hu data=%*s"
+          , oid, mp->sequence, mp->data_count, mp->data);
+    mp->data_count = 0;
+    mp->sequence++;
+}
+
+// Report buffer and fifo status
+static void
+mp9250_status(struct mpu9250 *mp, uint_fast8_t oid
+            , uint32_t time1, uint32_t time2, uint16_t fifo)
+{
+    sendf("mpu9250_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
+          " buffered=%c fifo=%u limit_count=%hu"
+          , oid, time1, time2-time1, mp->sequence
+          , mp->data_count, fifo, mp->limit_count);
+}
+
+// Helper code to reschedule the mpu9250_event() timer
+static void
+mp9250_reschedule_timer(struct mpu9250 *mp)
+{
+    irq_disable();
+    mp->timer.waketime = timer_read_time() + mp->rest_ticks;
+    sched_add_timer(&mp->timer);
+    irq_enable();
+}
+
+// Query accelerometer data
+static void
+mp9250_query(struct mpu9250 *mp, uint8_t oid)
+{
+    // Check fifo status
+    uint16_t fifo_bytes = get_fifo_status(mp);
+    if (fifo_bytes >= AR_FIFO_SIZE - BYTES_PER_FIFO_ENTRY)
+        mp->limit_count++;
+
+    // Read data
+    // FIFO data are: [Xh, Xl, Yh, Yl, Zh, Zl]
+    uint8_t reg = AR_FIFO;
+    uint8_t bytes_to_read = fifo_bytes < sizeof(mp->data) - mp->data_count ?
+                                    fifo_bytes & 0xFF :
+                                    (sizeof(mp->data) - mp->data_count) & 0xFF;
+
+    // round down to nearest full packet of data
+    bytes_to_read = bytes_to_read / BYTES_PER_FIFO_ENTRY * BYTES_PER_FIFO_ENTRY;
+
+    // Extract x, y, z measurements into data holder and report
+    if (bytes_to_read > 0) {
+        i2c_read(mp->i2c->i2c_config, sizeof(reg), &reg,
+                bytes_to_read, &mp->data[mp->data_count]);
+        mp->data_count += bytes_to_read;
+
+        // report data when buffer is full
+        if (mp->data_count + BYTES_PER_FIFO_ENTRY > sizeof(mp->data)) {
+            mp9250_report(mp, oid);
+        }
+    }
+
+    // check if we need to run the task again (more packets in fifo?)
+    if ( bytes_to_read > 0 &&
+            bytes_to_read / BYTES_PER_FIFO_ENTRY <
+            fifo_bytes / BYTES_PER_FIFO_ENTRY) {
+        // more data still ready in the fifo buffer
+        sched_wake_task(&mpu9250_wake);
+    }
+    else if (mp->flags & AX_RUNNING) {
+        // No more fifo data, but actively running. Sleep until next check
+        sched_del_timer(&mp->timer);
+        mp->flags &= ~AX_PENDING;
+        mp9250_reschedule_timer(mp);
+    }
+}
+
+// Startup measurements
+static void
+mp9250_start(struct mpu9250 *mp, uint8_t oid)
+{
+    sched_del_timer(&mp->timer);
+    mp->flags = AX_RUNNING;
+    uint8_t msg[2];
+
+    msg[0] = AR_FIFO_EN;
+    msg[1] = SET_DISABLE_FIFO; // disable FIFO
+    i2c_write(mp->i2c->i2c_config, sizeof(msg), msg);
+
+    msg[0] = AR_USER_CTRL;
+    msg[1] = SET_USER_FIFO_RESET; // reset FIFO buffer
+    i2c_write(mp->i2c->i2c_config, sizeof(msg), msg);
+
+    msg[0] = AR_USER_CTRL;
+    msg[1] = SET_USER_FIFO_EN; // enable FIFO buffer access
+    i2c_write(mp->i2c->i2c_config, sizeof(msg), msg);
+
+    msg[0] = AR_FIFO_EN;
+    msg[1] = SET_ENABLE_FIFO; // enable accel output to FIFO
+    i2c_write(mp->i2c->i2c_config, sizeof(msg), msg);
+
+    mp9250_reschedule_timer(mp);
+}
+
+// End measurements
+static void
+mp9250_stop(struct mpu9250 *mp, uint8_t oid)
+{
+    // Disable measurements
+    sched_del_timer(&mp->timer);
+    mp->flags = 0;
+
+    // disable accel FIFO
+    uint8_t msg[2] = { AR_FIFO_EN, SET_DISABLE_FIFO };
+    uint32_t end1_time = timer_read_time();
+    i2c_write(mp->i2c->i2c_config, sizeof(msg), msg);
+    uint32_t end2_time = timer_read_time();
+
+    // Drain any measurements still in fifo
+    uint16_t fifo_bytes = get_fifo_status(mp);
+    while (fifo_bytes >= BYTES_PER_FIFO_ENTRY) {
+        mp9250_query(mp, oid);
+        fifo_bytes = get_fifo_status(mp);
+    }
+
+    // Report final data
+    if (mp->data_count > 0)
+        mp9250_report(mp, oid);
+    mp9250_status(mp, oid, end1_time, end2_time,
+                    fifo_bytes / BYTES_PER_FIFO_ENTRY);
+}
+
+void
+command_query_mpu9250(uint32_t *args)
+{
+    struct mpu9250 *mp = oid_lookup(args[0], command_config_mpu9250);
+
+    if (!args[2]) {
+        // End measurements
+        mp9250_stop(mp, args[0]);
+        return;
+    }
+    // Start new measurements query
+    sched_del_timer(&mp->timer);
+    mp->timer.waketime = args[1];
+    mp->rest_ticks = args[2];
+    mp->flags = AX_HAVE_START;
+    mp->sequence = mp->limit_count = 0;
+    mp->data_count = 0;
+    sched_add_timer(&mp->timer);
+}
+DECL_COMMAND(command_query_mpu9250,
+             "query_mpu9250 oid=%c clock=%u rest_ticks=%u");
+
+void
+command_query_mpu9250_status(uint32_t *args)
+{
+    struct mpu9250 *mp = oid_lookup(args[0], command_config_mpu9250);
+    uint8_t msg[2];
+    uint32_t time1 = timer_read_time();
+    uint8_t regs[] = {AR_FIFO_COUNT_H};
+    i2c_read(mp->i2c->i2c_config, 1, regs, 2, msg);
+    uint32_t time2 = timer_read_time();
+    msg[0] = 0x1F & msg[0]; // discard 3 MSB
+    uint16_t fifo_bytes = (((uint16_t)msg[0]) << 8) | msg[1];
+    mp9250_status(mp, args[0], time1, time2, fifo_bytes / BYTES_PER_FIFO_ENTRY);
+}
+DECL_COMMAND(command_query_mpu9250_status, "query_mpu9250_status oid=%c");
+
+void
+mpu9250_task(void)
+{
+    if (!sched_check_wake(&mpu9250_wake))
+        return;
+    uint8_t oid;
+    struct mpu9250 *mp;
+    foreach_oid(oid, mp, command_config_mpu9250) {
+        uint_fast8_t flags = mp->flags;
+        if (!(flags & AX_PENDING)) {
+            continue;
+        }
+        if (flags & AX_HAVE_START) {
+            mp9250_start(mp, oid);
+        }
+        else {
+            mp9250_query(mp, oid);
+        }
+    }
+}
+DECL_TASK(mpu9250_task);