735 lines
20 KiB
C
735 lines
20 KiB
C
/*******************************************************************************
|
|
* @file imu_i2c.c
|
|
* @brief ICM42670P IMU Driver (Zephyr I2C API)
|
|
|
|
* I2C 핀: SCL=P1.14, SDA=P1.15 (overlay에서 설정)
|
|
******************************************************************************/
|
|
|
|
#include <zephyr/kernel.h>
|
|
#include <zephyr/drivers/i2c.h>
|
|
#include <zephyr/devicetree.h>
|
|
#include <zephyr/sys/util.h>
|
|
#include <errno.h>
|
|
#include <limits.h>
|
|
#include <string.h>
|
|
|
|
#include "imu_i2c.h"
|
|
#include "debug_print.h"
|
|
|
|
/*==============================================================================
|
|
* 디바이스트리 / I2C 설정
|
|
*============================================================================*/
|
|
#define IMU_I2C_NODE DT_NODELABEL(i2c0)
|
|
#define IMU_I2C_ADDR 0x68 // ICM42670P 기본 I2C 주소 (AD0=LOW)
|
|
|
|
/*==============================================================================
|
|
* ICM42670P 레지스터 주소
|
|
*============================================================================*/
|
|
#define REG_PWR_MGMT0 0x1F // 전원 관리: accel/gyro 동작 모드
|
|
#define REG_GYRO_CONFIG0 0x20 // 자이로 FSR + ODR 설정
|
|
#define REG_ACCEL_CONFIG0 0x21 // 가속도 FSR + ODR 설정
|
|
#define REG_GYRO_CONFIG1 0x23 // 자이로 LN 필터 BW
|
|
#define REG_ACCEL_CONFIG1 0x24 // 가속도 LN 필터 BW
|
|
#define REG_FIFO_CONFIG1 0x28 // FIFO bypass/mode
|
|
#define REG_FIFO_CONFIG2 0x29 // FIFO watermark LSB
|
|
#define REG_FIFO_CONFIG3 0x2A // FIFO watermark MSB
|
|
#define REG_INTF_CONFIG0 0x35 // FIFO count/endian 설정
|
|
#define REG_FIFO_COUNTH 0x3D // FIFO count high
|
|
#define REG_FIFO_DATA 0x3F // FIFO data pop
|
|
#define REG_BLK_SEL_W 0x79 // MREG write block
|
|
#define REG_MADDR_W 0x7A // MREG write addr
|
|
#define REG_M_W 0x7B // MREG write data
|
|
#define REG_BLK_SEL_R 0x7C // MREG read block
|
|
#define REG_MADDR_R 0x7D // MREG read addr
|
|
#define REG_M_R 0x7E // MREG read data
|
|
#define REG_TEMP_DATA1 0x09 // IMU 내부 온도 상위 바이트
|
|
#define REG_ACCEL_DATA_X1 0x0B // 가속도 X축 상위 바이트 (시작 레지스터)
|
|
#define REG_WHO_AM_I 0x75 // WHOAMI 레지스터
|
|
#define REG_MCLK_RDY 0x00 // MCLK ready
|
|
#define REG_SIGNAL_PATH_RESET 0x02 // FIFO flush
|
|
#define ICM_WHOAMI 0x67 // ICM42670P 식별값
|
|
|
|
/* 자이로 스타트업 대기 시간 (ms) — 스펙 최소 45ms, 80ms로 여유 확보 */
|
|
#define IMU_GYRO_STARTUP_MS 80
|
|
#define IMU_TEMP_AVG_SAMPLES 4U
|
|
|
|
/* FIFO 설정값: 50Hz, ±4g, ±500dps, Low Noise, 16Hz 필터 */
|
|
#define IMU_CFG_50HZ_4G 0x4A
|
|
#define IMU_CFG_50HZ_500DPS 0x4A
|
|
#define IMU_CFG_LN_BW_16HZ 0x07
|
|
#define IMU_PWR_ACCEL_GYRO_LN 0x0F
|
|
|
|
#define IMU_PWR_IDLE_MASK BIT(4)
|
|
#define IMU_MCLK_RDY_MASK BIT(3)
|
|
#define IMU_FIFO_FLUSH_MASK BIT(2)
|
|
|
|
#define IMU_FIFO_CONFIG1_STREAM 0x00
|
|
#define IMU_FIFO_CONFIG1_BYPASS 0x01
|
|
#define IMU_INTF_FIFO_COUNT_FORMAT_MASK BIT(6)
|
|
#define IMU_INTF_FIFO_COUNT_ENDIAN_MASK BIT(5)
|
|
#define IMU_INTF_SENSOR_DATA_ENDIAN_MASK BIT(4)
|
|
#define IMU_INTF_FIFO_COUNT_RECORD BIT(6)
|
|
#define IMU_INTF_FIFO_COUNT_LITTLE_ENDIAN 0x00
|
|
#define IMU_INTF_SENSOR_DATA_BIG_ENDIAN BIT(4)
|
|
#define IMU_FIFO_CONFIG5_MREG1 0x01
|
|
#define IMU_TMST_CONFIG1_MREG1 0x00
|
|
#define IMU_FIFO_CONFIG5_ACCEL_GYRO_TMST 0x27
|
|
#define IMU_FIFO_PACKET_BYTES 16U
|
|
#define IMU_FIFO_MAX_RECORDS 258U
|
|
#define IMU_FIFO_READ_BURST_RECORDS 14U
|
|
#define IMU_FIFO_ACCEL_OFFSET 1U
|
|
#define IMU_FIFO_GYRO_OFFSET 7U
|
|
#define IMU_FIFO_INVALID_HEADER 0x80
|
|
#define IMU_FIFO_INVALID_AXIS 0x8000
|
|
|
|
static const struct device *i2c_bus;
|
|
static bool fifo_active;
|
|
static uint8_t fifo_raw[IMU_FIFO_PACKET_BYTES * IMU_FIFO_MAX_RECORDS];
|
|
|
|
/* 내부 I2C 래퍼 */
|
|
|
|
/* 레지스터 1바이트 쓰기: [reg, val] 2바이트 TX */
|
|
static int imu_write_reg(uint8_t reg, uint8_t val)
|
|
{
|
|
uint8_t buf[2] = { reg, val };
|
|
return i2c_write(i2c_bus, buf, 2, IMU_I2C_ADDR);
|
|
}
|
|
|
|
/* 레지스터 연속 읽기: reg 주소 TX → data RX */
|
|
static int imu_read_regs(uint8_t reg, uint8_t *data, uint8_t len)
|
|
{
|
|
return i2c_write_read(i2c_bus, IMU_I2C_ADDR, ®, 1, data, len);
|
|
}
|
|
|
|
static int16_t imu_temp_raw_to_cdeg(int16_t temp_raw)
|
|
{
|
|
int32_t cdeg = 2500 + (((int32_t)temp_raw * 100) / 128);
|
|
|
|
if (cdeg > INT16_MAX)
|
|
{
|
|
cdeg = INT16_MAX;
|
|
}
|
|
else if (cdeg < INT16_MIN)
|
|
{
|
|
cdeg = INT16_MIN;
|
|
}
|
|
|
|
return (int16_t)cdeg;
|
|
}
|
|
|
|
static int imu_read_temp_raw(int16_t *temp_raw)
|
|
{
|
|
uint8_t raw[2];
|
|
int ret;
|
|
|
|
if (temp_raw == NULL)
|
|
{
|
|
return -EINVAL;
|
|
}
|
|
|
|
ret = imu_read_regs(REG_TEMP_DATA1, raw, sizeof(raw));
|
|
if (ret)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
*temp_raw = (int16_t)(((uint16_t)raw[0] << 8) | raw[1]);
|
|
return 0;
|
|
}
|
|
|
|
static int imu_update_reg(uint8_t reg, uint8_t clear_mask, uint8_t set_mask)
|
|
{
|
|
uint8_t val;
|
|
int ret = imu_read_regs(reg, &val, 1);
|
|
|
|
if (ret)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
val = (uint8_t)((val & ~clear_mask) | set_mask);
|
|
return imu_write_reg(reg, val);
|
|
}
|
|
|
|
static int imu_mclk_on(void)
|
|
{
|
|
uint8_t val = 0;
|
|
int ret = imu_update_reg(REG_PWR_MGMT0, 0U, IMU_PWR_IDLE_MASK);
|
|
|
|
if (ret)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
for (uint16_t i = 0; i < 1000U; i++)
|
|
{
|
|
ret = imu_read_regs(REG_MCLK_RDY, &val, 1);
|
|
if ((ret == 0) && ((val & IMU_MCLK_RDY_MASK) != 0U))
|
|
{
|
|
return 0;
|
|
}
|
|
k_busy_wait(10);
|
|
}
|
|
|
|
return -ETIMEDOUT;
|
|
}
|
|
|
|
static int imu_mclk_off(void)
|
|
{
|
|
return imu_update_reg(REG_PWR_MGMT0, IMU_PWR_IDLE_MASK, 0U);
|
|
}
|
|
|
|
static int imu_mreg_write(uint8_t addr, uint8_t val)
|
|
{
|
|
int ret = imu_mclk_on();
|
|
|
|
if (ret)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
ret = imu_write_reg(REG_BLK_SEL_W, 0U);
|
|
ret |= imu_write_reg(REG_MADDR_W, addr);
|
|
ret |= imu_write_reg(REG_M_W, val);
|
|
k_busy_wait(10);
|
|
ret |= imu_write_reg(REG_BLK_SEL_W, 0U);
|
|
ret |= imu_mclk_off();
|
|
return ret;
|
|
}
|
|
|
|
static int imu_set_data_big_endian(void)
|
|
{
|
|
return imu_update_reg(REG_INTF_CONFIG0,
|
|
IMU_INTF_SENSOR_DATA_ENDIAN_MASK,
|
|
IMU_INTF_SENSOR_DATA_BIG_ENDIAN);
|
|
}
|
|
|
|
static int imu_configure_fifo_count_format(void)
|
|
{
|
|
return imu_update_reg(REG_INTF_CONFIG0,
|
|
IMU_INTF_FIFO_COUNT_FORMAT_MASK |
|
|
IMU_INTF_FIFO_COUNT_ENDIAN_MASK |
|
|
IMU_INTF_SENSOR_DATA_ENDIAN_MASK,
|
|
IMU_INTF_FIFO_COUNT_RECORD |
|
|
IMU_INTF_FIFO_COUNT_LITTLE_ENDIAN |
|
|
IMU_INTF_SENSOR_DATA_BIG_ENDIAN);
|
|
}
|
|
|
|
static int imu_fifo_reset(void)
|
|
{
|
|
uint8_t val = IMU_FIFO_FLUSH_MASK;
|
|
int ret = imu_mclk_on();
|
|
|
|
if (ret)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
ret = imu_write_reg(REG_SIGNAL_PATH_RESET, val);
|
|
for (uint16_t i = 0; (ret == 0) && (i < 1000U); i++)
|
|
{
|
|
ret = imu_read_regs(REG_SIGNAL_PATH_RESET, &val, 1);
|
|
if ((val & IMU_FIFO_FLUSH_MASK) == 0U)
|
|
{
|
|
break;
|
|
}
|
|
k_busy_wait(10);
|
|
}
|
|
|
|
ret |= imu_mclk_off();
|
|
return ret;
|
|
}
|
|
|
|
static int imu_fifo_read_count(uint16_t *count)
|
|
{
|
|
uint8_t raw[2] = {0};
|
|
int ret;
|
|
|
|
if (count == NULL)
|
|
{
|
|
return -EINVAL;
|
|
}
|
|
|
|
ret = imu_mclk_on();
|
|
if (ret)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
ret = imu_read_regs(REG_FIFO_COUNTH, raw, sizeof(raw));
|
|
ret |= imu_mclk_off();
|
|
|
|
if (ret)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
*count = (uint16_t)raw[0] | ((uint16_t)raw[1] << 8);
|
|
if (*count > IMU_FIFO_MAX_RECORDS)
|
|
{
|
|
*count = IMU_FIFO_MAX_RECORDS;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int imu_fifo_read_records(uint16_t record_count)
|
|
{
|
|
uint16_t record_idx = 0;
|
|
int ret = imu_mclk_on();
|
|
|
|
if (ret)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
while ((record_idx < record_count) && (ret == 0))
|
|
{
|
|
uint16_t burst_records = record_count - record_idx;
|
|
uint16_t burst_bytes;
|
|
|
|
if (burst_records > IMU_FIFO_READ_BURST_RECORDS)
|
|
{
|
|
burst_records = IMU_FIFO_READ_BURST_RECORDS;
|
|
}
|
|
|
|
burst_bytes = burst_records * IMU_FIFO_PACKET_BYTES;
|
|
ret = imu_read_regs(REG_FIFO_DATA,
|
|
&fifo_raw[record_idx * IMU_FIFO_PACKET_BYTES],
|
|
(uint8_t)burst_bytes);
|
|
record_idx = (uint16_t)(record_idx + burst_records);
|
|
}
|
|
|
|
ret |= imu_mclk_off();
|
|
return ret;
|
|
}
|
|
|
|
static bool imu_fifo_record_is_placeholder(const uint8_t *rec)
|
|
{
|
|
if (rec[0] != IMU_FIFO_INVALID_HEADER)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
for (uint8_t i = 1U; i < IMU_FIFO_PACKET_BYTES; i++)
|
|
{
|
|
if (rec[i] != 0U)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static int16_t imu_fifo_axis_be(const uint8_t *rec, uint8_t offset)
|
|
{
|
|
return (int16_t)(((uint16_t)rec[offset] << 8) | rec[offset + 1U]);
|
|
}
|
|
|
|
static bool imu_fifo_record_has_valid_gyro(const uint8_t *rec)
|
|
{
|
|
for (uint8_t axis = 0U; axis < 3U; axis++)
|
|
{
|
|
if (imu_fifo_axis_be(rec, (uint8_t)(IMU_FIFO_GYRO_OFFSET + axis * 2U)) !=
|
|
(int16_t)IMU_FIFO_INVALID_AXIS)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
static uint16_t imu_fifo_compact_records(uint16_t record_count)
|
|
{
|
|
uint16_t write_idx = 0;
|
|
|
|
for (uint16_t read_idx = 0; read_idx < record_count; read_idx++)
|
|
{
|
|
uint8_t *rec = &fifo_raw[read_idx * IMU_FIFO_PACKET_BYTES];
|
|
|
|
if (imu_fifo_record_is_placeholder(rec) || !imu_fifo_record_has_valid_gyro(rec))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (write_idx != read_idx)
|
|
{
|
|
memcpy(&fifo_raw[write_idx * IMU_FIFO_PACKET_BYTES], rec, IMU_FIFO_PACKET_BYTES);
|
|
}
|
|
write_idx++;
|
|
}
|
|
|
|
return write_idx;
|
|
}
|
|
|
|
/* 공개 API */
|
|
|
|
int imu_init(void)
|
|
{
|
|
i2c_bus = DEVICE_DT_GET(IMU_I2C_NODE);
|
|
if (!device_is_ready(i2c_bus)) {
|
|
DBG_PRINTF("[IMU] FAIL — I2C bus not ready\r\n");
|
|
return -1;
|
|
}
|
|
|
|
uint8_t who_am_i = 0;
|
|
if (imu_read_regs(REG_WHO_AM_I, &who_am_i, 1) != 0) {
|
|
DBG_PRINTF("[IMU] FAIL — WHOAMI read error (check SCL=P1.14, SDA=P1.15)\r\n");
|
|
return -2;
|
|
}
|
|
|
|
if (who_am_i != ICM_WHOAMI) {
|
|
DBG_PRINTF("[IMU] FAIL — WHOAMI mismatch (got=0x%02X, expected=0x%02X)\r\n", who_am_i, ICM_WHOAMI);
|
|
return -3;
|
|
}
|
|
|
|
//DBG_PRINTF("[IMU] OK — ICM42670P detected (WHOAMI=0x%02X, addr=0x%02X)\r\n", who_am_i, IMU_I2C_ADDR);
|
|
return 0;
|
|
}
|
|
|
|
int imu_read(int16_t accel[3], int16_t gyro[3])
|
|
{
|
|
uint8_t raw[12];
|
|
int ret;
|
|
|
|
// 자이로 설정: ±2000dps FSR, 100Hz ODR
|
|
ret = imu_write_reg(REG_GYRO_CONFIG0, 0x09);
|
|
if (ret) { DBG_PRINTF("[IMU] FAIL — gyro config write (ret=%d)\r\n", ret); return -1; }
|
|
|
|
// 가속도 설정: ±4g FSR, 100Hz ODR
|
|
ret = imu_write_reg(REG_ACCEL_CONFIG0, 0x29);
|
|
if (ret) { DBG_PRINTF("[IMU] FAIL — accel config write (ret=%d)\r\n", ret); return -1; }
|
|
|
|
ret = imu_set_data_big_endian();
|
|
if (ret) { DBG_PRINTF("[IMU] FAIL — endian config write (ret=%d)\r\n", ret); return -1; }
|
|
|
|
// 전원 ON: accel(저잡음) + gyro(저잡음) 모두 활성화
|
|
ret = imu_write_reg(REG_PWR_MGMT0, 0x0F);
|
|
if (ret) { DBG_PRINTF("[IMU] FAIL — power on write (ret=%d)\r\n", ret); return -1; }
|
|
|
|
// 자이로 스타트업 대기
|
|
k_msleep(IMU_GYRO_STARTUP_MS);
|
|
|
|
// ACCEL_DATA_X1(0x0B)부터 12바이트 연속 읽기
|
|
// [0..5] = accel X,Y,Z (MSB first)
|
|
// [6..11] = gyro X,Y,Z (MSB first)
|
|
ret = imu_read_regs(REG_ACCEL_DATA_X1, raw, 12);
|
|
if (ret) { DBG_PRINTF("[IMU] FAIL — data read (ret=%d)\r\n", ret); return -2; }
|
|
|
|
// 빅엔디안 → int16_t 변환
|
|
accel[0] = (int16_t)((raw[0] << 8) | raw[1]);
|
|
accel[1] = (int16_t)((raw[2] << 8) | raw[3]);
|
|
accel[2] = (int16_t)((raw[4] << 8) | raw[5]);
|
|
gyro[0] = (int16_t)((raw[6] << 8) | raw[7]);
|
|
gyro[1] = (int16_t)((raw[8] << 8) | raw[9]);
|
|
gyro[2] = (int16_t)((raw[10] << 8) | raw[11]);
|
|
|
|
//DBG_PRINTF("[IMU] msp: A=(%6d,%6d,%6d) G=(%6d,%6d,%6d)\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
|
|
|
|
// 슬립 모드: 전력 절감
|
|
imu_write_reg(REG_PWR_MGMT0, 0x00);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int imu_read_with_temperature(int16_t accel[3], int16_t gyro[3], int16_t *temp_cdeg)
|
|
{
|
|
uint8_t raw[14];
|
|
int16_t temp_raw;
|
|
int16_t temp_samples[IMU_TEMP_AVG_SAMPLES];
|
|
int32_t temp_sum;
|
|
int ret;
|
|
|
|
if ((accel == NULL) || (gyro == NULL) || (temp_cdeg == NULL))
|
|
{
|
|
return -EINVAL;
|
|
}
|
|
|
|
ret = imu_write_reg(REG_GYRO_CONFIG0, 0x09);
|
|
if (ret) { DBG_PRINTF("[IMU] FAIL — gyro config write (ret=%d)\r\n", ret); return -1; }
|
|
|
|
ret = imu_write_reg(REG_ACCEL_CONFIG0, 0x29);
|
|
if (ret) { DBG_PRINTF("[IMU] FAIL — accel config write (ret=%d)\r\n", ret); return -1; }
|
|
|
|
ret = imu_set_data_big_endian();
|
|
if (ret) { DBG_PRINTF("[IMU] FAIL — endian config write (ret=%d)\r\n", ret); return -1; }
|
|
|
|
ret = imu_write_reg(REG_PWR_MGMT0, 0x0F);
|
|
if (ret) { DBG_PRINTF("[IMU] FAIL — power on write (ret=%d)\r\n", ret); return -1; }
|
|
|
|
k_msleep(IMU_GYRO_STARTUP_MS);
|
|
|
|
ret = imu_read_regs(REG_TEMP_DATA1, raw, sizeof(raw));
|
|
if (ret) { DBG_PRINTF("[IMU] FAIL — temp/data read (ret=%d)\r\n", ret); return -2; }
|
|
|
|
temp_raw = (int16_t)(((uint16_t)raw[0] << 8) | raw[1]);
|
|
temp_sum = temp_raw;
|
|
temp_samples[0] = temp_raw;
|
|
for (uint8_t i = 1U; i < IMU_TEMP_AVG_SAMPLES; i++)
|
|
{
|
|
int16_t next_temp_raw;
|
|
|
|
ret = imu_read_temp_raw(&next_temp_raw);
|
|
if (ret)
|
|
{
|
|
DBG_PRINTF("[IMU] temp avg read fail ret=%d idx=%u\r\n", ret, i);
|
|
return -3;
|
|
}
|
|
temp_samples[i] = next_temp_raw;
|
|
temp_sum += next_temp_raw;
|
|
}
|
|
|
|
temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES);
|
|
*temp_cdeg = imu_temp_raw_to_cdeg(temp_raw);
|
|
accel[0] = (int16_t)((raw[2] << 8) | raw[3]);
|
|
accel[1] = (int16_t)((raw[4] << 8) | raw[5]);
|
|
accel[2] = (int16_t)((raw[6] << 8) | raw[7]);
|
|
gyro[0] = (int16_t)((raw[8] << 8) | raw[9]);
|
|
gyro[1] = (int16_t)((raw[10] << 8) | raw[11]);
|
|
gyro[2] = (int16_t)((raw[12] << 8) | raw[13]);
|
|
|
|
/*
|
|
DBG_PRINTF("[IMU] mbb: A=(%6d,%6d,%6d) G=(%6d,%6d,%6d) T=%d.%02d C temp_raw=%d/%d/%d/%d avg=%d first=0x%02X%02X\r\n",
|
|
accel[0], accel[1], accel[2],
|
|
gyro[0], gyro[1], gyro[2],
|
|
*temp_cdeg / 100,
|
|
(*temp_cdeg < 0 ? -*temp_cdeg : *temp_cdeg) % 100,
|
|
temp_samples[0],
|
|
temp_samples[1],
|
|
temp_samples[2],
|
|
temp_samples[3],
|
|
temp_raw,
|
|
raw[0], raw[1]);
|
|
*/
|
|
imu_write_reg(REG_PWR_MGMT0, 0x00);
|
|
return 0;
|
|
}
|
|
|
|
int imu_read_temperature_cdeg(int16_t *temp_cdeg)
|
|
{
|
|
int16_t temp_raw;
|
|
int32_t temp_sum = 0;
|
|
int32_t cdeg;
|
|
int ret;
|
|
bool was_fifo_active = fifo_active;
|
|
|
|
if (temp_cdeg == NULL)
|
|
{
|
|
return -EINVAL;
|
|
}
|
|
|
|
ret = imu_init();
|
|
if (ret)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
if (!was_fifo_active)
|
|
{
|
|
ret = imu_write_reg(REG_PWR_MGMT0, IMU_PWR_ACCEL_GYRO_LN);
|
|
if (ret)
|
|
{
|
|
DBG_PRINTF("[IMU] temp power on fail ret=%d\r\n", ret);
|
|
return ret;
|
|
}
|
|
k_msleep(IMU_GYRO_STARTUP_MS);
|
|
}
|
|
|
|
for (uint8_t i = 0U; i < IMU_TEMP_AVG_SAMPLES; i++)
|
|
{
|
|
ret = imu_read_temp_raw(&temp_raw);
|
|
if (ret)
|
|
{
|
|
DBG_PRINTF("[IMU] temp read fail ret=%d idx=%u\r\n", ret, i);
|
|
if (!was_fifo_active)
|
|
{
|
|
(void)imu_write_reg(REG_PWR_MGMT0, 0x00);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
temp_sum += temp_raw;
|
|
}
|
|
|
|
temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES);
|
|
cdeg = imu_temp_raw_to_cdeg(temp_raw);
|
|
*temp_cdeg = (int16_t)cdeg;
|
|
/*
|
|
DBG_PRINTF("[IMU] temp=%d.%02d C raw=%d\r\n",
|
|
*temp_cdeg / 100,
|
|
(*temp_cdeg < 0 ? -*temp_cdeg : *temp_cdeg) % 100,
|
|
temp_raw);
|
|
*/
|
|
if (!was_fifo_active)
|
|
{
|
|
(void)imu_write_reg(REG_PWR_MGMT0, 0x00);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int imu_fifo_start(void)
|
|
{
|
|
int ret = imu_init();
|
|
|
|
if (ret)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
if (fifo_active)
|
|
{
|
|
DBG_PRINTF("[IMU FIFO] already running\r\n");
|
|
return 0;
|
|
}
|
|
|
|
ret = imu_write_reg(REG_PWR_MGMT0, 0x00);
|
|
ret |= imu_write_reg(REG_GYRO_CONFIG0, IMU_CFG_50HZ_500DPS);
|
|
ret |= imu_write_reg(REG_ACCEL_CONFIG0, IMU_CFG_50HZ_4G);
|
|
ret |= imu_write_reg(REG_GYRO_CONFIG1, IMU_CFG_LN_BW_16HZ);
|
|
ret |= imu_write_reg(REG_ACCEL_CONFIG1, IMU_CFG_LN_BW_16HZ);
|
|
ret |= imu_configure_fifo_count_format();
|
|
ret |= imu_write_reg(REG_FIFO_CONFIG1, IMU_FIFO_CONFIG1_STREAM);
|
|
ret |= imu_write_reg(REG_FIFO_CONFIG2, 0x01);
|
|
ret |= imu_write_reg(REG_FIFO_CONFIG3, 0x00);
|
|
ret |= imu_mreg_write(IMU_TMST_CONFIG1_MREG1, 0x01);
|
|
ret |= imu_mreg_write(IMU_FIFO_CONFIG5_MREG1, IMU_FIFO_CONFIG5_ACCEL_GYRO_TMST);
|
|
ret |= imu_fifo_reset();
|
|
ret |= imu_write_reg(REG_PWR_MGMT0, IMU_PWR_ACCEL_GYRO_LN);
|
|
|
|
k_msleep(IMU_GYRO_STARTUP_MS);
|
|
ret |= imu_fifo_reset();
|
|
|
|
if (ret)
|
|
{
|
|
DBG_PRINTF("[IMU FIFO] start fail ret=%d\r\n", ret);
|
|
(void)imu_write_reg(REG_FIFO_CONFIG1, IMU_FIFO_CONFIG1_BYPASS);
|
|
(void)imu_write_reg(REG_PWR_MGMT0, 0x00);
|
|
return ret;
|
|
}
|
|
|
|
fifo_active = true;
|
|
DBG_PRINTF("[IMU FIFO] start 50Hz LN\r\n");
|
|
return 0;
|
|
}
|
|
|
|
int imu_fifo_wait_samples(uint16_t target_samples, uint32_t timeout_ms)
|
|
{
|
|
uint32_t waited_ms = 0U;
|
|
const uint32_t poll_ms = 10U;
|
|
|
|
if (target_samples == 0U)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
if (!fifo_active)
|
|
{
|
|
return -EALREADY;
|
|
}
|
|
|
|
while (waited_ms <= timeout_ms)
|
|
{
|
|
uint16_t record_count = 0U;
|
|
int ret = imu_fifo_read_count(&record_count);
|
|
|
|
if (ret)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
if (record_count >= target_samples)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
k_msleep(poll_ms);
|
|
waited_ms += poll_ms;
|
|
}
|
|
|
|
return -ETIMEDOUT;
|
|
}
|
|
|
|
int imu_fifo_read_latest(uint8_t *sample_bytes,
|
|
uint16_t max_samples,
|
|
uint16_t *out_count)
|
|
{
|
|
uint16_t record_count = 0;
|
|
uint16_t start_idx = 0;
|
|
int ret;
|
|
|
|
if ((sample_bytes == NULL) || (out_count == NULL))
|
|
{
|
|
return -EINVAL;
|
|
}
|
|
|
|
*out_count = 0U;
|
|
|
|
if (!fifo_active)
|
|
{
|
|
return -EALREADY;
|
|
}
|
|
|
|
ret = imu_fifo_read_count(&record_count);
|
|
//DBG_PRINTF("[IMU FIFO] raw count=%u ret=%d target=%u\r\n", record_count, ret, max_samples);
|
|
if ((ret == 0) && (record_count > 0U))
|
|
{
|
|
ret = imu_fifo_read_records(record_count);
|
|
//DBG_PRINTF("[IMU FIFO] read records ret=%d\r\n", ret);
|
|
}
|
|
|
|
if (ret)
|
|
{
|
|
DBG_PRINTF("[IMU FIFO] drain fail ret=%d count=%u\r\n", ret, record_count);
|
|
return ret;
|
|
}
|
|
|
|
{
|
|
record_count = imu_fifo_compact_records(record_count);
|
|
DBG_PRINTF("[IMU FIFO] compact %u -> %u\r\n", raw_count, record_count);
|
|
}
|
|
if (record_count > max_samples)
|
|
{
|
|
start_idx = (uint16_t)(record_count - max_samples);
|
|
record_count = max_samples;
|
|
}
|
|
|
|
for (uint16_t i = 0; i < record_count; i++)
|
|
{
|
|
const uint8_t *rec = &fifo_raw[(start_idx + i) * IMU_FIFO_PACKET_BYTES];
|
|
uint8_t *dst = &sample_bytes[i * IMU_FIFO_SAMPLE_BYTES];
|
|
|
|
memcpy(dst, &rec[IMU_FIFO_ACCEL_OFFSET], 6U);
|
|
memcpy(&dst[6], &rec[IMU_FIFO_GYRO_OFFSET], 6U);
|
|
}
|
|
|
|
*out_count = record_count;
|
|
//DBG_PRINTF("[IMU FIFO] drained samples=%u\r\n", record_count);
|
|
return 0;
|
|
}
|
|
|
|
bool imu_fifo_is_active(void)
|
|
{
|
|
return fifo_active;
|
|
}
|
|
|
|
int imu_fifo_stop(void)
|
|
{
|
|
int ret = 0;
|
|
|
|
if (!fifo_active)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
ret |= imu_write_reg(REG_FIFO_CONFIG1, IMU_FIFO_CONFIG1_BYPASS);
|
|
ret |= imu_mreg_write(IMU_FIFO_CONFIG5_MREG1, 0x00);
|
|
ret |= imu_write_reg(REG_PWR_MGMT0, 0x00);
|
|
fifo_active = false;
|
|
|
|
DBG_PRINTF("[IMU FIFO] stop ret=%d\r\n", ret);
|
|
return ret;
|
|
}
|