crazepony算法.doc

/* _ _ +-+ / _ / _ | R | / / / /_/ / +-+ / / _ _ _ / _/_ _ _ _ / / / _/ _ /_ / / _ / / / _ / _ / / / / / /_/ / / /_/ / / /_/ _/ / / /_/ / / / / /_/ / _/_/ _,_/ /_/_/_/ _ /_/ /_/_ / / / _/ / /_/Filename:IMUSO3.cAuthor:祥 、nieyong说明：这是Crazepony软件姿态解算融合文件，Crazepony已经不再使用DMP硬件解算Part of this algrithom is referred from pixhawk.-*/#include stm32f10x.h#include stm32f10x_it.h#include #include IMU.h#include IMUSO3.h/! Auxiliary variables to reduce number of repeated operationsstatic float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;/* quaternion of sensor frame relative to auxiliary frame */static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f;/* quaternion of sensor frame relative to auxiliary frame */static float gyro_bias3 = 0.0f, 0.0f, 0.0f; /* bias estimation */static float q0q0, q0q1, q0q2, q0q3;static float q1q1, q1q2, q1q3;static float q2q2, q2q3;static float q3q3;static uint8_t bFilterInit = 0;/函数名：invSqrt(void)/描述：求平方根的倒数/该函数是经典的Carmack求平方根算法，效率极高，使用魔数0x5f375a86static float invSqrt(float number) volatile long i; volatile float x, y; volatile const float f = 1.5F; x = number * 0.5F; y = number; i = * ( long * ) &y); i = 0x5f375a86 - ( i 1 ); y = * ( float * ) &i); y = y * ( f - ( x * y * y ) ); return y;/! Using accelerometer, sense the gravity vector./! Using magnetometer, sense yaw.static void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz) float initialRoll, initialPitch; float cosRoll, sinRoll, cosPitch, sinPitch; float magX, magY; float initialHdg, cosHeading, sinHeading; initialRoll = atan2(-ay, -az); initialPitch = atan2(ax, -az); cosRoll = cosf(initialRoll); sinRoll = sinf(initialRoll); cosPitch = cosf(initialPitch); sinPitch = sinf(initialPitch); magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; magY = my * cosRoll - mz * sinRoll; initialHdg = atan2f(-magY, magX); cosRoll = cosf(initialRoll * 0.5f); sinRoll = sinf(initialRoll * 0.5f); cosPitch = cosf(initialPitch * 0.5f); sinPitch = sinf(initialPitch * 0.5f); cosHeading = cosf(initialHdg * 0.5f); sinHeading = sinf(initialHdg * 0.5f); q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; / auxillary variables to reduce number of repeated operations, for 1st pass q0q0 = q0 * q0; q0q1 = q0 * q1; q0q2 = q0 * q2; q0q3 = q0 * q3; q1q1 = q1 * q1; q1q2 = q1 * q2; q1q3 = q1 * q3; q2q2 = q2 * q2; q2q3 = q2 * q3; q3q3 = q3 * q3;static void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) float recipNorm;float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;/ Make filter converge to initial solution faster/ This function assumes you are in static position./ WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.if(bFilterInit = 0) NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);bFilterInit = 1; /! If magnetometer measurement is available, use it.if(!(mx = 0.0f) & (my = 0.0f) & (mz = 0.0f) float hx, hy, hz, bx, bz;float halfwx, halfwy, halfwz;/ Normalise magnetometer measurement/ Will sqrt work better? PX4 system is powerful enough? recipNorm = invSqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; / Reference direction of Earths 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);hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2); bx = sqrt(hx * hx + hy * hy); bz = hz; / Estimated direction of magnetic field 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 += (my * halfwz - mz * halfwy); halfey += (mz * halfwx - mx * halfwz); halfez += (mx * halfwy - my * halfwx);/增加一个条件： 加速度的模量与G相差不远时。 0.75*G normAcc 0.0f) gyro_bias0 += twoKi * halfex * dt;/ integral error scaled by Kigyro_bias1 += twoKi * halfey * dt;gyro_bias2 += twoKi * halfez * dt;/ apply integral feedbackgx += gyro_bias0;gy += gyro_bias1;gz += gyro_bias2;else gyro_bias0 = 0.0f;/ prevent integral windupgyro_bias1 = 0.0f;gyro_bias2 = 0.0f;/ Apply proportional feedbackgx += twoKp * halfex;gy += twoKp * halfey;gz += twoKp * halfez;/ Time derivative of quaternion. q_dot = 0.5*qotimes omega./! q_k = q_k-1 + dt*dotq/! dotq = 0.5*q otimes P(omega)dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx); q0 += dt*dq0;q1 += dt*dq1;q2 += dt*dq2;q3 += dt*dq3;/ Normalise quaternionrecipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);q0 *= recipNorm;q1 *= recipNorm;q2 *= recipNorm;q3 *= recipNorm;/ Auxiliary variables to avoid repeated arithmeticq0q0 = q0 * q0;q0q1 = q0 * q1;q0q2 = q0 * q2;q0q3 = q0 * q3;q1q1 = q1 * q1;q1q2 = q1 * q2;q1q3 = q1 * q3;q2q2 = q2 * q2;q2q3 = q2 * q3;q3q3 = q3 * q3; #define so3_comp_params_Kp 1.0f#define so3_comp_params_Ki 0.05f /函数名：IMUSO3Thread(void)/描述：姿态软件解算融合函数/该函数对姿态的融合是软件解算，Crazepony现在不使用DMP硬件解算/对应的硬件解算函数为IMU_Process()void IMUSO3Thread(void)/! Time constantfloat dt = 0.01f;/sstatic uint32_t tPrev=0,startTime=0;/usuint32_t now;uint8_t i;/* output euler angles */float euler3 = 0.0f, 0.0f, 0.0f;/rad/* Initialization */float Rot_matrix9 = 1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f ;/*0)?(now-tPrev)/1000000.0f:0;tPrev=now;ReadIMUSensorHandle();if(!imu.ready) if(startTime=0)startTime=now;gyro_offsets_sum0 += imu.gyroRaw0;gyro_offsets_sum1 += imu.gyroRaw1;gyro_offsets_sum2 += imu.gyroRaw2;offset_count+;if(now startTime + GYRO_CALC_TIME)imu.gyroOffset0 = gyro_offsets_sum0/offset_count;imu.gyroOffset1 = gyro_offsets_sum1/offset_count;imu.gyroOffset2 = gyro_offsets_sum2/offset_count;offset_count=0;gyro_offsets_sum0=0;gyro_offsets_sum1=0;gyro_offsets_sum2=0;imu.ready = 1;startTime=0;return;gyro0 = imu.gyro0 - imu.gyroOffset0;gyro1 = imu.gyro1 - imu.gyroOffset1;gyro2 = imu.gyro2 - imu.gyroOffset2;acc0 = -imu.accb0;acc1 = -imu.accb1;acc2 = -imu.accb2;/ NOTE : Accelerometer is reversed./ Because proper mount of PX4 will give you a reversed accelerometer readings.NonlinearSO3AHRSupdate(gyro0, gyro1, gyro2,-acc0, -acc1, -acc2,mag0, mag1, mag2,so3_comp_params_Kp,so3_comp_params_Ki, dt);/ Convert q-R, This R converts inertial frame to body frame.Rot_matrix0 = q0q0 + q1q1 - q2q2 - q3q3;/ 11Rot_matrix1 = 2.f * (q1*q2 + q0*q3);/ 12Rot_matrix2 = 2.f * (q1*q3 - q0*q2);/ 13Rot_matrix3 = 2.f * (q1*q2 - q0*q3);/ 21Rot_matrix4 =