Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1954 → Rev 1955

/branches/dongfang_FC_rewrite/attitude.c
203,6 → 203,8
void getAnalogData(void) {
uint8_t axis;
 
analog_update();
 
for (axis = PITCH; axis <= ROLL; axis++) {
rate_PID[axis] = gyro_PID[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis];
rate_ATT[axis] = gyro_ATT[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis];
216,7 → 218,7
// We are done reading variables from the analog module.
// Interrupt-driven sensor reading may restart.
analogDataReady = 0;
startAnalogConversionCycle(0);
startAnalogConversionCycle();
}
 
/*
295,8 → 297,8
// Well actually the Z axis acc. check is not so silly.
uint8_t axis;
int32_t temp;
DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER;
DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
 
if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
<= dynamicParams.UserParams[7]) {
310,8 → 312,6
debugFullWeight = 0;
}
 
/*
 
if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
permilleAcc /= 2;
debugFullWeight = 0;
319,10 → 319,10
 
if (controlActivity > 10000) { // reduce effect during stick commands
permilleAcc /= 4;
DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
debugOut.digital[0] |= DEBUG_ACC0THORDER;
if (controlActivity > 20000) { // reduce effect during stick commands
permilleAcc /= 4;
DebugOut.Digital[1] |= DEBUG_ACC0THORDER;
debugOut.digital[1] |= DEBUG_ACC0THORDER;
}
}
 
331,7 → 331,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.
temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
340,11 → 340,9
correctionSum[axis] += angle[axis] - temp;
}
} else {
DebugOut.Analog[9] = 0;
DebugOut.Analog[10] = 0;
debugOut.analog[9] = 0;
debugOut.analog[10] = 0;
 
DebugOut.Analog[16] = 0;
DebugOut.Analog[17] = 0;
// experiment: Kill drift compensation updates when not flying smooth.
correctionSum[PITCH] = correctionSum[ROLL] = 0;
}
381,8 → 379,8
driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
// DebugOut.Analog[11 + axis] = correctionSum[axis];
DebugOut.Analog[16 + axis] = correctionSum[axis];
DebugOut.Analog[28 + axis] = driftComp[axis];
// DebugOut.Analog[16 + axis] = correctionSum[axis];
debugOut.analog[28 + axis] = driftComp[axis];
 
correctionSum[axis] = 0;
}
396,10 → 394,6
getAnalogData();
integrate();
 
DebugOut.Analog[3] = rate_PID[PITCH];
DebugOut.Analog[4] = rate_PID[ROLL];
DebugOut.Analog[5] = yawRate;
 
#ifdef ATTITUDE_USE_ACC_SENSORS
correctIntegralsByAcc0thOrder();
driftCorrection();