239,9 → 239,8 |
ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
* sinroll) / (int32_t) MATH_UNIT_FACTOR; |
ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll |
/ ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) |
/ ANTIOVF * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF |
* MATH_UNIT_FACTOR)); |
/ ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) / ANTIOVF |
* tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
+ ((int32_t) yawRate * cosroll) / cospitch; |
} |
327,8 → 326,7 |
*/ |
for (axis = PITCH; axis <= ROLL; axis++) { |
accDerived = getAngleEstimateFromAcc(axis); |
DebugOut.Analog[9 + axis] = (10 * accDerived) |
/ GYRO_DEG_FACTOR_PITCHROLL; |
DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
|
// 1000 * the correction amount that will be added to the gyro angle in next line. |
correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
370,16 → 368,14 |
timer = DRIFTCORRECTION_TIME; |
for (axis = PITCH; axis <= ROLL; axis++) { |
// Take the sum of corrections applied, add it to delta |
deltaCorrection = (correctionSum[axis] |
* HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2) |
/ DRIFTCORRECTION_TIME; |
deltaCorrection = (correctionSum[axis] * HIRES_GYRO_INTEGRATION_FACTOR |
+ DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
// Add the delta to the compensation. So positive delta means, gyro should have higher value. |
driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
// DebugOut.Analog[11 + axis] = correctionSum[axis]; |
|
DebugOut.Analog[18 + axis] = deltaCorrection |
/ staticParams.GyroAccTrim; |
DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim; |
DebugOut.Analog[28 + axis] = driftComp[axis]; |
|
correctionSum[axis] = 0; |
432,16 → 428,16 |
v = abs(angle[ROLL] / 512); |
if (v > w) |
w = v; |
correction = w/8 + 1; |
correction = w / 8 + 1; |
// calculate the deviation of the yaw gyro heading and the compass heading |
if (compassHeading < 0) |
error = 0; // disable yaw drift compensation if compass heading is undefined |
else |
if (abs(yawRate) > 128) { // spinning fast |
else if (abs(yawRate) > 128) { // spinning fast |
error = 0; |
} else { |
// compassHeading - yawGyroHeading, on a -180..179 deg interval. |
error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) |
% 360) - 180; |
} |
if (!ignoreCompassTimer && w < 25) { |
yawGyroDrift += error; |
456,10 → 452,11 |
yawGyroHeading += (error * 8) / correction; |
|
/* |
w = (w * dynamicParams.CompassYawEffect) / 32; |
w = dynamicParams.CompassYawEffect - w; |
*/ |
w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) / 32; |
w = (w * dynamicParams.CompassYawEffect) / 32; |
w = dynamicParams.CompassYawEffect - w; |
*/ |
w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) |
/ 32; |
|
// As readable formula: |
// w = dynamicParams.CompassYawEffect * (1-w/32); |
468,7 → 465,9 |
if (!ignoreCompassTimer) { |
v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
// yawGyroHeading - compassCourse on a -180..179 degree interval. |
r = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) % 360) - 180; |
r |
= ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
% 360) - 180; |
v = (r * w) / v; // align to compass course |
// limit yaw rate |
w = 3 * dynamicParams.CompassYawEffect; |