diff options
Diffstat (limited to 'src/sensor_icm20948.c')
| -rw-r--r-- | src/sensor_icm20948.c | 184 |
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), ®, 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); |