/****************************************************************************************************** * 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 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***************************************************/