32343
843447835d
- IPS200 display menu with real-time status and parameter settings - Camera image processing and line tracking - IMU attitude solution and PID control - WiFi SPI oscilloscope data transmission
271 lines
9.3 KiB
C
271 lines
9.3 KiB
C
/*********************************************************************************************************************
|
|
* TC264 智能车 - 姿态解算模块 (attitude_solution.c)
|
|
*
|
|
* 基于 Mahony 互补滤波算法：
|
|
* 陀螺仪积分得到姿态角，加速度计修正陀螺零漂。
|
|
* 使用四元数表示姿态，避免万向节死锁。
|
|
* 移植自 RT1064 开源库中 ICM_AHRSupdate 实现。
|
|
*
|
|
* 调用周期: 5ms (在 isr.c 的 CCU60_CH0 中断中调用, 陀螺仪校准完成后)
|
|
*
|
|
* IMU660RB 转换因子 (2000DPS / 8G):
|
|
* 陀螺仪: raw / 14.3 = °/s, * π/180 = rad/s
|
|
* 加速度: raw / 4098 = G (m/s^2单位)
|
|
*********************************************************************************************************************/
|
|
|
|
#include "zf_common_typedef.h"
|
|
#include "zf_driver_delay.h"
|
|
#include "zf_device_imu660rb.h"
|
|
#include "attitude_solution.h"
|
|
|
|
/* ---------------------------------------------------------------------------
|
|
* 常数与可调参数 (匹配 RT1064 参考实现)
|
|
* --------------------------------------------------------------------------- */
|
|
#define DELTA_T 0.005f /* 姿态更新周期 (秒), 匹配 5ms ISR */
|
|
#define DEG_TO_RAD 0.01745329252f /* π / 180 */
|
|
#define RAD_TO_DEG 57.2957795131f /* 180 / π */
|
|
|
|
#define KP 0.17f /* 加速度计收敛比例增益 (同 RT1064) */
|
|
#define KI 0.004f /* 陀螺仪零漂积分增益 (同 RT1064) */
|
|
#define ACC_ALPHA 0.3f /* 加速度低通滤波系数 (同 RT1064) */
|
|
|
|
/* ---------------------------------------------------------------------------
|
|
* 全局姿态角 (外部引用)
|
|
* --------------------------------------------------------------------------- */
|
|
euler_param_t eulerAngle;
|
|
|
|
/* ---------------------------------------------------------------------------
|
|
* 静态变量 — 四元数 & 误差积分 & 加速度滤波状态
|
|
* --------------------------------------------------------------------------- */
|
|
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
|
|
static float i_ex = 0.0f, i_ey = 0.0f, i_ez = 0.0f;
|
|
static float acc_fx = 0.0f, acc_fy = 0.0f, acc_fz = 0.0f;
|
|
|
|
/* ---------------------------------------------------------------------------
|
|
* 快速反平方根 (Quake III 算法, 两次牛顿迭代)
|
|
* --------------------------------------------------------------------------- */
|
|
static float fast_rsqrt(float x)
|
|
{
|
|
float halfx = 0.5f * x;
|
|
float y = x;
|
|
long i = *(long *)&y;
|
|
i = 0x5f3759df - (i >> 1);
|
|
y = *(float *)&i;
|
|
y = y * (1.5f - halfx * y * y);
|
|
y = y * (1.5f - halfx * y * y);
|
|
return y;
|
|
}
|
|
|
|
/* ---------------------------------------------------------------------------
|
|
* 姿态解算初始化
|
|
* 多次读取加速度计取平均, 用加速度计计算初始 pitch/roll, 设置正确的初始四元数
|
|
* --------------------------------------------------------------------------- */
|
|
void attitude_init(void)
|
|
{
|
|
float ax_sum, ay_sum, az_sum;
|
|
float ax, ay, az;
|
|
float norm, mag;
|
|
float roll_init, pitch_init;
|
|
float cr, sr, cp, sp;
|
|
int i;
|
|
|
|
/* 传感器上电后需要时间稳定, 丢弃前若干次读数 */
|
|
for(i = 0; i < 10; i++)
|
|
{
|
|
imu660rb_get_acc();
|
|
system_delay_ms(5);
|
|
}
|
|
|
|
/* 多次采样取平均, 提高初始姿态估计的稳健性 */
|
|
ax_sum = 0.0f; ay_sum = 0.0f; az_sum = 0.0f;
|
|
for(i = 0; i < 20; i++)
|
|
{
|
|
imu660rb_get_acc();
|
|
ax_sum += imu660rb_acc_transition(imu660rb_acc_x);
|
|
ay_sum += imu660rb_acc_transition(imu660rb_acc_y);
|
|
az_sum += imu660rb_acc_transition(imu660rb_acc_z);
|
|
system_delay_ms(5);
|
|
}
|
|
ax = ax_sum / 20.0f;
|
|
ay = ay_sum / 20.0f;
|
|
az = az_sum / 20.0f;
|
|
|
|
/* 校验加速度幅值是否在合理范围 (0.6G ~ 1.6G), 若异常则回退到默认值 */
|
|
mag = ax * ax + ay * ay + az * az;
|
|
if(mag < 0.36f || mag > 2.56f)
|
|
{
|
|
ax = 0.0f;
|
|
ay = 0.0f;
|
|
az = 1.0f;
|
|
}
|
|
else
|
|
{
|
|
norm = fast_rsqrt(mag);
|
|
ax *= norm;
|
|
ay *= norm;
|
|
az *= norm;
|
|
}
|
|
|
|
/* 从重力矢量估计初始 roll 和 pitch */
|
|
roll_init = atan2f(ay, az);
|
|
pitch_init = atan2f(-ax, sqrtf(ay * ay + az * az));
|
|
|
|
/* 欧拉角 -> 四元数 */
|
|
cr = cosf(roll_init * 0.5f);
|
|
sr = sinf(roll_init * 0.5f);
|
|
cp = cosf(pitch_init * 0.5f);
|
|
sp = sinf(pitch_init * 0.5f);
|
|
|
|
q0 = cr * cp;
|
|
q1 = sr * cp;
|
|
q2 = cr * sp;
|
|
q3 = -sr * sp;
|
|
|
|
/* 四元数归一化 */
|
|
norm = fast_rsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
|
q0 *= norm;
|
|
q1 *= norm;
|
|
q2 *= norm;
|
|
q3 *= norm;
|
|
|
|
/* 复位积分项 */
|
|
i_ex = 0.0f; i_ey = 0.0f; i_ez = 0.0f;
|
|
|
|
/* 加速度滤波状态初始化为首次有效读数 (避免从零开始的衰减过程) */
|
|
acc_fx = ax;
|
|
acc_fy = ay;
|
|
acc_fz = az;
|
|
|
|
/* 根据初始四元数计算初始欧拉角 */
|
|
{
|
|
float q0q0 = q0 * q0;
|
|
float q0q1 = q0 * q1;
|
|
float q0q2 = q0 * q2;
|
|
float q0q3 = q0 * q3;
|
|
float q1q1 = q1 * q1;
|
|
float q1q2 = q1 * q2;
|
|
float q1q3 = q1 * q3;
|
|
float q2q2 = q2 * q2;
|
|
float q2q3 = q2 * q3;
|
|
float q3q3 = q3 * q3;
|
|
|
|
eulerAngle.pitch = asinf(-2.0f * q1q3 + 2.0f * q0q2) * RAD_TO_DEG;
|
|
eulerAngle.roll = atan2f(2.0f * q2q3 + 2.0f * q0q1,
|
|
-2.0f * q1q1 - 2.0f * q2q2 + 1.0f) * RAD_TO_DEG;
|
|
eulerAngle.yaw = 0.0f; /* 无磁力计，yaw 从 0 开始 */
|
|
}
|
|
}
|
|
|
|
/* ---------------------------------------------------------------------------
|
|
* 姿态更新 — 每 5ms 调用一次 (陀螺仪校准完成后)
|
|
* 读取已去除零偏的陀螺仪数据 + 原始加速度计数据,
|
|
* 运行 Mahony 互补滤波, 输出欧拉角
|
|
* --------------------------------------------------------------------------- */
|
|
void attitude_update(void)
|
|
{
|
|
float gx, gy, gz;
|
|
float ax, ay, az;
|
|
float norm;
|
|
float vx, vy, vz;
|
|
float ex, ey, ez;
|
|
float halfT = 0.5f * DELTA_T;
|
|
|
|
/* --- 1. 获取传感器数据并转换为物理量 --- */
|
|
/* 陀螺仪: ISR 已减零偏 (LSB) -> °/s -> rad/s, 量程因子由驱动层统一管理 */
|
|
gx = imu660rb_gyro_transition(imu660rb_gyro_x) * DEG_TO_RAD;
|
|
gy = imu660rb_gyro_transition(imu660rb_gyro_y) * DEG_TO_RAD;
|
|
gz = imu660rb_gyro_transition(imu660rb_gyro_z) * DEG_TO_RAD;
|
|
|
|
/* 加速度计: LSB -> G, 一阶低通滤波抑制振动噪声 (同 RT1064 alpha=0.3) */
|
|
ax = imu660rb_acc_transition(imu660rb_acc_x);
|
|
ay = imu660rb_acc_transition(imu660rb_acc_y);
|
|
az = imu660rb_acc_transition(imu660rb_acc_z);
|
|
acc_fx = ACC_ALPHA * ax + (1.0f - ACC_ALPHA) * acc_fx;
|
|
acc_fy = ACC_ALPHA * ay + (1.0f - ACC_ALPHA) * acc_fy;
|
|
acc_fz = ACC_ALPHA * az + (1.0f - ACC_ALPHA) * acc_fz;
|
|
ax = acc_fx;
|
|
ay = acc_fy;
|
|
az = acc_fz;
|
|
|
|
/* --- 2. 加速度归一化 (单位重力向量) --- */
|
|
norm = fast_rsqrt(ax * ax + ay * ay + az * az);
|
|
ax *= norm;
|
|
ay *= norm;
|
|
az *= norm;
|
|
|
|
/* --- 3. 估算重力分量 (从当前四元数) --- */
|
|
vx = 2.0f * (q1 * q3 - q0 * q2);
|
|
vy = 2.0f * (q0 * q1 + q2 * q3);
|
|
vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
|
|
|
|
/* --- 3.5 重力方向检测: 若估计重力与实测重力反平行 (点积<0),
|
|
* 则四元数处于翻转平衡点, 绕X轴旋转180°拉回正确方向 --- */
|
|
if(ax * vx + ay * vy + az * vz < 0.0f)
|
|
{
|
|
float t0 = -q1, t1 = q0, t2 = q3, t3 = -q2;
|
|
q0 = t0; q1 = t1; q2 = t2; q3 = t3;
|
|
vx = 2.0f * (q1 * q3 - q0 * q2);
|
|
vy = 2.0f * (q0 * q1 + q2 * q3);
|
|
vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
|
|
}
|
|
|
|
/* --- 4. 叉积求重力误差 --- */
|
|
ex = ay * vz - az * vy;
|
|
ey = az * vx - ax * vz;
|
|
ez = ax * vy - ay * vx;
|
|
|
|
/* --- 5. PI 修正陀螺仪 --- */
|
|
i_ex += halfT * ex;
|
|
i_ey += halfT * ey;
|
|
i_ez += halfT * ez;
|
|
gx += KP * ex + KI * i_ex;
|
|
gy += KP * ey + KI * i_ey;
|
|
gz += KP * ez + KI * i_ez;
|
|
|
|
/* --- 6. 一阶龙格库塔更新四元数 --- */
|
|
{
|
|
float dq0 = -q1 * gx - q2 * gy - q3 * gz;
|
|
float dq1 = q0 * gx + q2 * gz - q3 * gy;
|
|
float dq2 = q0 * gy - q1 * gz + q3 * gx;
|
|
float dq3 = q0 * gz + q1 * gy - q2 * gx;
|
|
|
|
q0 += dq0 * halfT;
|
|
q1 += dq1 * halfT;
|
|
q2 += dq2 * halfT;
|
|
q3 += dq3 * halfT;
|
|
}
|
|
|
|
/* --- 7. 四元数归一化 --- */
|
|
norm = fast_rsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
|
q0 *= norm;
|
|
q1 *= norm;
|
|
q2 *= norm;
|
|
q3 *= norm;
|
|
|
|
/* --- 8. 四元数 -> 欧拉角 (°) --- */
|
|
{
|
|
float q0q0 = q0 * q0;
|
|
float q0q1 = q0 * q1;
|
|
float q0q2 = q0 * q2;
|
|
float q0q3 = q0 * q3;
|
|
float q1q1 = q1 * q1;
|
|
float q1q2 = q1 * q2;
|
|
float q1q3 = q1 * q3;
|
|
float q2q2 = q2 * q2;
|
|
float q2q3 = q2 * q3;
|
|
float q3q3 = q3 * q3;
|
|
|
|
eulerAngle.pitch = asinf(-2.0f * q1q3 + 2.0f * q0q2) * RAD_TO_DEG;
|
|
eulerAngle.roll = atan2f(2.0f * q2q3 + 2.0f * q0q1,
|
|
-2.0f * q1q1 - 2.0f * q2q2 + 1.0f) * RAD_TO_DEG;
|
|
eulerAngle.yaw = atan2f(2.0f * q1q2 + 2.0f * q0q3,
|
|
-2.0f * q2q2 - 2.0f * q3q3 + 1.0f) * RAD_TO_DEG;
|
|
}
|
|
|
|
/* --- 9. Yaw 归一化到 [0, 360) --- */
|
|
if (eulerAngle.yaw >= 360.0f)
|
|
eulerAngle.yaw -= 360.0f;
|
|
else if (eulerAngle.yaw < 0.0f)
|
|
eulerAngle.yaw += 360.0f;
|
|
}
|