/* * This file is part of Cleanflight. * * Cleanflight is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * Cleanflight is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ // Inertial Measurement Unit (IMU) #include #include #include #include "platform.h" #include "build/build_config.h" #include "build/debug.h" #include "common/axis.h" #include "pg/pg.h" #include "pg/pg_ids.h" #include "drivers/time.h" #include "fc/runtime_config.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" #include "io/gps.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/compass.h" #include "sensors/gyro.h" #include "sensors/sensors.h" #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) #include #include static pthread_mutex_t imuUpdateLock; #if defined(SIMULATOR_IMU_SYNC) static uint32_t imuDeltaT = 0; static bool imuUpdated = false; #endif #define IMU_LOCK pthread_mutex_unlock(&imuUpdateLock) #define IMU_UNLOCK pthread_mutex_unlock(&imuUpdateLock) #else #define IMU_LOCK #define IMU_UNLOCK #endif // the limit (in degrees/second) beyond which we stop integrating // omega_I. At larger spin rates the DCM PI controller can get 'dizzy' // which results in false gyro drift. See // http://gentlenav.googlecode.com/files/fastRotations.pdf #define SPIN_RATE_LIMIT 20 int32_t accSum[XYZ_AXIS_COUNT]; uint32_t accTimeSum = 0; // keep track for integration of acc int accSumCount = 0; float accVelScale; static float throttleAngleScale; static float fc_acc; static float smallAngleCosZ = 0; static imuRuntimeConfig_t imuRuntimeConfig; STATIC_UNIT_TESTED float rMat[3][3]; // quaternion of sensor frame relative to earth frame STATIC_UNIT_TESTED quaternion q = QUATERNION_INITIALIZE; STATIC_UNIT_TESTED quaternionProducts qP = QUATERNION_PRODUCTS_INITIALIZE; // headfree quaternions quaternion headfree = QUATERNION_INITIALIZE; quaternion offset = QUATERNION_INITIALIZE; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 attitudeEulerAngles_t attitude = EULER_INITIALIZE; PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); PG_RESET_TEMPLATE(imuConfig_t, imuConfig, .dcm_kp = 2500, // 1.0 * 10000 .dcm_ki = 0, // 0.003 * 10000 .small_angle = 25, .accDeadband = {.xy = 40, .z= 40}, .acc_unarmedcal = 1 ); STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){ imuQuaternionComputeProducts(&q, &qP); rMat[0][0] = 1.0f - 2.0f * qP.yy - 2.0f * qP.zz; rMat[0][1] = 2.0f * (qP.xy + -qP.wz); rMat[0][2] = 2.0f * (qP.xz - -qP.wy); rMat[1][0] = 2.0f * (qP.xy - -qP.wz); rMat[1][1] = 1.0f - 2.0f * qP.xx - 2.0f * qP.zz; rMat[1][2] = 2.0f * (qP.yz + -qP.wx); rMat[2][0] = 2.0f * (qP.xz + -qP.wy); rMat[2][1] = 2.0f * (qP.yz - -qP.wx); rMat[2][2] = 1.0f - 2.0f * qP.xx - 2.0f * qP.yy; #if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) && !defined(SET_IMU_FROM_EULER) rMat[1][0] = -2.0f * (qP.xy - -qP.wz); rMat[2][0] = -2.0f * (qP.xz + -qP.wy); #endif } /* * Calculate RC time constant used in the accZ lpf. */ static float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) { return 0.5f / (M_PIf * accz_lpf_cutoff); } static float calculateThrottleAngleScale(uint16_t throttle_correction_angle) { return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); } void imuConfigure(uint16_t throttle_correction_angle) { imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f; imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f; imuRuntimeConfig.acc_unarmedcal = imuConfig()->acc_unarmedcal; imuRuntimeConfig.small_angle = imuConfig()->small_angle; fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); } void imuInit(void) { smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle)); accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f; imuComputeRotationMatrix(); #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) if (pthread_mutex_init(&imuUpdateLock, NULL) != 0) { printf("Create imuUpdateLock error!\n"); } #endif } void imuResetAccelerationSum(void) { accSum[0] = 0; accSum[1] = 0; accSum[2] = 0; accSumCount = 0; accTimeSum = 0; } #if defined(USE_ALT_HOLD) static void imuTransformVectorBodyToEarth(t_fp_vector * v) { /* From body frame to earth frame */ const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z; const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z; const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z; v->V.X = x; v->V.Y = -y; v->V.Z = z; } // rotate acc into Earth frame and calculate acceleration in it static void imuCalculateAcceleration(uint32_t deltaT) { static int32_t accZoffset = 0; static float accz_smooth = 0; // deltaT is measured in us ticks const float dT = (float)deltaT * 1e-6f; t_fp_vector accel_ned; accel_ned.V.X = acc.accADC[X]; accel_ned.V.Y = acc.accADC[Y]; accel_ned.V.Z = acc.accADC[Z]; imuTransformVectorBodyToEarth(&accel_ned); if (imuRuntimeConfig.acc_unarmedcal == 1) { if (!ARMING_FLAG(ARMED)) { accZoffset -= accZoffset / 64; accZoffset += accel_ned.V.Z; } accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis } else { accel_ned.V.Z -= acc.dev.acc_1G; } accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter // apply Deadband to reduce integration drift and vibration influence accSum[X] += applyDeadband(lrintf(accel_ned.V.X), imuRuntimeConfig.accDeadband.xy); accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), imuRuntimeConfig.accDeadband.xy); accSum[Z] += applyDeadband(lrintf(accz_smooth), imuRuntimeConfig.accDeadband.z); // sum up Values for later integration to get velocity and distance accTimeSum += deltaT; accSumCount++; } #endif // USE_ALT_HOLD static float invSqrt(float x) { return 1.0f / sqrtf(x); } static bool imuUseFastGains(void) { return !ARMING_FLAG(ARMED) && millis() < 20000; } static float imuGetPGainScaleFactor(void) { if (imuUseFastGains()) { return 10.0f; } else { return 1.0f; } } 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 // Calculate general spin rate (rad/s) const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz)); // Use raw heading error (from GPS or whatever else) float ez = 0; 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 float ex = 0, ey = 0; float 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) const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz; const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz; const float bx = sqrtf(hx * hx + hy * hy); // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF) const 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)) { const 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 const 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); quaternion buffer; buffer.w = q.w; buffer.x = q.x; buffer.y = q.y; buffer.z = q.z; q.w += (-buffer.x * gx - buffer.y * gy - buffer.z * gz); q.x += (+buffer.w * gx + buffer.y * gz - buffer.z * gy); q.y += (+buffer.w * gy - buffer.x * gz + buffer.z * gx); q.z += (+buffer.w * gz + buffer.x * gy - buffer.y * gx); // Normalise quaternion recipNorm = invSqrt(sq(q.w) + sq(q.x) + sq(q.y) + sq(q.z)); q.w *= recipNorm; q.x *= recipNorm; q.y *= recipNorm; q.z *= recipNorm; // Pre-compute rotation matrix from quaternion imuComputeRotationMatrix(); } STATIC_UNIT_TESTED void imuUpdateEulerAngles(void){ quaternionProducts buffer; if (FLIGHT_MODE(HEADFREE_MODE)) { imuQuaternionComputeProducts(&headfree, &buffer); attitude.values.roll = lrintf(atan2_approx((+2.0f * (buffer.wx + buffer.yz)), (+1.0f - 2.0f * (buffer.xx + buffer.yy))) * (1800.0f / M_PIf)); attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(+2.0f * (buffer.wy - buffer.xz))) * (1800.0f / M_PIf)); attitude.values.yaw = lrintf((-atan2_approx((+2.0f * (buffer.wz + buffer.xy)), (+1.0f - 2.0f * (buffer.yy + buffer.zz))) * (1800.0f / M_PIf))); } else { attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf)); attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat[2][0])) * (1800.0f / M_PIf)); attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf))); } if (attitude.values.yaw < 0) attitude.values.yaw += 3600; /* Update small angle state */ if (rMat[2][2] > smallAngleCosZ) { ENABLE_STATE(SMALL_ANGLE); } else { DISABLE_STATE(SMALL_ANGLE); } } static bool imuIsAccelerometerHealthy(void) { float accMagnitude = 0; for (int axis = 0; axis < 3; axis++) { const float a = acc.accADC[axis]; accMagnitude += a * a; } accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.dev.acc_1G)); // Accept accel readings only in range 0.90g - 1.10g return (81 < accMagnitude) && (accMagnitude < 121); } static bool isMagnetometerHealthy(void) { return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0); } static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) { static uint32_t previousIMUUpdateTime; float rawYawError = 0; bool useAcc = false; bool useMag = false; bool useYaw = false; uint32_t deltaT = currentTimeUs - previousIMUUpdateTime; previousIMUUpdateTime = currentTimeUs; if (imuIsAccelerometerHealthy()) { useAcc = true; } if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) { useMag = true; } #if defined(USE_GPS) else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) { // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - gpsSol.groundCourse); useYaw = true; } #endif #if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) UNUSED(imuMahonyAHRSupdate); UNUSED(useAcc); UNUSED(useMag); UNUSED(useYaw); UNUSED(rawYawError); #else #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC) // printf("[imu]deltaT = %u, imuDeltaT = %u, currentTimeUs = %u, micros64_real = %lu\n", deltaT, imuDeltaT, currentTimeUs, micros64_real()); deltaT = imuDeltaT; #endif float gyroAverage[XYZ_AXIS_COUNT]; gyroGetAccumulationAverage(gyroAverage); float accAverage[XYZ_AXIS_COUNT]; if (!accGetAccumulationAverage(accAverage)) { useAcc = false; } imuMahonyAHRSupdate(deltaT * 1e-6f, DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]), useAcc, accAverage[X], accAverage[Y], accAverage[Z], useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z], useYaw, rawYawError); imuUpdateEulerAngles(); #endif #if defined(USE_ALT_HOLD) imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame #endif } void imuUpdateAttitude(timeUs_t currentTimeUs) { if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) { IMU_LOCK; #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC) if (imuUpdated == false) { IMU_UNLOCK; return; } imuUpdated = false; #endif imuCalculateEstimatedAttitude(currentTimeUs); IMU_UNLOCK; } else { acc.accADC[X] = 0; acc.accADC[Y] = 0; acc.accADC[Z] = 0; } } float getCosTiltAngle(void) { return rMat[2][2]; } int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) { /* * Use 0 as the throttle angle correction if we are inverted, vertical or with a * small angle < 0.86 deg * TODO: Define this small angle in config. */ if (rMat[2][2] <= 0.015f) { return 0; } int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale); if (angle > 900) angle = 900; return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f))); } #ifdef SIMULATOR_BUILD void imuSetAttitudeRPY(float roll, float pitch, float yaw) { IMU_LOCK; attitude.values.roll = roll * 10; attitude.values.pitch = pitch * 10; attitude.values.yaw = yaw * 10; IMU_UNLOCK; } void imuSetAttitudeQuat(float w, float x, float y, float z) { IMU_LOCK; q.w = w; q.x = x; q.y = y; q.z = z; imuComputeRotationMatrix(); imuUpdateEulerAngles(); IMU_UNLOCK; } #endif #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC) void imuSetHasNewData(uint32_t dt) { IMU_LOCK; imuUpdated = true; imuDeltaT = dt; IMU_UNLOCK; } #endif void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd) { quatProd->ww = quat->w * quat->w; quatProd->wx = quat->w * quat->x; quatProd->wy = quat->w * quat->y; quatProd->wz = quat->w * quat->z; quatProd->xx = quat->x * quat->x; quatProd->xy = quat->x * quat->y; quatProd->xz = quat->x * quat->z; quatProd->yy = quat->y * quat->y; quatProd->yz = quat->y * quat->z; quatProd->zz = quat->z * quat->z; } bool imuQuaternionHeadfreeOffsetSet(void) { if ((ABS(attitude.values.roll) < 450) && (ABS(attitude.values.pitch) < 450)) { const float yaw = -atan2_approx((+2.0f * (qP.wz + qP.xy)), (+1.0f - 2.0f * (qP.yy + qP.zz))); offset.w = cos_approx(yaw/2); offset.x = 0; offset.y = 0; offset.z = sin_approx(yaw/2); return(true); } else { return(false); } } void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *result) { const float A = (q1->w + q1->x) * (q2->w + q2->x); const float B = (q1->z - q1->y) * (q2->y - q2->z); const float C = (q1->w - q1->x) * (q2->y + q2->z); const float D = (q1->y + q1->z) * (q2->w - q2->x); const float E = (q1->x + q1->z) * (q2->x + q2->y); const float F = (q1->x - q1->z) * (q2->x - q2->y); const float G = (q1->w + q1->y) * (q2->w - q2->z); const float H = (q1->w - q1->y) * (q2->w + q2->z); result->w = B + (- E - F + G + H) / 2.0f; result->x = A - (+ E + F + G + H) / 2.0f; result->y = C + (+ E - F + G - H) / 2.0f; result->z = D + (+ E - F - G + H) / 2.0f; } void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v) { quaternionProducts buffer; imuQuaternionMultiplication(&offset, &q, &headfree); imuQuaternionComputeProducts(&headfree, &buffer); const float x = (buffer.ww + buffer.xx - buffer.yy - buffer.zz) * v->X + 2.0f * (buffer.xy + buffer.wz) * v->Y + 2.0f * (buffer.xz - buffer.wy) * v->Z; const float y = 2.0f * (buffer.xy - buffer.wz) * v->X + (buffer.ww - buffer.xx + buffer.yy - buffer.zz) * v->Y + 2.0f * (buffer.yz + buffer.wx) * v->Z; const float z = 2.0f * (buffer.xz + buffer.wy) * v->X + 2.0f * (buffer.yz - buffer.wx) * v->Y + (buffer.ww - buffer.xx - buffer.yy + buffer.zz) * v->Z; v->X = x; v->Y = y; v->Z = z; }