Rev 1986 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1986 | Rev 1988 | ||
---|---|---|---|
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 | return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
159 | return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis] + correctionTerm; |
Line 159... | Line 160... | ||
159 | } |
160 | } |
160 | 161 | ||
161 | void setStaticAttitudeAngles(void) { |
162 | void setStaticAttitudeAngles(void) { |
Line 293... | Line 294... | ||
293 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
294 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
294 | // are less than ....., or reintroduce Kalman. |
295 | // are less than ....., or reintroduce Kalman. |
295 | // Well actually the Z axis acc. check is not so silly. |
296 | // Well actually the Z axis acc. check is not so silly. |
296 | uint8_t axis; |
297 | uint8_t axis; |
297 | int32_t temp; |
298 | int32_t temp; |
- | 299 | ||
- | 300 | uint8_t ca = controlActivity >> 8; |
|
- | 301 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
|
- | 302 | ||
- | 303 | if (highControlActivity) { |
|
298 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
304 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
- | 305 | } else { |
|
299 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
306 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
- | 307 | } |
|
Line 300... | Line 308... | ||
300 | 308 | ||
301 | if (accVector <= dynamicParams.maxAccVector) { |
309 | if (accVector <= dynamicParams.maxAccVector) { |
Line 302... | Line 310... | ||
302 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
310 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
Line 312... | Line 320... | ||
312 | 320 | ||
313 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
321 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
314 | permilleAcc /= 2; |
322 | permilleAcc /= 2; |
315 | debugFullWeight = 0; |
323 | debugFullWeight = 0; |
316 | */ |
- | |
Line 317... | Line 324... | ||
317 | uint8_t ca = controlActivity >> 8; |
324 | */ |
318 | 325 | ||
319 | if (ca > staticParams.maxControlActivity) { // reduce effect during stick control activity |
- | |
320 | permilleAcc /= 4; |
326 | if (highControlActivity) { // reduce effect during stick control activity |
321 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
327 | permilleAcc /= 4; |
322 | if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
- | |
323 | permilleAcc /= 4; |
328 | if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
- | 329 | permilleAcc /= 4; |
|
- | 330 | } |
|
324 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
331 | } else { |
Line 325... | Line 332... | ||
325 | } |
332 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
326 | } |
333 | } |
327 | 334 |