| 53,20 → 53,13 |
|---|
| //int readingHeight = 0; |
| |
| // Yaw angle and compass stuff. |
| int32_t headingError; |
| |
| // This is updated/written from MM3. Negative angle indicates invalid data. |
| int16_t magneticHeading = -1; |
| |
| // This is NOT updated from MM3. Negative angle indicates invalid data. |
| // int16_t headingInDegrees = -1; |
| |
| int32_t targetHeading; |
| |
| // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
| // Not necessary. Never read anywhere. |
| // int16_t compassOffCourse = 0; |
| |
| uint16_t ignoreCompassTimer = 500; |
| uint16_t ignoreCompassTimer = 0;// 500; |
| |
| int32_t heading; // Yaw Gyro Integral supported by compass |
| int16_t yawGyroDrift; |
| 203,6 → 196,11 |
|---|
| */ |
| heading += ACYawRate; |
| intervalWrap(&heading, YAWOVER360); |
| |
| headingError += ACYawRate; |
| |
| debugOut.analog[27] = heading / 100; |
| |
| /* |
| * Pitch axis integration and range boundary wrap. |
| */ |
| 346,7 → 344,8 |
|---|
| return; |
| |
| heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
| targetHeading = heading; |
| //targetHeading = heading; |
| headingError = 0; |
| |
| debugOut.digital[0] ^= DEBUG_COMPASS; |
| } |
| 354,10 → 353,10 |
|---|
| void correctHeadingToMagnetic(void) { |
| int32_t error; |
| |
| debugOut.analog[27] = heading; |
| |
| if (commands_isCalibratingCompass()) |
| if (commands_isCalibratingCompass()) { |
| debugOut.analog[29] = 1; |
| return; |
| } |
| |
| // Compass is off, skip. |
| // Naaah this is assumed. |
| 365,21 → 364,28 |
|---|
| // return; |
| |
| // Compass is invalid, skip. |
| if (magneticHeading < 0) |
| if (magneticHeading < 0) { |
| debugOut.analog[29] = 2; |
| return; |
| } |
| |
| // Spinning fast, skip |
| if (abs(yawRate) > 128) |
| if (abs(yawRate) > 128) { |
| debugOut.analog[29] = 3; |
| return; |
| } |
| |
| // Otherwise invalidated, skip |
| if (ignoreCompassTimer) { |
| ignoreCompassTimer--; |
| debugOut.analog[29] = 4; |
| return; |
| } |
| |
| // TODO: Find computational cost of this. |
| error = (magneticHeading*GYRO_DEG_FACTOR_YAW - heading) % GYRO_DEG_FACTOR_YAW; |
| error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading); |
| if (error <= -YAWOVER180) error += YAWOVER360; |
| else if (error > YAWOVER180) error -= YAWOVER360; |
| |
| // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
| // better resolution of the gyros to the worse resolution of the compass all the time. |
| 386,7 → 392,8 |
|---|
| // The correction should really only serve to compensate for gyro drift. |
| if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
| |
| int32_t correction = (error * (int32_t)dynamicParams.compassYawEffect) >> 8; |
| int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
| debugOut.analog[30] = correction; |
| |
| // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
| // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
| 395,10 → 402,11 |
|---|
| heading += correction; |
| intervalWrap(&heading, YAWOVER360); |
| |
| targetHeading += correction; |
| intervalWrap(&targetHeading, YAWOVER360); |
| |
| debugOut.digital[1] ^= DEBUG_COMPASS; |
| // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all |
| // when the compass corrects the heading - it only corrects numbers!) we want to add: |
| // This will however cause drift to remain uncorrected! |
| // headingError += correction; |
| debugOut.analog[29] = 0; |
| } |
| |
| /************************************************************************ |