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;
}
}
if (isOnFinalGyroCalibrationCycle()) {
beeper(BEEPER_GYRO_CALIBRATED);
}
calibratingG--;
}
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;
}
