Rev 2084 | 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; |