264 lines
7.9 KiB
C
264 lines
7.9 KiB
C
/******************************************************************************************************
|
|
* Update the PID parameters.
|
|
*
|
|
* @param[in] pid A pointer to the pid object.
|
|
* @param[in] measured The measured value
|
|
* @param[in] updateError Set to TRUE if error should be calculated.
|
|
* Set to False if pidSetError() has been used.
|
|
* @return PID algorithm output
|
|
*******************************************************************************************************/
|
|
#include "imu.h"
|
|
#include "myMath.h"
|
|
#include <math.h>
|
|
|
|
|
|
static float NormAcc;
|
|
|
|
|
|
//float ex_int = 0, ey_int = 0, ez_int = 0; //X、Y、Z轴的比例误差
|
|
//float q0 = 1, q1 = 0, q2 = 0, q3 = 0; //定义四元素
|
|
//float his_q0 = 1, his_q1 = 0, his_q2 = 0, his_q3 = 0;
|
|
//float q0_yaw = 1, q1_yaw = 0, q2_yaw = 0, q3_yaw = 0; //弥补Mahony算法在无地磁情况解算Yaw轴满足不了大扰动要求的现象
|
|
//float his_q0_yaw = 1, his_q1_yaw = 0, his_q2_yaw = 0, his_q3_yaw = 0;
|
|
|
|
//void GetAngle(const stMpu *pMpu,void *pAngE, float dt)
|
|
//{
|
|
// static const float KpDef = 0.85f ;
|
|
// static const float KiDef = 0.0035f;
|
|
// const float Gyro_Gr = 0.0005326f; //面计算度每秒,转换弧度每秒则 0.03051756 * 0.0174533f = 0.0005326
|
|
// float HalfTime = 0.5 * dt;
|
|
// float gx = pMpu->gyroX * Gyro_Gr;
|
|
// float gy = pMpu->gyroY * Gyro_Gr;
|
|
// float gz = pMpu->gyroZ * Gyro_Gr;
|
|
// float ax = pMpu->accX;
|
|
// float ay = pMpu->accY;
|
|
// float az = pMpu->accZ;
|
|
//
|
|
// float vx, vy, vz;
|
|
// float ex, ey, ez;
|
|
//
|
|
// static float his_q0q0;
|
|
// static float his_q0q1;
|
|
// static float his_q0q2;
|
|
// static float his_q1q1;
|
|
// static float his_q1q3;
|
|
// static float his_q2q2;
|
|
// static float his_q2q3;
|
|
// static float his_q3q3;
|
|
//
|
|
// float q0q0;
|
|
// float q0q1;
|
|
// float q0q2;
|
|
// float q1q1;
|
|
// float q1q3;
|
|
// float q2q2;
|
|
// float q2q3;
|
|
// float q3q3;
|
|
//
|
|
// float q0_yawq0_yaw;
|
|
// float q1_yawq1_yaw;
|
|
// float q2_yawq2_yaw;
|
|
// float q3_yawq3_yaw;
|
|
// float q1_yawq2_yaw;
|
|
// float q0_yawq3_yaw;
|
|
//
|
|
////**************************Yaw轴计算******************************
|
|
//
|
|
// //Yaw轴四元素的微分方程
|
|
// q0_yaw = his_q0_yaw + (-his_q1_yaw * gx - his_q2_yaw * gy - his_q3_yaw * gz) * HalfTime;
|
|
// q1_yaw = his_q1_yaw + ( his_q0_yaw * gx + his_q2_yaw * gz - his_q3_yaw * gy) * HalfTime;
|
|
// q2_yaw = his_q2_yaw + ( his_q0_yaw * gy - his_q1_yaw * gz + his_q3_yaw * gx) * HalfTime;
|
|
// q3_yaw = his_q3_yaw + ( his_q0_yaw * gz + his_q1_yaw * gy - his_q2_yaw * gx) * HalfTime;
|
|
//
|
|
// q0_yawq0_yaw = q0_yaw * q0_yaw;
|
|
// q1_yawq1_yaw = q1_yaw * q1_yaw;
|
|
// q2_yawq2_yaw = q2_yaw * q2_yaw;
|
|
// q3_yawq3_yaw = q3_yaw * q3_yaw;
|
|
// q1_yawq2_yaw = q1_yaw * q2_yaw;
|
|
// q0_yawq3_yaw = q0_yaw * q3_yaw;
|
|
//
|
|
// //规范化Yaw轴四元数
|
|
// norm = Q_rsqrt(q0_yawq0_yaw + q1_yawq1_yaw + q2_yawq2_yaw + q3_yawq3_yaw);
|
|
// q0_yaw = q0_yaw * norm;
|
|
// q1_yaw = q1_yaw * norm;
|
|
// q2_yaw = q2_yaw * norm;
|
|
// q3_yaw = q3_yaw * norm;
|
|
//
|
|
// //if(ax * ay * az == 0)
|
|
// //return ;
|
|
//
|
|
// //归一化加速度计值
|
|
// norm = Q_rsqrt(ax * ax + ay * ay + az * az);
|
|
// ax = ax * norm;
|
|
// ay = ay * norm;
|
|
// az = az * norm;
|
|
//
|
|
// //由姿态阵估计重力方向和流量/变迁
|
|
// vx = 2 * (his_q1q3 - his_q0q2);
|
|
// vy = 2 * (his_q0q1 + his_q2q3);
|
|
// vz = his_q0q0 - his_q1q1 - his_q2q2 + his_q3q3;
|
|
//
|
|
// //向量外积再相减得到差分就是误差 两个单位向量的差积即为误差向量
|
|
// ex = (ay * vz - az * vy) ;
|
|
// ey = (az * vx - ax * vz) ;
|
|
// ez = (ax * vy - ay * vx) ;
|
|
|
|
// //对误差进行PI计算
|
|
// ex_int = ex_int + ex * KiDef;
|
|
// ey_int = ey_int + ey * KiDef;
|
|
// ez_int = ez_int + ez * KiDef;
|
|
|
|
// //校正陀螺仪
|
|
// gx = gx + KpDef * ex + ex_int;
|
|
// gy = gy + KpDef * ey + ey_int;
|
|
// gz = gz + KpDef * ez + ez_int;
|
|
//
|
|
// //四元素的微分方程
|
|
// q0 = his_q0 + (-his_q1 * gx - his_q2 * gy - his_q3 * gz) * HalfTime;
|
|
// q1 = his_q1 + ( his_q0 * gx + his_q2 * gz - his_q3 * gy) * HalfTime;
|
|
// q2 = his_q2 + ( his_q0 * gy - his_q1 * gz + his_q3 * gx) * HalfTime;
|
|
// q3 = his_q3 + ( his_q0 * gz + his_q1 * gy - his_q2 * gx) * HalfTime;
|
|
|
|
// q0q0 = q0 * q0;
|
|
// q0q1 = q0 * q1;
|
|
// q0q2 = q0 * q2;
|
|
// q1q1 = q1 * q1;
|
|
// q1q3 = q1 * q3;
|
|
// q2q2 = q2 * q2;
|
|
// q2q3 = q2 * q3;
|
|
// q3q3 = q3 * q3;
|
|
|
|
// //规范化Pitch、Roll轴四元数
|
|
// norm = Q_rsqrt(q0q0 + q1q1 + q2q2 + q3q3);
|
|
// q0 = q0 * norm;
|
|
// q1 = q1 * norm;
|
|
// q2 = q2 * norm;
|
|
// q3 = q3 * norm;
|
|
//
|
|
// //求解欧拉角
|
|
// *((float *)pAngE+2) = atan2f(2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1) * RtA; //ROLL
|
|
// *((float *)pAngE+1) = asin(2 * q0q2 -2 * q1q3) * 57.3; //PITCH
|
|
// *(float *)pAngE = atan2f(2 * q1_yawq2_yaw + 2 * q0_yawq3_yaw, -2 * q2_yawq2_yaw - 2 * q3_yawq3_yaw + 1) * RtA; //YAW
|
|
//
|
|
//
|
|
// //存储更替相应的四元数
|
|
// his_q0_yaw = q0_yaw;
|
|
// his_q1_yaw = q1_yaw;
|
|
// his_q2_yaw = q2_yaw;
|
|
// his_q3_yaw = q3_yaw;
|
|
|
|
// his_q0 = q0;
|
|
// his_q1 = q1;
|
|
// his_q2 = q2;
|
|
// his_q3 = q3;
|
|
//
|
|
// his_q0q0 = q0q0;
|
|
// his_q0q1 = q0q1;
|
|
// his_q0q2 = q0q2;
|
|
// his_q1q1 = q1q1;
|
|
// his_q1q3 = q1q3;
|
|
// his_q2q2 = q2q2;
|
|
// his_q2q3 = q2q3;
|
|
// his_q3q3 = q3q3;
|
|
//}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
typedef volatile struct {
|
|
float q0;
|
|
float q1;
|
|
float q2;
|
|
float q3;
|
|
} Quaternion;
|
|
|
|
|
|
void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE, float dt)
|
|
{
|
|
volatile struct V{
|
|
float x;
|
|
float y;
|
|
float z;
|
|
} Gravity,Acc,Gyro,AccGravity;
|
|
|
|
static struct V GyroIntegError = {0};
|
|
static float KpDef = 0.8f ;
|
|
static float KiDef = 0.0003f;
|
|
static Quaternion NumQ = {1, 0, 0, 0};
|
|
float q0_t,q1_t,q2_t,q3_t;
|
|
//float NormAcc;
|
|
float NormQuat;
|
|
float HalfTime = dt * 0.5f;
|
|
|
|
|
|
|
|
// 提取等效旋转矩阵中的重力分量
|
|
Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
|
|
Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
|
|
Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);
|
|
// 加速度归一化
|
|
NormAcc = Q_rsqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));
|
|
|
|
Acc.x = pMpu->accX * NormAcc;
|
|
Acc.y = pMpu->accY * NormAcc;
|
|
Acc.z = pMpu->accZ * NormAcc;
|
|
//向量差乘得出的值
|
|
AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
|
|
AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
|
|
AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);
|
|
//再做加速度积分补偿角速度的补偿值
|
|
GyroIntegError.x += AccGravity.x * KiDef;
|
|
GyroIntegError.y += AccGravity.y * KiDef;
|
|
GyroIntegError.z += AccGravity.z * KiDef;
|
|
//角速度融合加速度积分补偿值
|
|
Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x + GyroIntegError.x;//弧度制
|
|
Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y + GyroIntegError.y;
|
|
Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z + GyroIntegError.z;
|
|
// 一阶龙格库塔法, 更新四元数
|
|
|
|
q0_t = (-NumQ.q1*Gyro.x - NumQ.q2*Gyro.y - NumQ.q3*Gyro.z) * HalfTime;
|
|
q1_t = ( NumQ.q0*Gyro.x - NumQ.q3*Gyro.y + NumQ.q2*Gyro.z) * HalfTime;
|
|
q2_t = ( NumQ.q3*Gyro.x + NumQ.q0*Gyro.y - NumQ.q1*Gyro.z) * HalfTime;
|
|
q3_t = (-NumQ.q2*Gyro.x + NumQ.q1*Gyro.y + NumQ.q0*Gyro.z) * HalfTime;
|
|
|
|
NumQ.q0 += q0_t;
|
|
NumQ.q1 += q1_t;
|
|
NumQ.q2 += q2_t;
|
|
NumQ.q3 += q3_t;
|
|
// 四元数归一化
|
|
NormQuat = Q_rsqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));
|
|
NumQ.q0 *= NormQuat;
|
|
NumQ.q1 *= NormQuat;
|
|
NumQ.q2 *= NormQuat;
|
|
NumQ.q3 *= NormQuat;
|
|
|
|
|
|
// 四元数转欧拉角
|
|
{
|
|
|
|
#ifdef YAW_GYRO
|
|
*(float *)pAngE = atan2f(2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3, 1 - 2 * NumQ.q2 *NumQ.q2 - 2 * NumQ.q3 * NumQ.q3) * RtA; //yaw
|
|
#else
|
|
float yaw_G = pMpu->gyroZ * Gyro_G;
|
|
if((yaw_G > 3.0f) || (yaw_G < -3.0f)) //数据太小可以认为是干扰，不是偏航动作
|
|
{
|
|
pAngE->yaw += yaw_G * dt;
|
|
}
|
|
#endif
|
|
pAngE->pitch = asin(2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3) * RtA;
|
|
|
|
pAngE->roll = atan2(2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1, 1 - 2 * NumQ.q1 *NumQ.q1 - 2 * NumQ.q2 * NumQ.q2) * RtA; //PITCH
|
|
}
|
|
}
|
|
|
|
|
|
/***************************************************END OF FILE***************************************************/
|
|
|
|
|
|
|
|
|