static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, bool useAcc, float ax, float ay, float az, bool useMag, float mx, float my, float mz, bool useYaw, float yawError) { static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki float recipNorm; float hx, hy, bx; float ex = 0, ey = 0, ez = 0; float qa, qb, qc; // Calculate general spin rate (rad/s) float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz)); // Use raw heading error (from GPS or whatever else) if (useYaw) { while (yawError > M_PIf) yawError -= (2.0f * M_PIf); while (yawError < -M_PIf) yawError += (2.0f * M_PIf); ez += sin_approx(yawError / 2.0f); } // Use measured magnetic field vector recipNorm = sq(mx) + sq(my) + sq(mz); if (useMag && recipNorm > 0.01f) { // Normalise magnetometer measurement recipNorm = invSqrt(recipNorm); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF). // This way magnetic field will only affect heading and wont mess roll/pitch angles // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero) // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero) hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz; hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz; bx = sqrtf(hx * hx + hy * hy); // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF) float ez_ef = -(hy * bx); // Rotate mag error vector back to BF and accumulate ex += rMat[2][0] * ez_ef; ey += rMat[2][1] * ez_ef; ez += rMat[2][2] * ez_ef; } // Use measured acceleration vector recipNorm = sq(ax) + sq(ay) + sq(az); if (useAcc && recipNorm > 0.01f) { // Normalise accelerometer measurement recipNorm = invSqrt(recipNorm); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Error is sum of cross product between estimated direction and measured direction of gravity 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]); } // Compute and apply integral feedback if enabled if(imuRuntimeConfig->dcm_ki > 0.0f) { // Stop integrating if spinning beyond the certain limit if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) { float dcmKiGain = imuRuntimeConfig->dcm_ki; integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki integralFBy += dcmKiGain * ey * dt; integralFBz += dcmKiGain * ez * dt; } } else { integralFBx = 0.0f; // prevent integral windup integralFBy = 0.0f; integralFBz = 0.0f; } // Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor(); // Apply proportional and integral feedback gx += dcmKpGain * ex + integralFBx; gy += dcmKpGain * ey + integralFBy; gz += dcmKpGain * ez + integralFBz; // Integrate rate of change of quaternion gx *= (0.5f * dt); gy *= (0.5f * dt); gz *= (0.5f * dt); qa = q0; qb = q1; 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); // Normalise quaternion recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3)); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; // Pre-compute rotation matrix from quaternion imuComputeRotationMatrix(); }
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, int accWeight, float ax, float ay, float az, bool useMag, float mx, float my, float mz, bool useCOG, float courseOverGround) { static float integralAccX = 0.0f, integralAccY = 0.0f, integralAccZ = 0.0f; // integral error terms scaled by Ki static float integralMagX = 0.0f, integralMagY = 0.0f, integralMagZ = 0.0f; // integral error terms scaled by Ki float recipNorm; float ex, ey, ez; float qa, qb, qc; /* Calculate general spin rate (rad/s) */ float spin_rate_sq = sq(gx) + sq(gy) + sq(gz); /* Step 1: Yaw correction */ // Use measured magnetic field vector if (useMag || useCOG) { float kpMag = imuRuntimeConfig->dcm_kp_mag * imuGetPGainScaleFactor(); recipNorm = mx * mx + my * my + mz * mz; if (useMag && recipNorm > 0.01f) { // Normalise magnetometer measurement recipNorm = invSqrt(recipNorm); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF). // This way magnetic field will only affect heading and wont mess roll/pitch angles // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero) // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero) float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz; float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz; float bx = sqrtf(hx * hx + hy * hy); // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF) float ez_ef = -(hy * bx); // Rotate mag error vector back to BF and accumulate ex = rMat[2][0] * ez_ef; ey = rMat[2][1] * ez_ef; ez = rMat[2][2] * ez_ef; } else if (useCOG) { // Use raw heading error (from GPS or whatever else) while (courseOverGround > M_PIf) courseOverGround -= (2.0f * M_PIf); while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf); // William Premerlani and Paul Bizard, Direction Cosine Matrix IMU - Eqn. 22-23 // (Rxx; Ryx) - measured (estimated) heading vector (EF) // (cos(COG), sin(COG)) - reference heading vector (EF) // error is cross product between reference heading and estimated heading (calculated in EF) float ez_ef = - sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]; 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; } // Compute and apply integral feedback if enabled if(imuRuntimeConfig->dcm_ki_mag > 0.0f) { // Stop integrating if spinning beyond the certain limit if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) { integralMagX += imuRuntimeConfig->dcm_ki_mag * ex * dt; // integral error scaled by Ki integralMagY += imuRuntimeConfig->dcm_ki_mag * ey * dt; integralMagZ += imuRuntimeConfig->dcm_ki_mag * ez * dt; gx += integralMagX; gy += integralMagY; gz += integralMagZ; } } // Calculate kP gain and apply proportional feedback gx += kpMag * ex; gy += kpMag * ey; gz += kpMag * ez; } /* Step 2: Roll and pitch correction - use measured acceleration vector */ if (accWeight > 0) { float kpAcc = imuRuntimeConfig->dcm_kp_acc * imuGetPGainScaleFactor(); // Just scale by 1G length - That's our vector adjustment. Rather than // using one-over-exact length (which needs a costly square root), we already // know the vector is enough "roughly unit length" and since it is only weighted // in by a certain amount anyway later, having that exact is meaningless. (c) MasterZap recipNorm = 1.0f / GRAVITY_CMSS; ax *= recipNorm; ay *= recipNorm; az *= recipNorm; float fAccWeightScaler = accWeight / (float)MAX_ACC_SQ_NEARNESS; // Error is sum of cross product between estimated direction and measured direction of gravity ex = (ay * rMat[2][2] - az * rMat[2][1]) * fAccWeightScaler; ey = (az * rMat[2][0] - ax * rMat[2][2]) * fAccWeightScaler; ez = (ax * rMat[2][1] - ay * rMat[2][0]) * fAccWeightScaler; // Compute and apply integral feedback if enabled if(imuRuntimeConfig->dcm_ki_acc > 0.0f) { // Stop integrating if spinning beyond the certain limit if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) { integralAccX += imuRuntimeConfig->dcm_ki_acc * ex * dt; // integral error scaled by Ki integralAccY += imuRuntimeConfig->dcm_ki_acc * ey * dt; integralAccZ += imuRuntimeConfig->dcm_ki_acc * ez * dt; gx += integralAccX; gy += integralAccY; gz += integralAccZ; } } // Calculate kP gain and apply proportional feedback gx += kpAcc * ex; gy += kpAcc * ey; gz += kpAcc * ez; } // Integrate rate of change of quaternion gx *= (0.5f * dt); gy *= (0.5f * dt); gz *= (0.5f * dt); qa = q0; qb = q1; 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); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; // Pre-compute rotation matrix from quaternion imuComputeRotationMatrix(); }