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;
}
static void getEstimatedAttitude(void)
{
int32_t axis;
int32_t accMag = 0;
static t_fp_vector EstM;
static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } };
static float accLPF[3];
static uint32_t previousT;
uint32_t currentT = micros();
uint32_t deltaT;
float scale, deltaGyroAngle[3];
deltaT = currentT - previousT;
scale = deltaT * gyro.scale;
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroADC[axis] * scale;
if (cfg.acc_lpf_factor > 0) {
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor);
accSmooth[axis] = accLPF[axis];
} else {
accSmooth[axis] = accADC[axis];
}
accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
}
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
rotateV(&EstG.V, deltaGyroAngle);
// Apply complimentary 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 (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);
// Attitude of the estimated vector
anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI));
angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI));
if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, deltaGyroAngle);
for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
heading = calculateHeading(&EstM);
} else {
rotateV(&EstN.V, deltaGyroAngle);
normalizeV(&EstN.V, &EstN.V);
heading = calculateHeading(&EstN);
}
acc_calc(deltaT); // rotate acc vector into earth frame
if (cfg.throttle_correction_value) {
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
throttleAngleCorrection = 0;
} else {
int angle = lrintf(acosf(cosZ) * throttleAngleScale);
if (angle > 900)
angle = 900;
throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ;
}
}
}
static void getEstimatedAttitude(void)
{
int32_t axis;
int32_t accMag = 0;
static t_fp_vector EstM;
static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } };
static float accLPF[3];
static uint32_t previousT;
uint32_t currentT = micros();
uint32_t deltaT;
float scale;
fp_angles_t deltaGyroAngle;
deltaT = currentT - previousT;
scale = deltaT * gyroScaleRad;
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
deltaGyroAngle.raw[axis] = gyroADC[axis] * scale;
if (imuRuntimeConfig->acc_lpf_factor > 0) {
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / imuRuntimeConfig->acc_lpf_factor)) + accADC[axis] * (1.0f / imuRuntimeConfig->acc_lpf_factor);
accSmooth[axis] = accLPF[axis];
} else {
accSmooth[axis] = accADC[axis];
}
accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
}
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
rotateV(&EstG.V, &deltaGyroAngle);
// Apply complimentary 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
float invGyroComplimentaryFilterFactor = (1.0f / (imuRuntimeConfig->gyro_cmpf_factor + 1.0f));
if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * imuRuntimeConfig->gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor;
}
f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);
// Attitude of the estimated vector
anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, &deltaGyroAngle);
// FIXME what does the _M_ mean?
float invGyroComplimentaryFilter_M_Factor = (1.0f / (imuRuntimeConfig->gyro_cmpfm_factor + 1.0f));
for (axis = 0; axis < 3; axis++) {
EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
}
heading = calculateHeading(&EstM);
} else {
rotateV(&EstN.V, &deltaGyroAngle);
normalizeV(&EstN.V, &EstN.V);
heading = calculateHeading(&EstN);
}
acc_calc(deltaT); // rotate acc vector into earth frame
}