Rev 1874 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1874 | Rev 1908 | ||
|---|---|---|---|
| Line 293... | Line 293... | ||
| 293 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
293 | // 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. |
294 | // are less than ....., or reintroduce Kalman. |
| 295 | // Well actually the Z axis acc. check is not so silly. |
295 | // Well actually the Z axis acc. check is not so silly. |
| 296 | uint8_t axis; |
296 | uint8_t axis; |
| 297 | int32_t temp; |
297 | int32_t temp; |
| - | 298 | DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
|
| - | 299 | DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
|
| - | 300 | ||
| 298 | if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
301 | if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
| 299 | <= dynamicParams.UserParams[7]) { |
302 | <= dynamicParams.UserParams[7]) { |
| 300 | DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
- | |
| Line 301... | Line 303... | ||
| 301 | 303 | ||
| 302 | uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
- | |
| 303 | uint8_t debugFullWeight = 1; |
304 | uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
| Line -... | Line 305... | ||
| - | 305 | int32_t accDerived; |
|
| 304 | int32_t accDerived; |
306 | |
| 305 | 307 | /* |
|
| 306 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
308 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
| 307 | permilleAcc /= 2; |
309 | permilleAcc /= 2; |
| 308 | debugFullWeight = 0; |
310 | debugFullWeight = 0; |
| 309 | } |
311 | } |
| 310 | 312 | */ |
|
| - | 313 | if (controlActivity > 10000) { // reduce effect during stick commands |
|
| - | 314 | permilleAcc /= 4; |
|
| 311 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands |
315 | DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| - | 316 | if (controlActivity > 20000) { // reduce effect during stick commands |
|
| - | 317 | permilleAcc /= 4; |
|
| 312 | permilleAcc /= 2; |
318 | DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
| Line 313... | Line -... | ||
| 313 | debugFullWeight = 0; |
- | |
| 314 | } |
- | |
| 315 | - | ||
| 316 | if (debugFullWeight) |
- | |
| 317 | DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
- | |
| 318 | else |
319 | } |
| 319 | DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
320 | } |
| 320 | 321 | ||
| 321 | /* |
322 | /* |
| 322 | * Add to each sum: The amount by which the angle is changed just below. |
323 | * Add to each sum: The amount by which the angle is changed just below. |
| Line 330... | Line 331... | ||
| 330 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
331 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| 331 | + (int32_t) permilleAcc * accDerived) / 1000L; |
332 | + (int32_t) permilleAcc * accDerived) / 1000L; |
| 332 | correctionSum[axis] += angle[axis] - temp; |
333 | correctionSum[axis] += angle[axis] - temp; |
| 333 | } |
334 | } |
| 334 | } else { |
335 | } else { |
| 335 | DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
- | |
| 336 | DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
- | |
| 337 | DebugOut.Analog[9] = 0; |
336 | DebugOut.Analog[9] = 0; |
| 338 | DebugOut.Analog[10] = 0; |
337 | DebugOut.Analog[10] = 0; |
| Line 339... | Line 338... | ||
| 339 | 338 | ||
| 340 | DebugOut.Analog[16] = 0; |
339 | DebugOut.Analog[16] = 0; |
| Line 450... | Line 449... | ||
| 450 | // As readable formula: |
449 | // As readable formula: |
| 451 | // w = dynamicParams.CompassYawEffect * (1-w/32); |
450 | // w = dynamicParams.CompassYawEffect * (1-w/32); |
| Line 452... | Line 451... | ||
| 452 | 451 | ||
| 453 | if (w >= 0) { // maxAttitudeAngle < 32 |
452 | if (w >= 0) { // maxAttitudeAngle < 32 |
| 454 | if (!ignoreCompassTimer) { |
453 | if (!ignoreCompassTimer) { |
| - | 454 | /*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/ |
|
| 455 | v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
455 | v = 64 + controlActivity / 100; |
| 456 | // yawGyroHeading - compassCourse on a -180..179 degree interval. |
456 | // yawGyroHeading - compassCourse on a -180..179 degree interval. |
| 457 | r |
457 | r |
| 458 | = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
458 | = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
| 459 | % 360) - 180; |
459 | % 360) - 180; |