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