init:全链路闭环控制调通
This commit is contained in:
SkyLi
@@ -0,0 +1,172 @@
|
||||
#include "ICM_42688.h"
|
||||
|
||||
//==================================================================================================
|
||||
// 内部变量
|
||||
//==================================================================================================
|
||||
|
||||
static float accel_scale = 1.0f; // 加速度计 LSB → g
|
||||
static float gyro_scale = 1.0f; // 陀螺仪 LSB → °/s
|
||||
|
||||
//==================================================================================================
|
||||
// SPI 底层操作宏
|
||||
//==================================================================================================
|
||||
|
||||
#define ICM42688_WRITE_REG(reg, data) spi_write_8bit_register(ICM42688_SPI, reg, data)
|
||||
#define ICM42688_READ_REGS(reg, data, num) spi_read_8bit_registers(ICM42688_SPI, (reg) | 0x80, data, num)
|
||||
|
||||
//==================================================================================================
|
||||
// 静态函数 — 寄存器级 SPI 操作
|
||||
//==================================================================================================
|
||||
|
||||
/** @brief 向指定寄存器写入一字节
|
||||
* @param reg 寄存器地址
|
||||
* @param data 写入值
|
||||
* 片选 → 写寄存器 → 释放片选
|
||||
*/
|
||||
static void icm42688_write_reg(uint8_t reg, uint8_t data)
|
||||
{
|
||||
ICM42688_CS_LEVEL(0);
|
||||
ICM42688_WRITE_REG(reg, data);
|
||||
ICM42688_CS_LEVEL(1);
|
||||
}
|
||||
|
||||
/** @brief 从指定寄存器连续读取多字节
|
||||
* @param reg 起始寄存器地址
|
||||
* @param data 输出缓冲区指针
|
||||
* @param num 读取字节数
|
||||
* 片选 → 读多字节 → 释放片选
|
||||
*/
|
||||
static void icm42688_read_regs(uint8_t reg, uint8_t *data, uint8_t num)
|
||||
{
|
||||
ICM42688_CS_LEVEL(0);
|
||||
ICM42688_READ_REGS(reg, data, num);
|
||||
ICM42688_CS_LEVEL(1);
|
||||
}
|
||||
|
||||
/** @brief 将两个 uint8 拼接为 int16（大端序）
|
||||
* @param hi 高字节
|
||||
* @param lo 低字节
|
||||
* @return 有符号 16 位整数
|
||||
*/
|
||||
static int16_t icm42688_combine_bytes(uint8_t hi, uint8_t lo)
|
||||
{
|
||||
return (int16_t)(((uint16_t)hi << 8) | lo);
|
||||
}
|
||||
|
||||
/** @brief 配置加速度计/陀螺仪的量程和输出速率
|
||||
* @param afs 加速度计量程
|
||||
* @param aodr 加速度计输出速率
|
||||
* @param gfs 陀螺仪量程
|
||||
* @param godr 陀螺仪输出速率
|
||||
* 同时更新内部比例系数 accel_scale / gyro_scale
|
||||
*/
|
||||
static void icm42688_set_range_odr(enum icm42688_afs afs, enum icm42688_aodr aodr,
|
||||
enum icm42688_gfs gfs, enum icm42688_godr godr)
|
||||
{
|
||||
icm42688_write_reg(ICM42688_ACCEL_CONFIG0, (afs << 5) | (aodr + 1));
|
||||
icm42688_write_reg(ICM42688_GYRO_CONFIG0, (gfs << 5) | (godr + 1));
|
||||
|
||||
switch (afs) {
|
||||
case ICM42688_AFS_2G: accel_scale = 2000.0f / 32768.0f; break;
|
||||
case ICM42688_AFS_4G: accel_scale = 4000.0f / 32768.0f; break;
|
||||
case ICM42688_AFS_8G: accel_scale = 8000.0f / 32768.0f; break;
|
||||
case ICM42688_AFS_16G: accel_scale = 16000.0f / 32768.0f; break;
|
||||
default: accel_scale = 1.0f; break;
|
||||
}
|
||||
|
||||
switch (gfs) {
|
||||
case ICM42688_GFS_15_625DPS: gyro_scale = 15.625f / 32768.0f; break;
|
||||
case ICM42688_GFS_31_25DPS: gyro_scale = 31.25f / 32768.0f; break;
|
||||
case ICM42688_GFS_62_5DPS: gyro_scale = 62.5f / 32768.0f; break;
|
||||
case ICM42688_GFS_125DPS: gyro_scale = 125.0f / 32768.0f; break;
|
||||
case ICM42688_GFS_250DPS: gyro_scale = 250.0f / 32768.0f; break;
|
||||
case ICM42688_GFS_500DPS: gyro_scale = 500.0f / 32768.0f; break;
|
||||
case ICM42688_GFS_1000DPS: gyro_scale = 1000.0f / 32768.0f; break;
|
||||
case ICM42688_GFS_2000DPS: gyro_scale = 2000.0f / 32768.0f; break;
|
||||
default: gyro_scale = 1.0f; break;
|
||||
}
|
||||
}
|
||||
|
||||
//==================================================================================================
|
||||
// 公开接口
|
||||
//==================================================================================================
|
||||
|
||||
/** @brief 返回当前陀螺仪比例系数 (LSB → °/s)
|
||||
* 供 imu_calibrate_gyro() 将原始 ADC 均值转换为物理偏移量
|
||||
*/
|
||||
float icm42688_gyro_scale(void)
|
||||
{
|
||||
return gyro_scale;
|
||||
}
|
||||
|
||||
/** @brief 初始化 ICM-42688
|
||||
* 1. 初始化 SPI 外设和 CS 引脚
|
||||
* 2. 读取 WHO_AM_I 进行自检（超时 500ms 后报错死循环）
|
||||
* 3. 复位并配置量程/速率（默认 ±16G / ±2000dps / 200Hz ODR）
|
||||
* 4. 设置为低噪声模式
|
||||
* 由 imu_init() 内部调用，用户一般不直接调用
|
||||
*/
|
||||
void icm42688_init(void)
|
||||
{
|
||||
system_delay_ms(10);
|
||||
spi_init(ICM42688_SPI, SPI_MODE0, ICM42688_SPI_SPEED,
|
||||
ICM42688_SPC_PIN, ICM42688_SDI_PIN, ICM42688_SDO_PIN, SPI_CS_NULL);
|
||||
gpio_init(ICM42688_CS_PIN, GPO, GPIO_HIGH, GPO_PUSH_PULL);
|
||||
|
||||
uint8_t model = 0xFF;
|
||||
int timeout = 50;
|
||||
while (1) {
|
||||
icm42688_read_regs(ICM42688_WHO_AM_I, &model, 1);
|
||||
if (model == 0x47) {
|
||||
break;
|
||||
}
|
||||
ICM42688_DELAY_MS(10);
|
||||
if (--timeout < 0) {
|
||||
zf_log(0, "ICM42688 Init Error!");
|
||||
while (1);
|
||||
}
|
||||
}
|
||||
|
||||
icm42688_write_reg(ICM42688_PWR_MGMT0, 0x00);
|
||||
ICM42688_DELAY_MS(20);
|
||||
|
||||
// 如果修改了采样频率（定时器中断周期），则需要对应修改下方的配置
|
||||
icm42688_set_range_odr(ICM42688_AFS_16G, ICM42688_AODR_200HZ,
|
||||
ICM42688_GFS_2000DPS, ICM42688_GODR_200HZ);
|
||||
|
||||
icm42688_write_reg(ICM42688_PWR_MGMT0, 0x0F);
|
||||
ICM42688_DELAY_MS(10);
|
||||
}
|
||||
|
||||
/** @brief 读取全部传感器数据（加速度 + 角速度）
|
||||
* @param data 输出结构体指针
|
||||
* 一次 12 字节 SPI 突发从 ACCEL_DATA_X1 开始连续读取
|
||||
* 读取顺序: AX→AY→AZ→GX→GY→GZ（与传感器内部 FIFO 顺序一致）
|
||||
*/
|
||||
void icm42688_read_all(icm42688_data_t *data)
|
||||
{
|
||||
uint8_t buf[12];
|
||||
icm42688_read_regs(ICM42688_ACCEL_DATA_X1, buf, 12);
|
||||
|
||||
data->accel_x = accel_scale * icm42688_combine_bytes(buf[0], buf[1]);
|
||||
data->accel_y = accel_scale * icm42688_combine_bytes(buf[2], buf[3]);
|
||||
data->accel_z = accel_scale * icm42688_combine_bytes(buf[4], buf[5]);
|
||||
|
||||
data->gyro_x = gyro_scale * icm42688_combine_bytes(buf[6], buf[7]);
|
||||
data->gyro_y = gyro_scale * icm42688_combine_bytes(buf[8], buf[9]);
|
||||
data->gyro_z = gyro_scale * icm42688_combine_bytes(buf[10], buf[11]);
|
||||
}
|
||||
|
||||
/** @brief 读取陀螺仪原始 ADC 值（不做比例换算）
|
||||
* @param gx/gy/gz 输出 int16 原始值指针
|
||||
* 供 imu_calibrate_gyro() 在静止状态下采集原始数据计算零偏
|
||||
*/
|
||||
void icm42688_read_gyro_raw(int16_t *gx, int16_t *gy, int16_t *gz)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
icm42688_read_regs(ICM42688_GYRO_DATA_X1, buf, 6);
|
||||
|
||||
*gx = icm42688_combine_bytes(buf[0], buf[1]);
|
||||
*gy = icm42688_combine_bytes(buf[2], buf[3]);
|
||||
*gz = icm42688_combine_bytes(buf[4], buf[5]);
|
||||
}
|
||||
Reference in New Issue
Block a user