//============================================================================================= // MahonyAHRS.c //============================================================================================= // // Madgwick's implementation of Mayhony's AHRS algorithm. // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ // // From the x-io website "Open-source resources available on this website are // provided under the GNU General Public Licence unless an alternative licence // is provided in source." // // Date Author Notes // 29/09/2011 SOH Madgwick Initial release // 02/10/2011 SOH Madgwick Optimised for reduced CPU load // // Algorithm paper: // http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934 // //============================================================================================= //------------------------------------------------------------------------------------------- // Header files #include "hal_mahonyAHRS.h" #include #include "system.h" #include "ble_comm.h" #include "hal_ble_client.h" //------------------------------------------------------------------------------------------- // Definitions #define twoKpDef (200.0f * 0.5f) // 2 * proportional gain #define twoKiDef (0.0f * 1.0f) // 2 * integral gain //float Mahony_GetRoll(void) {return P->roll;} //float Mahony_GetPitch(void) {return P->pitch;} //float Mahony_GetYaw(void) {return P->yaw;} //void Mahony_GetAccN(float* accn){accn[0] = P->accN[1]; accn[1] = P->accN[2]; accn[2] = P->accN[3];} void Mahony_send_ANO(uint8_t fun,uint8_t* p,int len) { uint8_t buf[256]; int L=0; uint8_t ver = 0; buf[L] = 0xAA; ver += buf[L++]; buf[L] = 0x05; ver += buf[L++]; buf[L] = 0xAF; ver += buf[L++]; buf[L] = fun; ver += buf[L++]; buf[L] = len; ver += buf[L++]; for(int i=0;i>1); y = *(float*)&i; y = y * (1.5f - (halfx * y * y)); y = y * (1.5f - (halfx * y * y)); return y; } void Mahony_update(MahonyAHRS_t *P,float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float recipNorm; float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; float hx, hy, bx, bz; float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz; float halfex, halfey, halfez; float qa, qb, qc; //因为下面算法会改变原始值，所以先保存一份 P->accN[0] = 0.0f; P->accN[1] = ax; P->accN[2] = ay; P->accN[3] = az; // Compute feedback only if accelerometer measurement valid // (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement recipNorm = Mahony_invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Normalise magnetometer measurement recipNorm = Mahony_invSqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; // Auxiliary variables to avoid repeated arithmetic q0q0 = P->q0 * P->q0; q0q1 = P->q0 * P->q1; q0q2 = P->q0 * P->q2; q0q3 = P->q0 * P->q3; q1q1 = P->q1 * P->q1; q1q2 = P->q1 * P->q2; q1q3 = P->q1 * P->q3; q2q2 = P->q2 * P->q2; q2q3 = P->q2 * P->q3; q3q3 = P->q3 * P->q3; // Reference direction of Earth's magnetic field hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); bx = sqrtf(hx * hx + hy * hy); bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); // Estimated direction of gravity and magnetic field halfvx = q1q3 - q0q2; halfvy = q0q1 + q2q3; halfvz = q0q0 - 0.5f + q3q3; halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); // Error is sum of cross product between estimated direction // and measured direction of field vectors halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy); halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz); halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx); // Compute and apply integral feedback if enabled if(P->twoKi > 0.0f) { // integral error scaled by Ki P->integralFBx += P->twoKi * halfex * P->invSampleFreq; P->integralFBy += P->twoKi * halfey * P->invSampleFreq; P->integralFBz += P->twoKi * halfez * P->invSampleFreq; gx += P->integralFBx; // apply integral feedback gy += P->integralFBy; gz += P->integralFBz; } else { P->integralFBx = 0.0f; // prevent integral windup P->integralFBy = 0.0f; P->integralFBz = 0.0f; } // Apply proportional feedback gx += P->twoKp * halfex; gy += P->twoKp * halfey; gz += P->twoKp * halfez; // SEGGER_RTT_printf(0,"P->twoKp %d\n",(int)(P->twoKp*100)); } // Integrate rate of change of quaternion gx *= (0.5f * P->invSampleFreq); // pre-multiply common factors gy *= (0.5f * P->invSampleFreq); gz *= (0.5f * P->invSampleFreq); qa = P->q0; qb = P->q1; qc = P->q2; P->q0 += (-qb * gx - qc * gy - P->q3 * gz); P->q1 += (qa * gx + qc * gz - P->q3 * gy); P->q2 += (qa * gy - qb * gz + P->q3 * gx); P->q3 += (qa * gz + qb * gy - qc * gx); // Normalise quaternion recipNorm = Mahony_invSqrt(P->q0 * P->q0 + P->q1 * P->q1 + P->q2 * P->q2 + P->q3 * P->q3); P->q0 *= recipNorm; P->q1 *= recipNorm; P->q2 *= recipNorm; P->q3 *= recipNorm; //计算三个角度 P->roll = atan2f(P->q0*P->q1 + P->q2*P->q3, 0.5f - P->q1*P->q1 - P->q2*P->q2) * 57.29578f;; P->pitch = asinf(-2.0f * (P->q1*P->q3 - P->q0*P->q2)) * 57.29578f;; P->yaw = atan2f(P->q1*P->q2 + P->q0*P->q3, 0.5f - P->q2*P->q2 - P->q3*P->q3) * 57.29578f;; //坐标系转换 P->q[0] = P->q0; P->q[1] = P->q1; P->q[2] = P->q2; P->q[3] = P->q3; float tmp_vector2[4]; quaternProd(tmp_vector2, P->q, P->accN); quaternConj(P->q); quaternProd(P->accN, tmp_vector2, P->q); P->accN[3] = P->accN[3] - 1.0f;//去重力 } void Mahony_SetKp(MahonyAHRS_t *P,float twoKp1) { P->twoKp = twoKp1; // 2 * integral gain (Ki } void Mahony_Init(MahonyAHRS_t *P,float sampleFrequency) { P->twoKi = twoKiDef; // 2 * integral gain (Ki) P->twoKp = twoKpDef; P->q0 = 1.0f; P->q1 = 0.0f; P->q2 = 0.0f; P->q3 = 0.0f; P->integralFBx = 0.0f; P->integralFBy = 0.0f; P->integralFBz = 0.0f; P->invSampleFreq = 1.0f / sampleFrequency; } //============================================================================================ // END OF CODE //============================================================================================