Rev 2032 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2032 | Rev 2033 | ||
|---|---|---|---|
| Line 154... | Line 154... | ||
| 154 | * it is hardly worth the trouble. |
154 | * it is hardly worth the trouble. |
| 155 | ************************************************************************/ |
155 | ************************************************************************/ |
| Line 156... | Line 156... | ||
| 156 | 156 | ||
| 157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| 158 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
158 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
| 159 | return GYRO_ACC_FACTOR * filteredAcc[axis];// + correctionTerm; |
159 | return (int32_t)GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];// + correctionTerm; |
| 160 | // return 342L * filteredAcc[axis]; |
160 | // return 342L * filteredAcc[axis]; |
| Line 161... | Line 161... | ||
| 161 | } |
161 | } |
| 162 | 162 | ||
| Line 330... | Line 330... | ||
| 330 | /* |
330 | /* |
| 331 | * Add to each sum: The amount by which the angle is changed just below. |
331 | * Add to each sum: The amount by which the angle is changed just below. |
| 332 | */ |
332 | */ |
| 333 | for (axis = PITCH; axis <= ROLL; axis++) { |
333 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 334 | accDerived = getAngleEstimateFromAcc(axis); |
334 | accDerived = getAngleEstimateFromAcc(axis); |
| 335 | //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
335 | debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
| 336 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
336 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
| 337 | temp = angle[axis]; |
337 | temp = angle[axis]; |
| 338 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
338 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| 339 | + (int32_t) permilleAcc * accDerived) / 1000L; |
339 | + (int32_t) permilleAcc * accDerived) / 1000L; |
| 340 | correctionSum[axis] += angle[axis] - temp; |
340 | correctionSum[axis] += angle[axis] - temp; |
| 341 | } |
341 | } |
| 342 | } else { |
342 | } else { |
| 343 | //debugOut.analog[9] = 0; |
343 | debugOut.analog[9] = 0; |
| 344 | //debugOut.analog[10] = 0; |
344 | debugOut.analog[10] = 0; |
| 345 | // experiment: Kill drift compensation updates when not flying smooth. |
345 | // experiment: Kill drift compensation updates when not flying smooth. |
| 346 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
346 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
| 347 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
347 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
| 348 | } |
348 | } |
| 349 | - | ||
| 350 | int32_t accDerived = getAngleEstimateFromAcc(0); |
- | |
| 351 | debugOut.analog[9 + 0] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
- | |
| 352 | - | ||
| 353 | accDerived = getAngleEstimateFromAcc(1); |
- | |
| 354 | debugOut.analog[9 + 1] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
- | |
| 355 | } |
349 | } |
| Line 356... | Line 350... | ||
| 356 | 350 | ||
| 357 | /************************************************************************ |
351 | /************************************************************************ |
| 358 | * This is an attempt to correct not the error in the angle integrals |
352 | * This is an attempt to correct not the error in the angle integrals |