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(); |