static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold) { int8_t axis; static int32_t g[3]; static stdev_t var[3]; for (axis = 0; axis < 3; axis++) { // Reset g[axis] at start of calibration if (isOnFirstGyroCalibrationCycle()) { g[axis] = 0; devClear(&var[axis]); } // Sum up CALIBRATING_GYRO_CYCLES readings g[axis] += gyroADC[axis]; devPush(&var[axis], gyroADC[axis]); // Reset global variables to prevent other code from using un-calibrated data gyroADC[axis] = 0; gyroZero[axis] = 0; if (isOnFinalGyroCalibrationCycle()) { float dev = devStandardDeviation(&var[axis]); // check deviation and startover in case the model was moved if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) { gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); return; } gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES; blinkLedAndSoundBeeper(10, 15, 1); } } calibratingG--; }
STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // Reset g[axis] at start of calibration if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) { gyroSensor->calibration.sum[axis] = 0.0f; devClear(&gyroSensor->calibration.var[axis]); // gyroZero is set to zero until calibration complete gyroSensor->gyroDev.gyroZero[axis] = 0.0f; } // Sum up CALIBRATING_GYRO_TIME_US readings gyroSensor->calibration.sum[axis] += gyroSensor->gyroDev.gyroADCRaw[axis]; devPush(&gyroSensor->calibration.var[axis], gyroSensor->gyroDev.gyroADCRaw[axis]); if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) { const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]); // DEBUG_GYRO_CALIBRATION records the standard deviation of roll // into the spare field - debug[3], in DEBUG_GYRO_RAW if (axis == X) { DEBUG_SET(DEBUG_GYRO_RAW, DEBUG_GYRO_CALIBRATION, lrintf(stddev)); } // check deviation and startover in case the model was moved if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) { gyroSetCalibrationCycles(gyroSensor); return; } // please take care with exotic boardalignment !! gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles(); if (axis == Z) { gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100); } } } if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) { schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) { beeper(BEEPER_GYRO_CALIBRATED); } } --gyroSensor->calibration.cyclesRemaining; }
static void GYRO_Common(void) { int axis; static int32_t g[3]; static stdev_t var[3]; if (calibratingG > 0) { for (axis = 0; axis < 3; axis++) { // Reset g[axis] at start of calibration if (calibratingG == CALIBRATING_GYRO_CYCLES) { g[axis] = 0; devClear(&var[axis]); } // Sum up 1000 readings g[axis] += gyroADC[axis]; devPush(&var[axis], gyroADC[axis]); // Clear global variables for next reading gyroADC[axis] = 0; gyroZero[axis] = 0; if (calibratingG == 1) { float dev = devStandardDeviation(&var[axis]); // check deviation and startover if idiot was moving the model if (mcfg.moron_threshold && dev > mcfg.moron_threshold) { calibratingG = CALIBRATING_GYRO_CYCLES; devClear(&var[0]); devClear(&var[1]); devClear(&var[2]); g[0] = g[1] = g[2] = 0; continue; } //gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES; gyroZero[axis] = (g[axis]+1) / CALIBRATING_GYRO_CYCLES; blinkLED(10, 15, 1); } } calibratingG--; } for (axis = 0; axis < 3; axis++) gyroADC[axis] -= gyroZero[axis]; }
static void Gyro_Calibrate(void) // Total Samples = Gyromaxcount * Gyroavgcount + Gyrodiscardcnt { float Temp[3]; uint16_t i, axis; uint8_t breakout = 0; stdev_t var[3]; Gyro_500Hz_AVG(Temp, Gyrodiscardcnt); // Discard some values here to let gyro settle do // Shaky Hands Loop NOTE: Removed Sphere stuff here because results are equal! { breakout++; // Increase Breakout Counter for (axis = 0; axis < 3; axis++) // Clear for Run { devClear(&var[axis]); gyroZero[axis] = 0.0f; } for (i = 0; i < Gyromaxcount; i++) // Outer loop for StdDev { Gyro_500Hz_AVG(Temp, Gyroavgcount); // Average some for (axis = 0; axis < 3; axis++) // Add StdDev, save values for next loopturn { devPush(&var[axis], Temp[axis]); gyroZero[axis] += Temp[axis]; } } Temp[0] = 0.0f; for (axis = 0; axis < 3; axis++) { gyroZero[axis] /= (float)Gyromaxcount; // Calculate gyrozero no matter what. Not timecritical anyway Temp[0] += devStandardDeviation(&var[axis]); } } while (Temp[0] > cfg.gy_stdev && breakout < Timeoutrun); // Breakout prevents endlessloop after time GyroCalCompromised = (breakout == Timeoutrun); // We timed out so gyro is problematic if(GyroCalCompromised && cfg.ShakyDataAvail) // Problem? Use backupdata, if available (normally they are..) { for (axis = 0; axis < 3; axis++) gyroZero[axis] = cfg.ShakyGyroZero[axis]; blinkLED(15, 20, 10); // Warnblink Flight may be degraded but possible. } }
void IMU::update(uint32_t currentTime, bool armed, uint16_t & calibratingA, uint16_t & calibratingG) { static float accelLPF[3]; static int32_t accelZoffset; static float accz_smooth; static int16_t accelZero[3]; static int32_t a[3]; static int16_t accelSmooth[3]; static float EstG[3]; static float EstN[3] = { 1.0f, 0.0f, 0.0f }; static int16_t gyroZero[3]; static uint32_t previousTime; int32_t accMag = 0; float dT = 0; float rpy[3]; float accel_ned[3]; float deltaGyroAngle[3]; uint32_t deltaT = currentTime - previousTime; float scale = deltaT * this->gyroScale; int16_t accelADC[3]; float anglerad[3]; previousTime = currentTime; this->_board->imuRead(accelADC, this->gyroADC); if (calibratingA > 0) { for (uint8_t axis = 0; axis < 3; axis++) { // Reset a[axis] at start of calibration if (calibratingA == this->calibratingAccCycles) a[axis] = 0; // Sum up this->calibratingAccCycles readings a[axis] += accelADC[axis]; // Clear global variables for next reading accelADC[axis] = 0; accelZero[axis] = 0; } // Calculate average, shift Z down by acc1G if (calibratingA == 1) { accelZero[ROLL] = (a[ROLL] + (this->calibratingAccCycles / 2)) / this->calibratingAccCycles; accelZero[PITCH] = (a[PITCH] + (this->calibratingAccCycles / 2)) / this->calibratingAccCycles; accelZero[YAW] = (a[YAW] + (this->calibratingAccCycles / 2)) / this->calibratingAccCycles - this->acc1G; } calibratingA--; } accelADC[ROLL] -= accelZero[ROLL]; accelADC[PITCH] -= accelZero[PITCH]; accelADC[YAW] -= accelZero[YAW]; // range: +/- 8192; +/- 2000 deg/sec static int32_t g[3]; static stdev_t var[3]; if (calibratingG > 0) { for (uint8_t axis = 0; axis < 3; axis++) { // Reset g[axis] at start of calibration if (calibratingG == this->calibratingGyroCycles) { g[axis] = 0; devClear(&var[axis]); } // Sum up 1000 readings g[axis] += this->gyroADC[axis]; devPush(&var[axis], this->gyroADC[axis]); // Clear global variables for next reading this->gyroADC[axis] = 0; gyroZero[axis] = 0; if (calibratingG == 1) { float dev = devStandardDeviation(&var[axis]); // check deviation and startover if idiot was moving the model if (CONFIG_MORON_THRESHOLD && dev > CONFIG_MORON_THRESHOLD) { calibratingG = this->calibratingGyroCycles; devClear(&var[0]); devClear(&var[1]); devClear(&var[2]); g[0] = g[1] = g[2] = 0; continue; } gyroZero[axis] = (g[axis] + (this->calibratingGyroCycles / 2)) / this->calibratingGyroCycles; } } calibratingG--; } for (uint8_t axis = 0; axis < 3; axis++) this->gyroADC[axis] -= gyroZero[axis]; // Initialization for (uint8_t axis = 0; axis < 3; axis++) { deltaGyroAngle[axis] = this->gyroADC[axis] * scale; if (CONFIG_ACC_LPF_FACTOR > 0) { accelLPF[axis] = accelLPF[axis] * (1.0f - (1.0f / CONFIG_ACC_LPF_FACTOR)) + accelADC[axis] * (1.0f / CONFIG_ACC_LPF_FACTOR); accelSmooth[axis] = (int16_t)accelLPF[axis]; } else { accelSmooth[axis] = accelADC[axis]; } accMag += (int32_t)accelSmooth[axis] * accelSmooth[axis]; } accMag = accMag * 100 / ((int32_t)this->acc1G * this->acc1G); rotateV(EstG, deltaGyroAngle); // Apply complementary filter (Gyro drift correction) // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit // range => we neutralize the effect of accelerometers in the angle // estimation. To do that, we just skip filter, as EstV already rotated by Gyro if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) { for (uint8_t axis = 0; axis < 3; axis++) EstG[axis] = (EstG[axis] * (float)CONFIG_GYRO_CMPF_FACTOR + accelSmooth[axis]) * INV_GYR_CMPF_FACTOR; } // Attitude of the estimated vector anglerad[ROLL] = atan2f(EstG[Y], EstG[Z]); anglerad[PITCH] = atan2f(-EstG[X], sqrtf(EstG[Y] * EstG[Y] + EstG[Z] * EstG[Z])); rotateV(EstN, deltaGyroAngle); normalizeV(EstN, EstN); // Calculate heading float cosineRoll = cosf(anglerad[ROLL]); float sineRoll = sinf(anglerad[ROLL]); float cosinePitch = cosf(anglerad[PITCH]); float sinePitch = sinf(anglerad[PITCH]); float Xh = EstN[X] * cosinePitch + EstN[Y] * sineRoll * sinePitch + EstN[Z] * sinePitch * cosineRoll; float Yh = EstN[Y] * cosineRoll - EstN[Z] * sineRoll; anglerad[YAW] = atan2f(Yh, Xh); // deltaT is measured in us ticks dT = (float)deltaT * 1e-6f; // the accel values have to be rotated into the earth frame rpy[0] = -(float)anglerad[ROLL]; rpy[1] = -(float)anglerad[PITCH]; rpy[2] = -(float)anglerad[YAW]; accel_ned[X] = accelSmooth[0]; accel_ned[Y] = accelSmooth[1]; accel_ned[Z] = accelSmooth[2]; rotateV(accel_ned, rpy); if (!armed) { accelZoffset -= accelZoffset / 64; accelZoffset += (int32_t)accel_ned[Z]; } accel_ned[Z] -= accelZoffset / 64; // compensate for gravitation on z-axis accz_smooth = accz_smooth + (dT / (fcAcc + dT)) * (accel_ned[Z] - accz_smooth); // low pass filter // apply Deadband to reduce integration drift and vibration influence and // sum up Values for later integration to get velocity and distance this->accelSum[X] += deadbandFilter(lrintf(accel_ned[X]), CONFIG_ACCXY_DEADBAND); this->accelSum[Y] += deadbandFilter(lrintf(accel_ned[Y]), CONFIG_ACCXY_DEADBAND); this->accelSum[Z] += deadbandFilter(lrintf(accz_smooth), CONFIG_ACCZ_DEADBAND); this->accelTimeSum += deltaT; this->accelSumCount++; // Convert angles from radians to tenths of a degrees this->angle[ROLL] = (int16_t)lrintf(anglerad[ROLL] * (1800.0f / M_PI)); this->angle[PITCH] = (int16_t)lrintf(anglerad[PITCH] * (1800.0f / M_PI)); this->angle[YAW] = (int16_t)(lrintf(anglerad[YAW] * 1800.0f / M_PI + CONFIG_MAGNETIC_DECLINATION) / 10.0f); // Convert heading from [-180,+180] to [0,360] if (this->angle[YAW] < 0) this->angle[YAW] += 360; }