Rev 1612 | Rev 1805 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1612 | Rev 1645 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | /*####################################################################################### |
1 | /*********************************************************************************/ |
| 2 | Attitude sense system (processing of gyro, accelerometer and altimeter data) |
2 | /* Attitude sense system (processing of gyro, accelerometer and altimeter data) */ |
| 3 | #######################################################################################*/ |
3 | /*********************************************************************************/ |
| Line 4... | Line 4... | ||
| 4 | 4 | ||
| 5 | #ifndef _ATTITUDE_H |
5 | #ifndef _ATTITUDE_H |
| Line 6... | Line 6... | ||
| 6 | #define _ATTITUDE_H |
6 | #define _ATTITUDE_H |
| Line 31... | Line 31... | ||
| 31 | 31 | ||
| 32 | /* |
32 | /* |
| 33 | * Gyro readings are divided by this before being used in attitude control. This will scale them |
33 | * Gyro readings are divided by this before being used in attitude control. This will scale them |
| 34 | * to match the scale of the stick control etc. variables. This is just a rough non-precision |
34 | * to match the scale of the stick control etc. variables. This is just a rough non-precision |
| 35 | * scaling - the below definitions make precise conversion factors. |
35 | * scaling - the below definitions make precise conversion factors. |
| 36 | * Will be about 4 for InvenSense, 8 for FC1.3 and 13 for ADXRS610 (hmmm - originally is was only 8???) |
36 | * Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610. |
| 37 | * The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees. |
37 | * The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees. |
| 38 | */ |
38 | */ |
| Line 39... | Line 39... | ||
| 39 | #define HIRES_GYRO_INTEGRATION_FACTOR (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250) |
39 | #define HIRES_GYRO_INTEGRATION_FACTOR (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250) |
| Line 79... | Line 79... | ||
| 79 | #define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
79 | #define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
| Line 80... | Line 80... | ||
| 80 | 80 | ||
| 81 | /* |
81 | /* |
| 82 | * Rotation rates |
82 | * Rotation rates |
| 83 | */ |
83 | */ |
| 84 | extern int16_t pitchRate_PID, rollRate_PID, yawRate; |
84 | extern int16_t rate_PID[2], yawRate; |
| Line 85... | Line 85... | ||
| 85 | extern int16_t pitchDifferential, rollDifferential; |
85 | extern int16_t differential[2]; |
| 86 | 86 | ||
| 87 | /* |
87 | /* |
| 88 | * Attitudes calcualted by numerical integration of gyro rates |
88 | * Attitudes calculated by numerical integration of gyro rates |
| Line 89... | Line 89... | ||
| 89 | */ |
89 | */ |
| Line 90... | Line 90... | ||
| 90 | extern int32_t pitchAngle, rollAngle, yawAngle; |
90 | extern int32_t angle[2], yawAngle; |
| 91 | 91 | ||
| Line 102... | Line 102... | ||
| 102 | extern int16_t yawGyroHeadingInDeg; |
102 | extern int16_t yawGyroHeadingInDeg; |
| 103 | extern uint16_t updateCompassCourse; |
103 | extern uint16_t updateCompassCourse; |
| 104 | extern uint16_t badCompassHeading; |
104 | extern uint16_t badCompassHeading; |
| Line 105... | Line 105... | ||
| 105 | 105 | ||
| 106 | /* |
- | |
| 107 | * Height control |
- | |
| 108 | */ |
- | |
| 109 | extern int readingHeight; |
- | |
| 110 | extern int setPointHeight; |
- | |
| 111 | - | ||
| 112 | /* |
106 | /* |
| 113 | * Interval wrap-over values for attitude integrals |
107 | * Interval wrap-over values for attitude integrals |
| 114 | */ |
108 | */ |
| Line 115... | Line 109... | ||
| 115 | extern long turnOver180Pitch, turnOver180Roll; |
109 | extern long turnOver180Pitch, turnOver180Roll; |
| Line 125... | Line 119... | ||
| 125 | * - Summing up how much acc. meter correction was done to the gyro integrals over the last n |
119 | * - Summing up how much acc. meter correction was done to the gyro integrals over the last n |
| 126 | * integration, and then adding the sum / n to the dynamic offset |
120 | * integration, and then adding the sum / n to the dynamic offset |
| 127 | * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
121 | * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
| 128 | * - Invent your own... |
122 | * - Invent your own... |
| 129 | */ |
123 | */ |
| 130 | extern int16_t dynamicOffsetPitch, dynamicOffsetRoll, dynamicOffsetYaw; |
- | |
| 131 | // extern int16_t savedDynamicOffsetPitch, savedDynamicOffsetRoll; |
124 | extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
| Line 132... | Line 125... | ||
| 132 | 125 | ||
| 133 | /* |
126 | /* |
| 134 | * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
127 | * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
| 135 | * To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right). |
128 | * To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right). |