Rev 1612 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1612 | Rev 1616 | ||
|---|---|---|---|
| Line 118... | Line 118... | ||
| 118 | 118 | ||
| 119 | // uint8_t FunnelCourse = 0; |
119 | // uint8_t FunnelCourse = 0; |
| 120 | uint16_t badCompassHeading = 500; |
120 | uint16_t badCompassHeading = 500; |
| Line 121... | Line 121... | ||
| 121 | int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
121 | int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
| 122 | 122 | ||
| - | 123 | #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
|
| Line 123... | Line 124... | ||
| 123 | int32_t turnOver180 = GYRO_DEG_FACTOR_PITCHROLL * 180L; |
124 | #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
| Line 124... | Line 125... | ||
| 124 | int32_t turnOver360 = GYRO_DEG_FACTOR_PITCHROLL * 360L; |
125 | #define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
| 125 | 126 | ||
| Line 135... | Line 136... | ||
| 135 | 136 | ||
| 136 | /************************************************************************ |
137 | /************************************************************************ |
| 137 | * Set inclination angles from the acc. sensor data. |
138 | * Set inclination angles from the acc. sensor data. |
| 138 | * If acc. sensors are not used, set to zero. |
139 | * If acc. sensors are not used, set to zero. |
| 139 | * TODO: One could use inverse sine to calculate the angles more |
140 | * TODO: One could use inverse sine to calculate the angles more |
| 140 | * accurately, but sinc: 1) the angles are rather at times when it |
141 | * accurately, but since: 1) the angles are rather small at times when |
| 141 | * makes sense to set the integrals (standing on ground, or flying at |
142 | * it makes sense to set the integrals (standing on ground, or flying at |
| 142 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
143 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
| 143 | * it is hardly worth the trouble. |
144 | * it is hardly worth the trouble. |
| Line 144... | Line 145... | ||
| 144 | ************************************************************************/ |
145 | ************************************************************************/ |
| Line 205... | Line 206... | ||
| 205 | rollDifferential = rollGyroD; |
206 | rollDifferential = rollGyroD; |
| Line 206... | Line 207... | ||
| 206 | 207 | ||
| Line 207... | Line 208... | ||
| 207 | yawRate = yawGyro + dynamicOffsetYaw; |
208 | yawRate = yawGyro + dynamicOffsetYaw; |
| 208 | - | ||
| 209 | // We are done reading variables from the analog module. Interrupt-driven sensor reading may restart. |
209 | |
| 210 | // TODO: Is that not a little early to measure for next control invocation? |
210 | // We are done reading variables from the analog module. Interrupt-driven sensor reading may restart. |
| 211 | analogDataReady = 0; |
211 | analogDataReady = 0; |
| Line 212... | Line 212... | ||
| 212 | analog_start(); |
212 | analog_start(); |
| Line 305... | Line 305... | ||
| 305 | * Calculate yaw gyro integral (~ to rotation angle) |
305 | * Calculate yaw gyro integral (~ to rotation angle) |
| 306 | * Limit yawGyroHeading proportional to 0 deg to 360 deg |
306 | * Limit yawGyroHeading proportional to 0 deg to 360 deg |
| 307 | */ |
307 | */ |
| 308 | yawGyroHeading += ACYawRate; |
308 | yawGyroHeading += ACYawRate; |
| 309 | yawAngle += ACYawRate; |
309 | yawAngle += ACYawRate; |
| 310 | if(yawGyroHeading >= (360L * GYRO_DEG_FACTOR_YAW)) yawGyroHeading -= 360L * GYRO_DEG_FACTOR_YAW; // 360 deg. wrap |
310 | if(yawGyroHeading >= YAWOVER360) yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
| 311 | if(yawGyroHeading < 0) yawGyroHeading += 360L * GYRO_DEG_FACTOR_YAW; |
311 | else if(yawGyroHeading < 0) yawGyroHeading += YAWOVER360; |
| Line 312... | Line 312... | ||
| 312 | 312 | ||
| 313 | /* |
313 | /* |
| 314 | * Pitch axis integration and range boundary wrap. |
314 | * Pitch axis integration and range boundary wrap. |
| 315 | */ |
315 | */ |
| 316 | pitchAngle += ACPitchRate; |
316 | pitchAngle += ACPitchRate; |
| 317 | if(pitchAngle > turnOver180) { |
317 | if(pitchAngle > PITCHROLLOVER180) { |
| 318 | pitchAngle -= turnOver360; |
318 | pitchAngle -= PITCHROLLOVER360; |
| 319 | } else if (pitchAngle <= -turnOver180) { |
319 | } else if (pitchAngle <= -PITCHROLLOVER180) { |
| 320 | pitchAngle += turnOver360; |
320 | pitchAngle += PITCHROLLOVER360; |
| Line 321... | Line 321... | ||
| 321 | } |
321 | } |
| 322 | 322 | ||
| 323 | /* |
323 | /* |
| 324 | * Pitch axis integration and range boundary wrap. |
324 | * Pitch axis integration and range boundary wrap. |
| 325 | */ |
325 | */ |
| 326 | rollAngle += ACRollRate; |
326 | rollAngle += ACRollRate; |
| 327 | if(rollAngle > turnOver180) { |
327 | if(rollAngle > PITCHROLLOVER180) { |
| 328 | rollAngle -= turnOver360; |
328 | rollAngle -= PITCHROLLOVER360; |
| 329 | } else if (rollAngle <= -turnOver180) { |
329 | } else if (rollAngle <= -PITCHROLLOVER180) { |
| 330 | rollAngle += turnOver360; |
330 | rollAngle += PITCHROLLOVER360; |
| Line 331... | Line 331... | ||
| 331 | } |
331 | } |
| 332 | } |
332 | } |