Rev 1988 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1988 | Rev 1989 | ||
|---|---|---|---|
| Line 153... | Line 153... | ||
| 153 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
153 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
| 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 | int16_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256; |
158 | int16_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 512; |
| 159 | return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis] + correctionTerm; |
159 | return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis] + correctionTerm; |
| Line 160... | Line 160... | ||
| 160 | } |
160 | } |
| 161 | 161 | ||
| Line 326... | Line 326... | ||
| 326 | if (highControlActivity) { // reduce effect during stick control activity |
326 | if (highControlActivity) { // reduce effect during stick control activity |
| 327 | permilleAcc /= 4; |
327 | permilleAcc /= 4; |
| 328 | if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
328 | if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
| 329 | permilleAcc /= 4; |
329 | permilleAcc /= 4; |
| 330 | } |
330 | } |
| 331 | } else { |
- | |
| 332 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
- | |
| 333 | } |
331 | } |
| Line 334... | Line 332... | ||
| 334 | 332 | ||
| 335 | /* |
333 | /* |
| 336 | * Add to each sum: The amount by which the angle is changed just below. |
334 | * Add to each sum: The amount by which the angle is changed just below. |
| 337 | */ |
335 | */ |
| Line 348... | Line 346... | ||
| 348 | } else { |
346 | } else { |
| 349 | debugOut.analog[9] = 0; |
347 | debugOut.analog[9] = 0; |
| 350 | debugOut.analog[10] = 0; |
348 | debugOut.analog[10] = 0; |
| 351 | // experiment: Kill drift compensation updates when not flying smooth. |
349 | // experiment: Kill drift compensation updates when not flying smooth. |
| 352 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
350 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
| - | 351 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
|
| 353 | } |
352 | } |
| 354 | } |
353 | } |
| Line 355... | Line 354... | ||
| 355 | 354 | ||
| 356 | /************************************************************************ |
355 | /************************************************************************ |