Rev 1612 | Rev 1775 | Go to most recent revision | Show entire file | Regard 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). |