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
}