path: root/src/sensor_mpu9250.c

// 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);