401,8 → 401,6 |
heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
//targetHeading = heading; |
headingError = 0; |
|
debugOut.digital[0] ^= DEBUG_COMPASS; |
} |
|
void correctHeadingToMagnetic(void) { |
452,6 → 450,15 |
int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
//debugOut.analog[30] = correction; |
|
debugOut.digital[0] = &= ~DEBUG_COMPASS; |
debugOut.digital[1] = &= ~DEBUG_COMPASS; |
|
if (correction > 0) { |
debugOut.digital[0] ^= DEBUG_COMPASS; |
} else if (correction < 0) { |
debugOut.digital[1] ^= DEBUG_COMPASS; |
} |
|
// The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
// and to the heading error (the angle of yaw that the copter is off the set heading). |
heading += correction; |