init:全链路闭环控制调通

2026-06-06 21:26:08 +08:00
parent b5be64b642
commit f28d129e11
32 changed files with 5120 additions and 181 deletions
@@ -0,0 +1,198 @@
+#ifndef __ICM_42688_H
+#define __ICM_42688_H
+
+#include "zf_common_headfile.h"
+
+//==================================================================================================
+// ICM-42688-P 6轴惯性传感器 硬件抽象层
+// 本文件仅负责 SPI 通信、寄存器读写、原始数据获取，不涉及姿态解算
+//==================================================================================================
+
+// SPI 引脚定义
+#define ICM42688_SPI            (SPI_1)
+#define ICM42688_SPI_SPEED      (8 * 1000 * 1000)
+#define ICM42688_SPC_PIN        (SPI1_SCK_B23)
+#define ICM42688_SDI_PIN        (SPI1_MOSI_B22)
+#define ICM42688_SDO_PIN        (SPI1_MISO_B21)
+#define ICM42688_CS_PIN         (B19)
+
+#define ICM42688_DELAY_MS(time) (system_delay_ms(time))
+#define ICM42688_CS_LEVEL(level) gpio_set_level(ICM42688_CS_PIN, level)
+
+// 传感器原始数据（物理单位：加速度 g，角速度 °/s，传感器坐标系）
+typedef struct {
+    float accel_x, accel_y, accel_z;
+    float gyro_x, gyro_y, gyro_z;
+} icm42688_data_t;
+
+// 加速度计量程
+enum icm42688_afs {
+    ICM42688_AFS_16G,
+    ICM42688_AFS_8G,
+    ICM42688_AFS_4G,
+    ICM42688_AFS_2G,
+};
+
+// 加速度计输出速率 (ODR)
+enum icm42688_aodr {
+    ICM42688_AODR_32000HZ,
+    ICM42688_AODR_16000HZ,
+    ICM42688_AODR_8000HZ,
+    ICM42688_AODR_4000HZ,
+    ICM42688_AODR_2000HZ,
+    ICM42688_AODR_1000HZ,
+    ICM42688_AODR_200HZ,
+    ICM42688_AODR_100HZ,
+    ICM42688_AODR_50HZ,
+    ICM42688_AODR_25HZ,
+    ICM42688_AODR_12_5HZ,
+    ICM42688_AODR_6_25HZ,
+    ICM42688_AODR_3_125HZ,
+    ICM42688_AODR_1_5625HZ,
+    ICM42688_AODR_500HZ,
+};
+
+// 陀螺仪量程
+enum icm42688_gfs {
+    ICM42688_GFS_2000DPS,
+    ICM42688_GFS_1000DPS,
+    ICM42688_GFS_500DPS,
+    ICM42688_GFS_250DPS,
+    ICM42688_GFS_125DPS,
+    ICM42688_GFS_62_5DPS,
+    ICM42688_GFS_31_25DPS,
+    ICM42688_GFS_15_625DPS,
+};
+
+// 陀螺仪输出速率 (ODR)
+enum icm42688_godr {
+    ICM42688_GODR_32000HZ,
+    ICM42688_GODR_16000HZ,
+    ICM42688_GODR_8000HZ,
+    ICM42688_GODR_4000HZ,
+    ICM42688_GODR_2000HZ,
+    ICM42688_GODR_1000HZ,
+    ICM42688_GODR_200HZ,
+    ICM42688_GODR_100HZ,
+    ICM42688_GODR_50HZ,
+    ICM42688_GODR_25HZ,
+    ICM42688_GODR_12_5HZ,
+    ICM42688_GODR_X0HZ,
+    ICM42688_GODR_X1HZ,
+    ICM42688_GODR_X2HZ,
+    ICM42688_GODR_500HZ,
+};
+
+//==================================================================================================
+// 公开接口（由 IMU 层调用，用户一般不直接使用）
+//==================================================================================================
+
+/** @brief 初始化 ICM-42688（SPI 配置 + 寄存器设置 + 自检）
+ *  由 imu_init() 内部调用，用户无需单独调用
+ */
+void icm42688_init(void);
+
+/** @brief 读取加速度和角速度（物理单位，传感器坐标系）
+ *  @param data 输出结构体指针
+ *  一次 12 字节 SPI 突发读取，节省通信时间
+ */
+void icm42688_read_all(icm42688_data_t *data);
+
+/** @brief 读取陀螺仪原始 ADC 值（不含比例换算）
+ *  @param gx/gy/gz 输出 int16 原始值指针
+ *  供校准函数使用
+ */
+void icm42688_read_gyro_raw(int16_t *gx, int16_t *gy, int16_t *gz);
+
+/** @brief 返回当前陀螺仪比例系数 (LSB → °/s)
+ *  供校准函数计算零偏
+ */
+float icm42688_gyro_scale(void);
+
+//==================================================================================================
+// 寄存器地址
+//==================================================================================================
+
+// Bank 0
+#define ICM42688_DEVICE_CONFIG          0x11
+#define ICM42688_DRIVE_CONFIG           0x13
+#define ICM42688_INT_CONFIG             0x14
+#define ICM42688_FIFO_CONFIG            0x16
+#define ICM42688_TEMP_DATA1             0x1D
+#define ICM42688_TEMP_DATA0             0x1E
+#define ICM42688_ACCEL_DATA_X1          0x1F
+#define ICM42688_ACCEL_DATA_X0          0x20
+#define ICM42688_ACCEL_DATA_Y1          0x21
+#define ICM42688_ACCEL_DATA_Y0          0x22
+#define ICM42688_ACCEL_DATA_Z1          0x23
+#define ICM42688_ACCEL_DATA_Z0          0x24
+#define ICM42688_GYRO_DATA_X1           0x25
+#define ICM42688_GYRO_DATA_X0           0x26
+#define ICM42688_GYRO_DATA_Y1           0x27
+#define ICM42688_GYRO_DATA_Y0           0x28
+#define ICM42688_GYRO_DATA_Z1           0x29
+#define ICM42688_GYRO_DATA_Z0           0x2A
+#define ICM42688_TMST_FSYNCH            0x2B
+#define ICM42688_TMST_FSYNCL            0x2C
+#define ICM42688_INT_STATUS             0x2D
+#define ICM42688_FIFO_COUNTH            0x2E
+#define ICM42688_FIFO_COUNTL            0x2F
+#define ICM42688_FIFO_DATA              0x30
+#define ICM42688_APEX_DATA0             0x31
+#define ICM42688_APEX_DATA1             0x32
+#define ICM42688_APEX_DATA2             0x33
+#define ICM42688_APEX_DATA3             0x34
+#define ICM42688_APEX_DATA4             0x35
+#define ICM42688_APEX_DATA5             0x36
+#define ICM42688_INT_STATUS2            0x37
+#define ICM42688_INT_STATUS3            0x38
+#define ICM42688_SIGNAL_PATH_RESET      0x4B
+#define ICM42688_INTF_CONFIG0           0x4C
+#define ICM42688_INTF_CONFIG1           0x4D
+#define ICM42688_PWR_MGMT0              0x4E
+#define ICM42688_GYRO_CONFIG0           0x4F
+#define ICM42688_ACCEL_CONFIG0          0x50
+#define ICM42688_GYRO_CONFIG1           0x51
+#define ICM42688_GYRO_ACCEL_CONFIG0     0x52
+#define ICM42688_ACCEL_CONFIG1          0x53
+#define ICM42688_TMST_CONFIG            0x54
+#define ICM42688_APEX_CONFIG0           0x56
+#define ICM42688_SMD_CONFIG             0x57
+#define ICM42688_FIFO_CONFIG1           0x5F
+#define ICM42688_FIFO_CONFIG2           0x60
+#define ICM42688_FIFO_CONFIG3           0x61
+#define ICM42688_FSYNC_CONFIG           0x62
+#define ICM42688_INT_CONFIG0            0x63
+#define ICM42688_INT_CONFIG1            0x64
+#define ICM42688_INT_SOURCE0            0x65
+#define ICM42688_INT_SOURCE1            0x66
+#define ICM42688_INT_SOURCE3            0x68
+#define ICM42688_INT_SOURCE4            0x69
+#define ICM42688_FIFO_LOST_PKT0         0x6C
+#define ICM42688_FIFO_LOST_PKT1         0x6D
+#define ICM42688_SELF_TEST_CONFIG       0x70
+#define ICM42688_WHO_AM_I               0x75
+#define ICM42688_REG_BANK_SEL           0x76
+
+// Bank 1
+#define ICM42688_SENSOR_CONFIG0         0x03
+#define ICM42688_GYRO_CONFIG_STATIC2    0x0B
+#define ICM42688_GYRO_CONFIG_STATIC3    0x0C
+#define ICM42688_GYRO_CONFIG_STATIC4    0x0D
+#define ICM42688_GYRO_CONFIG_STATIC5    0x0E
+#define ICM42688_GYRO_CONFIG_STATIC6    0x0F
+#define ICM42688_GYRO_CONFIG_STATIC7    0x10
+#define ICM42688_GYRO_CONFIG_STATIC8    0x11
+#define ICM42688_GYRO_CONFIG_STATIC9    0x12
+#define ICM42688_GYRO_CONFIG_STATIC10   0x13
+#define ICM42688_XG_ST_DATA             0x5F
+#define ICM42688_YG_ST_DATA             0x60
+#define ICM42688_ZG_ST_DATA             0x61
+#define ICM42688_TMSTVAL0               0x62
+#define ICM42688_TMSTVAL1               0x63
+#define ICM42688_TMSTVAL2               0x64
+#define ICM42688_INTF_CONFIG4           0x7A
+#define ICM42688_INTF_CONFIG5           0x7B
+#define ICM42688_INTF_CONFIG6           0x7C
+
+#endif