| 120,8 → 120,9 |
|---|
| uint16_t badCompassHeading = 500; |
| int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
| |
| int32_t turnOver180 = GYRO_DEG_FACTOR_PITCHROLL * 180L; |
| int32_t turnOver360 = GYRO_DEG_FACTOR_PITCHROLL * 360L; |
| #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
| #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
| #define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
| |
| int32_t pitchCorrectionSum = 0, rollCorrectionSum = 0; |
| |
| 137,8 → 138,8 |
|---|
| * Set inclination angles from the acc. sensor data. |
| * If acc. sensors are not used, set to zero. |
| * TODO: One could use inverse sine to calculate the angles more |
| * accurately, but sinc: 1) the angles are rather at times when it |
| * makes sense to set the integrals (standing on ground, or flying at |
| * accurately, but since: 1) the angles are rather small at times when |
| * it makes sense to set the integrals (standing on ground, or flying at |
| * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
| * it is hardly worth the trouble. |
| ************************************************************************/ |
| 207,7 → 208,6 |
|---|
| yawRate = yawGyro + dynamicOffsetYaw; |
| |
| // We are done reading variables from the analog module. Interrupt-driven sensor reading may restart. |
| // TODO: Is that not a little early to measure for next control invocation? |
| analogDataReady = 0; |
| analog_start(); |
| } |
| 307,17 → 307,17 |
|---|
| */ |
| yawGyroHeading += ACYawRate; |
| yawAngle += ACYawRate; |
| if(yawGyroHeading >= (360L * GYRO_DEG_FACTOR_YAW)) yawGyroHeading -= 360L * GYRO_DEG_FACTOR_YAW; // 360 deg. wrap |
| if(yawGyroHeading < 0) yawGyroHeading += 360L * GYRO_DEG_FACTOR_YAW; |
| if(yawGyroHeading >= YAWOVER360) yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
| else if(yawGyroHeading < 0) yawGyroHeading += YAWOVER360; |
| |
| /* |
| * Pitch axis integration and range boundary wrap. |
| */ |
| pitchAngle += ACPitchRate; |
| if(pitchAngle > turnOver180) { |
| pitchAngle -= turnOver360; |
| } else if (pitchAngle <= -turnOver180) { |
| pitchAngle += turnOver360; |
| if(pitchAngle > PITCHROLLOVER180) { |
| pitchAngle -= PITCHROLLOVER360; |
| } else if (pitchAngle <= -PITCHROLLOVER180) { |
| pitchAngle += PITCHROLLOVER360; |
| } |
| |
| /* |
| 324,10 → 324,10 |
|---|
| * Pitch axis integration and range boundary wrap. |
| */ |
| rollAngle += ACRollRate; |
| if(rollAngle > turnOver180) { |
| rollAngle -= turnOver360; |
| } else if (rollAngle <= -turnOver180) { |
| rollAngle += turnOver360; |
| if(rollAngle > PITCHROLLOVER180) { |
| rollAngle -= PITCHROLLOVER360; |
| } else if (rollAngle <= -PITCHROLLOVER180) { |
| rollAngle += PITCHROLLOVER360; |
| } |
| } |
| |