124,7 → 124,7 |
#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
#define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
|
int32_t correctionSum[2] = {0,0}; |
int16_t correctionSum[2] = {0,0}; |
|
/* |
* Experiment: Compensating for dynamic-induced gyro biasing. |
165,6 → 165,7 |
dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0; |
|
dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0; |
correctionSum[PITCH] = correctionSum[ROLL] = 0; |
|
// Calibrate hardware. |
analog_calibrate(); |
178,6 → 179,7 |
|
// update compass course to current heading |
compassCourse = compassHeading; |
|
// Inititialize YawGyroIntegral value with current compass heading |
yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
|
199,6 → 201,7 |
rate[axis] = (gyro_ATT[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
differential[axis] = gyroD[axis]; |
} |
|
yawRate = yawGyro + dynamicOffsetYaw; |
|
// We are done reading variables from the analog module. |
237,10 → 240,6 |
ACYawRate = yawRate; |
} |
|
DebugOut.Analog[3] = ACRate[PITCH]; |
DebugOut.Analog[4] = ACRate[ROLL]; |
DebugOut.Analog[5] = ACYawRate; |
|
/* |
* Yaw |
* Calculate yaw gyro integral (~ to rotation angle) |
273,7 → 272,7 |
/************************************************************************ |
* A kind of 0'th order integral correction, that corrects the integrals |
* directly. This is the "gyroAccFactor" stuff in the original code. |
* There is (there) also what I would call a "minus 1st order correction" |
* There is (there) also a drift compensation |
* - it corrects the differential of the integral = the gyro offsets. |
* That should only be necessary with drifty gyros like ENC-03. |
************************************************************************/ |
282,13 → 281,13 |
// are less than ....., or reintroduce Kalman. |
// Well actually the Z axis acc. check is not so silly. |
uint8_t axis; |
if(!looping && //((ZAcc >= -4) || (MKFlags & MKFLAG_MOTOR_RUN))) { // if not looping in any direction |
ZAcc >= -dynamicParams.UserParams[7] && ZAcc <= dynamicParams.UserParams[7]) { |
int32_t correction; |
if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) { |
DebugOut.Digital[0] = 1; |
|
uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
uint8_t debugFullWeight = 1; |
int32_t accDerived[2]; |
int32_t accDerived; |
|
if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands |
permilleAcc /= 2; |
304,11 → 303,23 |
* Add to each sum: The amount by which the angle is changed just below. |
*/ |
for (axis=PITCH; axis<=ROLL; axis++) { |
accDerived[axis] = getAngleEstimateFromAcc(axis); |
correctionSum[axis] += permilleAcc * (accDerived[axis] - angle[axis]); |
accDerived = getAngleEstimateFromAcc(axis); |
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; |
angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L; |
|
correctionSum[axis] += angle[axis] - correction; |
|
// There should not be a risk of overflow here, since the integrals do not exceed a few 100000. |
angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived[axis]) / 1000L; |
// change = ((1000 - permilleAcc) * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis] |
// = (1000 * angle[axis] - permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis] |
// = (- permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000 |
// = permilleAcc * (accDerived - angle[axis]) / 1000 |
|
// Experiment: Do not acutally apply the correction. See if drift compensation does that. |
// angle[axis] += correction / 1000; |
} |
|
DebugOut.Digital[1] = debugFullWeight; |
322,25 → 333,28 |
* (that happens in correctIntegralsByAcc0thOrder above) but rather the |
* cause of it: Gyro drift, vibration and rounding errors. |
* All the corrections made in correctIntegralsByAcc0thOrder over |
* MINUSFIRSTORDERCORRECTION_TIME cycles are summed up. This number is |
* then divided by MINUSFIRSTORDERCORRECTION_TIME to get the approx. |
* DRIFTCORRECTION_TIME cycles are summed up. This number is |
* then divided by DRIFTCORRECTION_TIME to get the approx. |
* correction that should have been applied to each iteration to fix |
* the error. This is then added to the dynamic offsets. |
************************************************************************/ |
// 2 times / sec. |
#define DRIFTCORRECTION_TIME 488/2 |
void driftCompensation(void) { |
// 2 times / sec. = 488/2 |
#define DRIFTCORRECTION_TIME 256L |
void driftCorrection(void) { |
static int16_t timer = DRIFTCORRECTION_TIME; |
int16_t deltaCompensation; |
int16_t deltaCorrection; |
uint8_t axis; |
if (! --timer) { |
timer = DRIFTCORRECTION_TIME; |
for (axis=PITCH; axis<=ROLL; axis++) { |
deltaCompensation = ((correctionSum[axis] + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME); |
CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp); |
dynamicOffset[axis] += deltaCompensation / staticParams.GyroAccTrim; |
// Take the sum of corrections applied, add it to delta |
deltaCorrection = ((correctionSum[axis] + DRIFTCORRECTION_TIME / 2) * HIRES_GYRO_INTEGRATION_FACTOR) / DRIFTCORRECTION_TIME; |
// Add the delta to the compensation. So positive delta means, gyro should have higher value. |
dynamicOffset[axis] += deltaCorrection / staticParams.GyroAccTrim; |
CHECK_MIN_MAX(dynamicOffset[axis], -staticParams.DriftComp, staticParams.DriftComp); |
DebugOut.Analog[11 + axis] = correctionSum[axis]; |
DebugOut.Analog[28 + axis] = dynamicOffset[axis]; |
correctionSum[axis] = 0; |
DebugOut.Analog[28 + axis] = dynamicOffset; |
} |
} |
} |
351,9 → 365,18 |
void calculateFlightAttitude(void) { |
getAnalogData(); |
integrate(); |
|
DebugOut.Analog[6] = ACRate[PITCH]; |
DebugOut.Analog[7] = ACRate[ROLL]; |
DebugOut.Analog[8] = ACYawRate; |
|
DebugOut.Analog[3] = rate_PID[PITCH]; |
DebugOut.Analog[4] = rate_PID[ROLL]; |
DebugOut.Analog[5] = yawRate; |
|
#ifdef ATTITUDE_USE_ACC_SENSORS |
correctIntegralsByAcc0thOrder(); |
driftCompensation(); |
driftCorrection(); |
#endif |
} |
|