Rev 1927 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1927 | Rev 2099 | ||
|---|---|---|---|
| Line 14... | Line 14... | ||
| 14 | 14 | ||
| 15 | /* |
15 | /* |
| 16 | * If you have no acc. sensor or do not want to use it, remove this define. This will cause the |
16 | * If you have no acc. sensor or do not want to use it, remove this define. This will cause the |
| 17 | * acc. sensor to be ignored at attitude calibration. |
17 | * acc. sensor to be ignored at attitude calibration. |
| 18 | */ |
18 | */ |
| Line 19... | Line 19... | ||
| 19 | #define ATTITUDE_USE_ACC_SENSORS yes |
19 | //#define ATTITUDE_USE_ACC_SENSORS yes |
| 20 | 20 | ||
| 21 | /* |
21 | /* |
| 22 | * The frequency at which numerical integration takes place. 488 in original code. |
22 | * The frequency at which numerical integration takes place. 488 in original code. |
| Line 23... | Line 23... | ||
| 23 | */ |
23 | */ |
| 24 | #define INTEGRATION_FREQUENCY 488 |
- | |
| 25 | - | ||
| 26 | /* |
- | |
| 27 | * Gyro readings are divided by this before being used in attitude control. This will scale them |
- | |
| 28 | * to match the scale of the stick control etc. variables. This is just a rough non-precision |
- | |
| 29 | * scaling - the below definitions make precise conversion factors. |
- | |
| 30 | * Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610. |
- | |
| 31 | * The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees. |
- | |
| 32 | */ |
- | |
| 33 | #define HIRES_GYRO_INTEGRATION_FACTOR 1 |
- | |
| 34 | // (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250) |
24 | #define INTEGRATION_FREQUENCY 488 |
| 35 | 25 | ||
| 36 | /* |
26 | /* |
| 37 | * Constant for deriving an attitude angle from acceleration measurement. |
27 | * Constant for deriving an attitude angle from acceleration measurement. |
| 38 | * |
28 | * |
| Line 60... | Line 50... | ||
| 60 | * Growth of the integral per degree: |
50 | * Growth of the integral per degree: |
| 61 | * The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by |
51 | * The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by |
| 62 | * HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration. |
52 | * HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration. |
| 63 | * The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable). |
53 | * The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable). |
| 64 | */ |
54 | */ |
| 65 | #define GYRO_DEG_FACTOR_PITCHROLL (GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR) |
- | |
| 66 | #define GYRO_DEG_FACTOR_YAW (GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION) |
55 | #define GYRO_DEG_FACTOR (GYRO_RATE_FACTOR * INTEGRATION_FREQUENCY * GYRO_CORRECTION) |
| Line 67... | Line 56... | ||
| 67 | 56 | ||
| 68 | /* |
57 | /* |
| 69 | * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value |
58 | * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value |
| 70 | * = the factor an acc. sensor should be multiplied by to get the gyro integral |
59 | * = the factor an acc. sensor should be multiplied by to get the gyro integral |
| 71 | * value for the same (small) angle. |
60 | * value for the same (small) angle. |
| 72 | * The value is about 200. |
61 | * The value is about 200. |
| 73 | */ |
62 | */ |
| Line 74... | Line 63... | ||
| 74 | #define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
63 | //#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
| 75 | - | ||
| 76 | #define PITCHROLLOVER180 ((int32_t)GYRO_DEG_FACTOR_PITCHROLL * 180) |
64 | |
| Line 77... | Line 65... | ||
| 77 | #define PITCHROLLOVER360 ((int32_t)GYRO_DEG_FACTOR_PITCHROLL * 360) |
65 | #define OVER180 ((int32_t)GYRO_DEG_FACTOR * 180) |
| 78 | #define YAWOVER360 ((int32_t)GYRO_DEG_FACTOR_YAW * 360) |
66 | #define OVER360 ((int32_t)GYRO_DEG_FACTOR * 360) |
| 79 | 67 | ||
| 80 | /* |
68 | /* |
| 81 | * Rotation rates |
69 | * Rotation rates |
| Line 82... | Line 70... | ||
| 82 | */ |
70 | */ |
| 83 | extern int16_t rate_PID[2], rate_ATT[2], yawRate; |
71 | extern int16_t rate_PID[3], rate_ATT[3]; |
| 84 | extern int16_t differential[3]; |
72 | extern int16_t differential[3]; |
| 85 | 73 | ||
| - | 74 | /* |
|
| - | 75 | * Attitudes calculated by numerical integration of gyro rates |
|
| - | 76 | */ |
|
| - | 77 | extern int32_t attitude[3]; |
|
| - | 78 | ||
| - | 79 | // extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
|
| - | 80 | ||
| - | 81 | /* |
|
| - | 82 | * Compass navigation |
|
| - | 83 | */ |
|
| 86 | /* |
84 | // extern int16_t compassHeading; |
| - | 85 | // extern int16_t compassCourse; |
|
| - | 86 | // extern int16_t compassOffCourse; |
|
| - | 87 | // extern uint8_t compassCalState; |
|
| Line 87... | Line 88... | ||
| 87 | * Attitudes calculated by numerical integration of gyro rates |
88 | // extern int32_t yawGyroHeading; |
| 88 | */ |
89 | // extern int16_t yawGyroHeadingInDeg; |
| 89 | extern int32_t angle[3]; |
90 | // extern uint8_t updateCompassCourse; |
| 90 | extern int32_t error[3]; |
91 | // extern uint16_t ignoreCompassTimer; |
| Line 97... | Line 98... | ||
| 97 | * - Summing up how much acc. meter correction was done to the gyro integrals over the last n |
98 | * - Summing up how much acc. meter correction was done to the gyro integrals over the last n |
| 98 | * integration, and then adding the sum / n to the dynamic offset |
99 | * integration, and then adding the sum / n to the dynamic offset |
| 99 | * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
100 | * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
| 100 | * - Invent your own... |
101 | * - Invent your own... |
| 101 | */ |
102 | */ |
| 102 | extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
103 | // extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
| Line 103... | Line 104... | ||
| 103 | 104 | ||
| 104 | /* |
105 | /* |
| 105 | * For NaviCtrl use. |
106 | * For NaviCtrl use. |
| 106 | */ |
107 | */ |
| Line 107... | Line 108... | ||
| 107 | extern int16_t averageAcc[2], averageAccCount; |
108 | // extern int16_t averageAcc[2], averageAccCount; |
| 108 | 109 | ||
| 109 | /* |
110 | /* |
| 110 | * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
111 | * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |