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; |
} |
} |
|