Rev 2084 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2084 | Rev 2086 | ||
|---|---|---|---|
| Line 399... | Line 399... | ||
| 399 | return; |
399 | return; |
| Line 400... | Line 400... | ||
| 400 | 400 | ||
| 401 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
401 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
| 402 | //targetHeading = heading; |
402 | //targetHeading = heading; |
| 403 | headingError = 0; |
- | |
| 404 | - | ||
| 405 | debugOut.digital[0] ^= DEBUG_COMPASS; |
403 | headingError = 0; |
| Line 406... | Line 404... | ||
| 406 | } |
404 | } |
| 407 | 405 | ||
| Line 450... | Line 448... | ||
| 450 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
448 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
| Line 451... | Line 449... | ||
| 451 | 449 | ||
| 452 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
450 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
| Line -... | Line 451... | ||
| - | 451 | //debugOut.analog[30] = correction; |
|
| - | 452 | ||
| - | 453 | debugOut.digital[0] = &= ~DEBUG_COMPASS; |
|
| - | 454 | debugOut.digital[1] = &= ~DEBUG_COMPASS; |
|
| - | 455 | ||
| - | 456 | if (correction > 0) { |
|
| - | 457 | debugOut.digital[0] ^= DEBUG_COMPASS; |
|
| - | 458 | } else if (correction < 0) { |
|
| - | 459 | debugOut.digital[1] ^= DEBUG_COMPASS; |
|
| 453 | //debugOut.analog[30] = correction; |
460 | } |
| 454 | 461 | ||
| 455 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
462 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
| 456 | // and to the heading error (the angle of yaw that the copter is off the set heading). |
463 | // and to the heading error (the angle of yaw that the copter is off the set heading). |
| 457 | heading += correction; |
464 | heading += correction; |