443 lines
12 KiB
C
443 lines
12 KiB
C
#include "imu.h"
|
||
|
||
|
||
|
||
#define DCM_KP_ACC 0.400f //加速度补偿陀螺仪PI参数
|
||
#define DCM_KI_ACC 0.001f
|
||
|
||
#define DCM_KP_MAG 1.000f //磁力计补偿陀螺仪PI参数
|
||
#define DCM_KI_MAG 0.000f
|
||
|
||
|
||
#define IMU_SMALL_ANGLE 15.0f //满足水平状态的最小角度（单位deg）
|
||
|
||
#define SPIN_RATE_LIMIT 20 //旋转速率
|
||
|
||
|
||
//float Kp = 0.4f; /*比例增益*/
|
||
//float Ki = 0.001f; /*积分增益*/
|
||
//float exInt = 0.0f;
|
||
//float eyInt = 0.0f;
|
||
//float ezInt = 0.0f; /*积分误差累计*/
|
||
|
||
|
||
float imuAttitudeYaw;//范围：-180~180，用于上传到匿名上位机（支持范围-180~180）
|
||
static float q0 = 1.0f, q1 = 1.0f, q2 = 0.0f, q3 = 0.0f, q4 = 0.0f;//四元数
|
||
static float rMat[3][3];//四元数的旋转矩阵
|
||
static float smallAngleCosZ;//水平最小角余弦值
|
||
|
||
|
||
//旋转机体坐标系加速度到NEU坐标系
|
||
void imuTransformVectorBodyToEarth(Axis3f *v)
|
||
{
|
||
const float x = rMat[0][0] * v->x + rMat[0][1] * v->y + rMat[0][2] * v->z;
|
||
const float y = rMat[1][0] * v->x + rMat[1][1] * v->y + rMat[1][2] * v->z;
|
||
const float z = rMat[2][0] * v->x + rMat[2][1] * v->y + rMat[2][2] * v->z;
|
||
|
||
v->x = x;
|
||
v->y = -y;//
|
||
v->z = z;
|
||
}
|
||
|
||
//NEU坐标系加速度到NEU坐标系
|
||
void imuTransformVectorEarthToBody(Axis3f *v)
|
||
{
|
||
v->y = -v->y;
|
||
|
||
/* From earth frame to body frame */
|
||
const float x = rMat[0][0] * v->x + rMat[1][0] * v->y + rMat[2][0] * v->z;
|
||
const float y = rMat[0][1] * v->x + rMat[1][1] * v->y + rMat[2][1] * v->z;
|
||
const float z = rMat[0][2] * v->x + rMat[1][2] * v->y + rMat[2][2] * v->z;
|
||
|
||
v->x = x;
|
||
v->y = y;
|
||
v->z = z;
|
||
}
|
||
|
||
|
||
float degreesToRadians(int16_t degrees1)
|
||
{
|
||
return degrees1 * RAD;
|
||
}
|
||
|
||
|
||
//计算四元数的旋转矩阵
|
||
static void imuComputeRotationMatrix(void)
|
||
{
|
||
float q1q1 = q1 * q1;
|
||
float q2q2 = q2 * q2;
|
||
float q3q3 = q3 * q3;
|
||
|
||
float q0q1 = q0 * q1;
|
||
float q0q2 = q0 * q2;
|
||
float q0q3 = q0 * q3;
|
||
float q1q2 = q1 * q2;
|
||
float q1q3 = q1 * q3;
|
||
float q2q3 = q2 * q3;
|
||
|
||
rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
|
||
rMat[0][1] = 2.0f * (q1q2 + -q0q3);
|
||
rMat[0][2] = 2.0f * (q1q3 - -q0q2);
|
||
|
||
rMat[1][0] = 2.0f * (q1q2 - -q0q3);
|
||
rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
|
||
rMat[1][2] = 2.0f * (q2q3 + -q0q1);
|
||
|
||
rMat[2][0] = 2.0f * (q1q3 + -q0q2);
|
||
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
|
||
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
|
||
}
|
||
|
||
|
||
void imuInit(void)
|
||
{
|
||
|
||
smallAngleCosZ = cos_approx(degreesToRadians(IMU_SMALL_ANGLE));//最小倾角余弦值
|
||
imuComputeRotationMatrix();//计算四元数的旋转矩阵
|
||
}
|
||
|
||
static float invSqrt(float x)
|
||
{
|
||
return 1.0f / sqrtf(x);
|
||
}
|
||
|
||
|
||
//磁力计快速增益
|
||
static float imuMagFastPGainSaleFactor(void)
|
||
{
|
||
//四轴上电后磁力计融合需要一段时间
|
||
//为了快速融合，前100次使用快速增益
|
||
static int32_t FastPGainCount = 100;
|
||
if(FastPGainCount>0)//
|
||
{
|
||
FastPGainCount--;
|
||
return 10.0f;
|
||
}
|
||
else
|
||
return 1.0f;
|
||
}
|
||
|
||
|
||
//快速增益
|
||
static float imuACCFastPGainSaleFactor(void)
|
||
{
|
||
//四轴上电后ACC融合需要一段时间
|
||
//为了快速融合，前100次使用快速增益
|
||
static int32_t FastPGainCount = 100;
|
||
if(FastPGainCount>0)//
|
||
{
|
||
FastPGainCount--;
|
||
return 100.0f;
|
||
}
|
||
else
|
||
return 1.0f;
|
||
}
|
||
|
||
|
||
|
||
//计算四元数和旋转矩阵
|
||
void imuMahonyAHRSupdate(float gx, float gy, float gz,float ax, float ay, float az,float mx, float my, float mz,bool useMag,float dt)
|
||
{
|
||
static float integralAccX = 0.0f, integralAccY = 0.0f, integralAccZ = 0.0f; //加速度积分误差
|
||
static float integralMagX = 0.0f, integralMagY = 0.0f, integralMagZ = 0.0f; //磁力计积分误差
|
||
float ex, ey, ez;
|
||
|
||
//计算旋转速率(rad/s)
|
||
const float spin_rate_sq = sq(gx) + sq(gy) + sq(gz);
|
||
|
||
//Step 1: Yaw correction
|
||
if (useMag)
|
||
{
|
||
const float magMagnitudeSq = mx * mx + my * my + mz * mz;
|
||
float kpMag = DCM_KP_MAG * imuMagFastPGainSaleFactor();
|
||
|
||
if (magMagnitudeSq > 0.01f)
|
||
{
|
||
//单位化磁力计测量值
|
||
const float magRecipNorm = invSqrt(magMagnitudeSq);
|
||
mx *= magRecipNorm;
|
||
my *= magRecipNorm;
|
||
mz *= magRecipNorm;
|
||
|
||
// printf(" mx:%f ",mx);
|
||
// printf(" my:%f ",my);
|
||
// printf(" mz:%f ",mz);
|
||
// printf(" magRecipNorm:%f ",magRecipNorm);
|
||
|
||
// printf(" \r\n ");
|
||
|
||
|
||
|
||
//计算X\Y方向的磁通，磁北方向磁通
|
||
const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
|
||
const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
|
||
const float bx = sqrtf(hx * hx + hy * hy);
|
||
|
||
//磁力计误差是估计磁北和测量磁北之间的交叉乘积
|
||
const float ez_ef = -(hy * bx);
|
||
|
||
//旋转误差到机体坐标系
|
||
ex = rMat[2][0] * ez_ef;
|
||
ey = rMat[2][1] * ez_ef;
|
||
ez = rMat[2][2] * ez_ef;
|
||
}
|
||
else
|
||
{
|
||
ex = 0;
|
||
ey = 0;
|
||
ez = 0;
|
||
}
|
||
|
||
//累计误差补偿
|
||
if (DCM_KI_MAG > 0.0f)
|
||
{
|
||
//如果旋转速率大于限制值则停止积分
|
||
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)))
|
||
{
|
||
integralMagX += DCM_KI_MAG * ex * dt;
|
||
integralMagY += DCM_KI_MAG * ey * dt;
|
||
integralMagZ += DCM_KI_MAG * ez * dt;
|
||
|
||
gx += integralMagX;
|
||
gy += integralMagY;
|
||
gz += integralMagZ;
|
||
}
|
||
}
|
||
|
||
//误差补偿
|
||
gx += kpMag * ex;
|
||
gy += kpMag * ey;
|
||
gz += kpMag * ez;
|
||
}
|
||
|
||
|
||
|
||
//Step 2: Roll and pitch correction
|
||
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))//加速度计输出有效时,利用加速度计补偿陀螺仪
|
||
{
|
||
//单位化加速计测量值
|
||
const float accRecipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||
ax *= accRecipNorm;
|
||
ay *= accRecipNorm;
|
||
az *= accRecipNorm;
|
||
|
||
//加速计读取的方向与重力加速计方向的差值，用向量叉乘计算
|
||
ex = (ay * rMat[2][2] - az * rMat[2][1]);
|
||
ey = (az * rMat[2][0] - ax * rMat[2][2]);
|
||
ez = (ax * rMat[2][1] - ay * rMat[2][0]);
|
||
|
||
//累计误差补偿
|
||
if (DCM_KI_ACC > 0.0f)
|
||
{
|
||
//如果旋转速率大于限制值则停止积分
|
||
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)))
|
||
{
|
||
/*误差累计，与积分常数相乘*/
|
||
integralAccX += DCM_KI_ACC * ex * dt;
|
||
integralAccY += DCM_KI_ACC * ey * dt;
|
||
integralAccZ += DCM_KI_ACC * ez * dt;
|
||
|
||
gx += integralAccX;
|
||
gy += integralAccY;
|
||
gz += integralAccZ;
|
||
}
|
||
}
|
||
|
||
//误差补偿 用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量
|
||
float ak = imuACCFastPGainSaleFactor();
|
||
//printf(" ak:%f ",ak);
|
||
gx += DCM_KP_ACC * ex * ak;
|
||
gy += DCM_KP_ACC * ey * ak;
|
||
gz += DCM_KP_ACC * ez * ak;
|
||
|
||
}
|
||
|
||
//一阶近似算法，四元数运动学方程的离散化形式和积分
|
||
gx *= (0.5f * dt);
|
||
gy *= (0.5f * dt);
|
||
gz *= (0.5f * dt);
|
||
|
||
const float qa = q0;
|
||
const float qb = q1;
|
||
const float qc = q2;
|
||
q0 += (-qb * gx - qc * gy - q3 * gz);
|
||
q1 += (qa * gx + qc * gz - q3 * gy);
|
||
q2 += (qa * gy - qb * gz + q3 * gx);
|
||
q3 += (qa * gz + qb * gy - qc * gx);
|
||
|
||
//单位化四元数
|
||
const float quatRecipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||
q0 *= quatRecipNorm;
|
||
q1 *= quatRecipNorm;
|
||
q2 *= quatRecipNorm;
|
||
q3 *= quatRecipNorm;
|
||
|
||
//计算四元数的旋转矩阵
|
||
imuComputeRotationMatrix();
|
||
}
|
||
|
||
|
||
|
||
//更新欧拉角
|
||
static void imuUpdateEulerAngles(attitude_t *attitude)
|
||
{
|
||
attitude->roll = RADIANS_TO_DEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
|
||
attitude->pitch = RADIANS_TO_DEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));//arcsin = 0.5PI - arccos
|
||
attitude->yaw = RADIANS_TO_DEGREES(atan2_approx(rMat[1][0], rMat[0][0]));
|
||
|
||
imuAttitudeYaw = attitude->yaw;
|
||
|
||
if (attitude->yaw < 0.0f)//转换位0~360
|
||
attitude->yaw += 360.0f;
|
||
|
||
//更新最小倾角状态
|
||
if (rMat[2][2] > smallAngleCosZ)
|
||
ENABLE_STATE(SMALL_ANGLE);
|
||
else
|
||
DISABLE_STATE(SMALL_ANGLE);
|
||
|
||
}
|
||
|
||
|
||
//四元数和欧拉角计算
|
||
void imuUpdateAttitude(const sensorData_t *sensorData, state_t *state, float dt)
|
||
{
|
||
bool useMag = 0;//compassIsHealthy();
|
||
|
||
Axis3f gyro = sensorData->gyro;
|
||
Axis3f acc = sensorData->acc;
|
||
Axis3f mag = sensorData->mag;
|
||
|
||
|
||
|
||
//角速度单位由度转为弧度
|
||
gyro.x = gyro.x * DEG2RAD;
|
||
gyro.y = gyro.y * DEG2RAD;
|
||
gyro.z = gyro.z * DEG2RAD;
|
||
|
||
|
||
//计算四元数和旋转矩阵
|
||
imuMahonyAHRSupdate(gyro.x, gyro.y, gyro.z,
|
||
acc.x, acc.y, acc.z,
|
||
mag.x, mag.y, mag.z,
|
||
useMag,dt);
|
||
|
||
//计算欧拉角
|
||
imuUpdateEulerAngles(&state->attitude);
|
||
|
||
}
|
||
|
||
|
||
|
||
float beta = 0.2;//0.04 加速计和磁力计补偿因子，数值越大纠正越快，越不稳定
|
||
|
||
void MadgwickQuaternionUpdate(Axis3f acc, Axis3f gyro, Axis3f mag, attitude_t *attitude , float dt)
|
||
{
|
||
float norm;
|
||
float hx, hy, _2bx, _2bz;
|
||
float s1, s2, s3, s4;
|
||
float qDot1, qDot2, qDot3, qDot4;
|
||
|
||
|
||
//提前计算好下面要用到的数值防止重复计算，减少运算量
|
||
float _2q1mx;
|
||
float _2q1my;
|
||
float _2q1mz;
|
||
float _2q2mx;
|
||
float _4bx;
|
||
float _4bz;
|
||
float _2q1 = 2.0f * q1;
|
||
float _2q2 = 2.0f * q2;
|
||
float _2q3 = 2.0f * q3;
|
||
float _2q4 = 2.0f * q4;
|
||
float _2q1q3 = 2.0f * q1 * q3;
|
||
float _2q3q4 = 2.0f * q3 * q4;
|
||
float q1q1 = q1 * q1;
|
||
float q1q2 = q1 * q2;
|
||
float q1q3 = q1 * q3;
|
||
float q1q4 = q1 * q4;
|
||
float q2q2 = q2 * q2;
|
||
float q2q3 = q2 * q3;
|
||
float q2q4 = q2 * q4;
|
||
float q3q3 = q3 * q3;
|
||
float q3q4 = q3 * q4;
|
||
float q4q4 = q4 * q4;
|
||
|
||
|
||
//度转弧度
|
||
gyro.x = gyro.x * DEG2RAD; /* 度转弧度 */
|
||
gyro.y = gyro.y * DEG2RAD;
|
||
gyro.z = gyro.z * DEG2RAD;
|
||
|
||
//单位化加速度向量
|
||
norm = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
|
||
if (norm == 0.0f) return; // handle NaN
|
||
norm = 1.0f/norm;
|
||
acc.x *= norm;
|
||
acc.y *= norm;
|
||
acc.z *= norm;
|
||
|
||
//单位化磁力计向量
|
||
norm = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
|
||
if (norm == 0.0f) return; // handle NaN
|
||
norm = 1.0f/norm;
|
||
mag.x *= norm;
|
||
mag.y *= norm;
|
||
mag.z *= norm;
|
||
|
||
//提前计算好下面要用到的值
|
||
_2q1mx = 2.0f * q1 * mag.x;
|
||
_2q1my = 2.0f * q1 * mag.y;
|
||
_2q1mz = 2.0f * q1 * mag.z;
|
||
_2q2mx = 2.0f * q2 * mag.x;
|
||
//磁力计从机体到地球
|
||
hx = mag.x * q1q1 - _2q1my * q4 + _2q1mz * q3 + mag.x * q2q2 + _2q2 * mag.y * q3 + _2q2 * mag.z * q4 - mag.x * q3q3 - mag.x * q4q4;
|
||
hy = _2q1mx * q4 + mag.y * q1q1 - _2q1mz * q2 + _2q2mx * q3 - mag.y * q2q2 + mag.y * q3q3 + _2q3 * mag.z * q4 - mag.y * q4q4;
|
||
/*让导航坐标系中X轴指向正北方*/
|
||
_2bx = sqrtf(hx * hx + hy * hy);
|
||
_2bz = -_2q1mx * q3 + _2q1my * q2 + mag.z * q1q1 + _2q2mx * q4 - mag.z * q2q2 + _2q3 * mag.y * q4 - mag.z * q3q3 + mag.z * q4q4;
|
||
_4bx = 2.0f * _2bx;
|
||
_4bz = 2.0f * _2bz;
|
||
|
||
//梯度下降法计算纠正误差
|
||
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q2 * (2.0f * q1q2 + _2q3q4 - acc.y) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
|
||
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q1 * (2.0f * q1q2 + _2q3q4 - acc.y) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - acc.z) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
|
||
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q4 * (2.0f * q1q2 + _2q3q4 - acc.y) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - acc.z) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
|
||
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q3 * (2.0f * q1q2 + _2q3q4 - acc.y) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
|
||
//单位化计算好的误差向量
|
||
norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);
|
||
norm = 1.0f/norm;
|
||
s1 *= norm;
|
||
s2 *= norm;
|
||
s3 *= norm;
|
||
s4 *= norm;
|
||
|
||
// 将计算好的误差向量补偿到四元数
|
||
qDot1 = 0.5f * (-q2 * gyro.x - q3 * gyro.y - q4 * gyro.z) - beta * s1;
|
||
qDot2 = 0.5f * (q1 * gyro.x + q3 * gyro.z - q4 * gyro.y) - beta * s2;
|
||
qDot3 = 0.5f * (q1 * gyro.y - q2 * gyro.z + q4 * gyro.x) - beta * s3;
|
||
qDot4 = 0.5f * (q1 * gyro.z + q2 * gyro.y - q3 * gyro.x) - beta * s4;
|
||
|
||
//更新四元数的值
|
||
q1 += qDot1 * dt;
|
||
q2 += qDot2 * dt;
|
||
q3 += qDot3 * dt;
|
||
q4 += qDot4 * dt;
|
||
norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
|
||
norm = 1.0f/norm;
|
||
q1 = q1 * norm;
|
||
q2 = q2 * norm;
|
||
q3 = q3 * norm;
|
||
q4 = q4 * norm;
|
||
|
||
//四元数转换为欧拉角
|
||
attitude->yaw = atan2(2.0f * (q2 * q3 + q1 * q4), q1 * q1 + q2 * q2 - q3 * q3 - q4 * q4) * RAD2DEG;
|
||
attitude->pitch = -asin(2.0f * (q2 * q4 - q1 * q3)) * RAD2DEG;
|
||
attitude->roll = atan2(2.0f * (q1 * q2 + q3 * q4), q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4) * RAD2DEG;
|
||
|
||
}
|
||
|
||
|
||
|
||
|