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 | /************************************************************************ |