Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1645 → Rev 1646

/branches/dongfang_FC_rewrite/attitude.c
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
}