ICM42688驱动与Mahony解算到PID控制

一辆小车真正跑起来，靠的不只是“给电机一个 PWM”。主控需要知道轮子转了多少、车体偏了多少、地面引导线在哪里，再把这些信息变成下一次控制量。F446CAR 工程已经搭好了这条链路的大部分骨架：STM32F446 负责调度，四路 PWM 驱动电机，四组定时器读取编码器，八路灰度传感器观察地面，IMU 则提供姿态信息。

uwTick：主循环里的非阻塞状态机

__IO uint32_t GrayTick;
void GRAY_Proc(void)
{
    if (uwTick - GrayTick < 10) return;
    GrayTick = uwTick;
    getGrayscaleData();
}

__IO uint32_t OLEDTick;
void OLED_Proc(void)
{
    if (uwTick - OLEDTick < 40) return;
    OLEDTick = uwTick;
    OLED_Printf(0, 0, OLED_8X16, "Tick:%d", uwTick);
    OLED_Printf(0, 16, OLED_8X16, "Yaw:%.2f", yaw);
}

uwTick 由 SysTick 每毫秒递增。每个 Proc 函数先计算“当前时刻减去上次执行时刻”，间隔没到就立刻返回：灰度采样保持 10 ms 周期，OLED 则以 40 ms 周期刷新。主循环因此不会停在 HAL_Delay() 里等待，UART、按键、传感器和显示可以协作运行。这里使用 uint32_t 无符号减法还有一个细节：即便 uwTick 约 49.7 天后回绕，只要任务周期远小于 2³¹ ms，差值判断依然成立。

目前这些任务只有“等待下一周期 / 执行”两个隐含状态；接入 ICM42688 时可以沿用相同模式，把复位、等待 200 µs、检查 WHO_AM_I、开启传感器、等待陀螺稳定和正常采样拆成显式状态。每次调用只推进一步，既满足芯片时序，也不会让初始化过程锁住整辆车。这比在驱动里连续堆叠延时更容易调试，也能让故障重试和超时退出拥有明确位置。

ICM42688：一次 SPI 读取到底发生了什么

如果把 MPU6050 换成 ICM42688-P，最直接的变化是把软件 I²C 改成硬件 SPI。ICM42688-P 支持 3 线或 4 线 SPI，默认兼容 Mode 0 与 Mode 3，数据高位先行，最高 SCLK 为 24 MHz。每次事务都由 CS 拉低开始：第一个字节的 bit7 是读写位，读寄存器时置 1，写寄存器时置 0，后七位才是寄存器地址。

#define ICM42688_READ_BIT  0x80U

static HAL_StatusTypeDef icm42688_read(
    uint8_t reg, uint8_t *data, uint16_t length)
{
    uint8_t address = reg | ICM42688_READ_BIT;
    HAL_StatusTypeDef status;

    // 一次 CS 低电平覆盖地址与全部返回数据。
    HAL_GPIO_WritePin(ICM_CS_GPIO_Port, ICM_CS_Pin, GPIO_PIN_RESET);
    status = HAL_SPI_Transmit(&hspi1, &address, 1, 10);
    if (status == HAL_OK) {
        status = HAL_SPI_Receive(&hspi1, data, length, 10);
    }
    HAL_GPIO_WritePin(ICM_CS_GPIO_Port, ICM_CS_Pin, GPIO_PIN_SET);
    return status;
}

上电后应先软复位，再读取 Bank 0 的 WHO_AM_I（地址 0x75）；ICM42688-P 的默认值应为 0x47。随后设置 GYRO_CONFIG0、ACCEL_CONFIG0 的量程与输出数据率，再通过 PWR_MGMT0 让陀螺仪和加速度计进入 Low Noise 模式。芯片从 OFF 切换到工作模式后的 200 µs 内不要继续写寄存器，陀螺仪还需要至少保持开启 45 ms。把这些等待条件写进初始化状态机，比散落几个“经验延时”更可靠。

uint8_t raw[12];
icm42688_read(0x1F, raw, sizeof(raw));

// ACCEL_DATA_X1 开始依次排列 accel XYZ、gyro XYZ。
int16_t ax = (int16_t)(((uint16_t)raw[0]  << 8) | raw[1]);
int16_t ay = (int16_t)(((uint16_t)raw[2]  << 8) | raw[3]);
int16_t az = (int16_t)(((uint16_t)raw[4]  << 8) | raw[5]);
int16_t gx = (int16_t)(((uint16_t)raw[6]  << 8) | raw[7]);
int16_t gy = (int16_t)(((uint16_t)raw[8]  << 8) | raw[9]);
int16_t gz = (int16_t)(((uint16_t)raw[10] << 8) | raw[11]);

这次 12 字节 burst read 很关键。现有 MPU6050_GetData() 为每个高低字节分别发起一次软件 I²C 读事务，不仅慢，也更容易让六个轴来自略有差异的采样时刻。ICM42688 的连续寄存器和同步读属性允许一次拿到同一组数据；采样率继续提高时，则应考虑 DATA_RDY 中断或 FIFO，避免主循环轮询造成抖动。

Mahony：用重力方向纠正陀螺仪

陀螺仪给出角速度，短时间内响应快，但对它积分会把零偏也一起积进去，姿态会慢慢漂移。加速度计在静止或匀速时能看到重力方向，长期参考稳定，却会被震动和车体加减速污染。Mahony 的核心不是简单加权平均，而是让当前四元数预测一个“理论重力方向”，再与加速度计测到的方向做叉积，用这个误差去修正角速度。

// 测量重力与四元数预测重力都先归一化。
norm = invSqrt(ax * ax + ay * ay + az * az);
ax *= norm; ay *= norm; az *= norm;

vx = 2.0f * (q1 * q3 - q0 * q2);
vy = 2.0f * (q0 * q1 + q2 * q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

// 叉积给出“应该往哪个方向修正”。
ex = ay * vz - az * vy;
ey = az * vx - ax * vz;
ez = ax * vy - ay * vx;

gx += Kp * ex + exInt;
gy += Kp * ey + eyInt;
gz += Kp * ez + ezInt;

q0 += (-q1*gx - q2*gy - q3*gz) * half_cycle_T;
q1 += ( q0*gx + q2*gz - q3*gy) * half_cycle_T;

Kp 决定姿态被拉回重力方向的速度：过小会显得软、漂移恢复慢，过大会把底盘震动带进姿态。Ki 用来长期估计陀螺零偏，但在小车频繁加减速时也更容易积累错误，因此常从 0 或很小的值开始。真正比“调一组神奇参数”更重要的是 dt：传感器 ODR、定时器更新周期和算法里的采样周期必须一致。

从姿态量到 PID 控制量

控制器需要先明确自己在控制什么。每个轮子的编码器适合构成速度内环，目标速度减去实测速度得到误差；Mahony 输出的 yaw 或 yaw rate 可以构成航向外环，算出的修正量再以相反符号叠加到左右轮。这样姿态解算不会直接粗暴地覆盖电机 PWM，而是通过目标轮速影响底盘。对于循迹小车，还可以由灰度误差产生航向目标，再交给航向环与速度环逐层完成。

typedef struct {
    float kp, ki, kd;
    float integral;
    float previous_error;
    float integral_limit;
    float output_limit;
} PID_t;

float PID_Update(PID_t *pid, float target, float measure, float dt)
{
    const float error = target - measure;
    const float derivative = (error - pid->previous_error) / dt;

    pid->integral += error * dt;
    pid->integral = clamp(pid->integral,
                          -pid->integral_limit,
                           pid->integral_limit);

    float output = pid->kp * error
                 + pid->ki * pid->integral
                 + pid->kd * derivative;

    pid->previous_error = error;
    return clamp(output, -pid->output_limit, pid->output_limit);
}

PID_Update() 必须与编码器采样保持固定 dt；若继续沿用 uwTick 调度，可以让 MOTOR_Proc() 每 10 ms 执行一次，再把 dt 固定传入 0.01 s。积分限幅用于抑制电机饱和时的 wind-up，输出限幅则必须与 motorSet() 接受的 PWM 范围一致。Kd 对编码器量化噪声敏感，实际调参通常先令 Ki、Kd 为 0 调通 Kp，再逐步加入 Ki 消除静差，最后只在确有必要时使用带低通的微分项。

六轴 IMU 无法凭空得到稳定航向

ICM42688 是六轴 IMU，没有磁力计。重力只约束 roll 和 pitch，绕重力方向旋转时，yaw 误差无法从加速度计中被观测到。因此 IMU 版 Mahony 的 yaw 仍会随陀螺零偏漂移。F446CAR 当前恰好主要显示和使用 yaw，这意味着迁移时不能把“姿态融合”误解成“航向永不漂”。可选办法包括加入磁力计、融合轮速里程计、利用循迹线约束航向，或像现有 KEY_Proc() 那样提供明确的零点重置。

把 ICM42688 真正接入这辆车

• 在 CubeMX 中启用硬件 SPI 与独立 CS GPIO；先用低速 Mode 0 验证 WHO_AM_I=0x47，再逐步提高时钟。
• 定义唯一的轴向映射，把芯片坐标转换成车体前、左、上的坐标，避免靠交换变量“试到能用”。
• 让 accel 与 gyro 使用一致 ODR，并由 DATA_RDY 中断或固定定时器触发一次完整 burst read。
• 静止收集一段陀螺数据估计零偏；只有通过静止判定时才更新 bias，避免把真实转动当成误差。
• 只保留一套 Mahony 实现，把 dt 作为参数传入；先 Ki=0 调 Kp，再观察长时间静止漂移决定是否加入积分。
• 把姿态、编码器和灰度传感器放进同一个控制时间轴，最后再建立速度环、航向环或循迹控制。

这条链路最终可以概括为：SPI 把物理运动变成可信的六轴数值，标定与坐标映射把数值变成车体语义，Mahony 把短期灵敏的陀螺仪与长期稳定的重力方向合成四元数，控制器再把姿态和轮速变成电机 PWM。每一层都不神秘，但每一层的时间基准、单位和方向都必须对得上。