Rev 1645 | Rev 1805 | Go to most recent revision | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1645 | Rev 1775 | ||
---|---|---|---|
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 | /*********************************************************************************/ |
4 | 4 | ||
5 | #ifndef _ATTITUDE_H |
5 | #ifndef _ATTITUDE_H |
6 | #define _ATTITUDE_H |
6 | #define _ATTITUDE_H |
7 | 7 | ||
8 | #include <inttypes.h> |
8 | #include <inttypes.h> |
9 | 9 | ||
10 | #include "analog.h" |
10 | #include "analog.h" |
11 | 11 | ||
12 | // For debugging only. |
12 | // For debugging only. |
13 | #include "uart0.h" |
13 | #include "uart0.h" |
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 | */ |
19 | #define ATTITUDE_USE_ACC_SENSORS yes |
19 | #define ATTITUDE_USE_ACC_SENSORS yes |
20 | 20 | ||
21 | /* |
21 | /* |
22 | * Default hysteresis to use for the -180 degree to 180 degree wrap. |
22 | * Default hysteresis to use for the -180 degree to 180 degree wrap. |
23 | */ |
23 | */ |
24 | #define PITCHOVER_HYSTERESIS 0L |
24 | #define PITCHOVER_HYSTERESIS 0L |
25 | #define ROLLOVER_HYSTERESIS 0L |
25 | #define ROLLOVER_HYSTERESIS 0L |
26 | 26 | ||
27 | /* |
27 | /* |
28 | * The frequency at which numerical integration takes place. 488 in original code. |
28 | * The frequency at which numerical integration takes place. 488 in original code. |
29 | */ |
29 | */ |
30 | #define INTEGRATION_FREQUENCY 488 |
30 | #define INTEGRATION_FREQUENCY 488 |
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 8 for ADXRS610. |
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 | */ |
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) |
40 | 40 | ||
41 | /* |
41 | /* |
42 | * Constant for deriving an attitude angle from acceleration measurement. |
42 | * Constant for deriving an attitude angle from acceleration measurement. |
43 | * |
43 | * |
44 | * The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef. |
44 | * The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef. |
45 | * 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each |
45 | * 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each |
46 | * [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g. |
46 | * [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g. |
47 | * We approximate a small pitch/roll angle v by assuming that the copter does not accelerate: |
47 | * We approximate a small pitch/roll angle v by assuming that the copter does not accelerate: |
48 | * In this explanation it is assumed that the ADC values are 0 based, and gravity is included. |
48 | * In this explanation it is assumed that the ADC values are 0 based, and gravity is included. |
49 | * The sine of v is the far side / the hypothenusis: |
49 | * The sine of v is the far side / the hypothenusis: |
50 | * sin v = acc / sqrt(acc^2 + acc_z^2) |
50 | * sin v = acc / sqrt(acc^2 + acc_z^2) |
51 | * Using that v is a small angle, and the near side is about equal to the the hypothenusis: |
51 | * Using that v is a small angle, and the near side is about equal to the the hypothenusis: |
52 | * sin v ~= acc / acc_z |
52 | * sin v ~= acc / acc_z |
53 | * Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410, |
53 | * Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410, |
54 | * and sin v ~= v (small angles, in radians): |
54 | * and sin v ~= v (small angles, in radians): |
55 | * sin v ~= acc / 410 |
55 | * sin v ~= acc / 410 |
56 | * v / 57.3 ~= acc / 410 |
56 | * v / 57.3 ~= acc / 410 |
57 | * v ~= acc * 57.3 / 410 |
57 | * v ~= acc * 57.3 / 410 |
58 | * acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree. |
58 | * acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree. |
59 | * |
59 | * |
60 | * Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3) |
60 | * Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3) |
61 | */ |
61 | */ |
62 | #define DEG_ACC_FACTOR 7 |
62 | #define DEG_ACC_FACTOR 7 |
63 | 63 | ||
64 | /* |
64 | /* |
65 | * Growth of the integral per degree: |
65 | * Growth of the integral per degree: |
66 | * The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by |
66 | * The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by |
67 | * HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration. |
67 | * HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration. |
68 | * The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable). |
68 | * The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable). |
69 | */ |
69 | */ |
70 | #define GYRO_DEG_FACTOR_PITCHROLL (int)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR) |
70 | #define GYRO_DEG_FACTOR_PITCHROLL (int)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR) |
71 | #define GYRO_DEG_FACTOR_YAW (int)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION) |
71 | #define GYRO_DEG_FACTOR_YAW (int)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION) |
72 | 72 | ||
73 | /* |
73 | /* |
74 | * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value |
74 | * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value |
75 | * = the factor an acc. sensor should be multiplied by to get the gyro integral |
75 | * = the factor an acc. sensor should be multiplied by to get the gyro integral |
76 | * value for the same (small) angle. |
76 | * value for the same (small) angle. |
77 | * The value is about 200. |
77 | * The value is about 200. |
78 | */ |
78 | */ |
79 | #define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
79 | #define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
80 | 80 | ||
81 | /* |
81 | /* |
82 | * Rotation rates |
82 | * Rotation rates |
83 | */ |
83 | */ |
84 | extern int16_t rate_PID[2], yawRate; |
84 | extern int16_t rate_PID[2], rate_ATT[2], yawRate; |
85 | extern int16_t differential[2]; |
85 | extern int16_t differential[2]; |
86 | 86 | ||
87 | /* |
87 | /* |
88 | * Attitudes calculated by numerical integration of gyro rates |
88 | * Attitudes calculated by numerical integration of gyro rates |
89 | */ |
89 | */ |
90 | extern int32_t angle[2], yawAngle; |
90 | extern int32_t angle[2], yawAngleDiff; |
91 | 91 | ||
92 | // extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
92 | // extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
93 | 93 | ||
94 | /* |
94 | /* |
95 | * Compass navigation |
95 | * Compass navigation |
96 | */ |
96 | */ |
97 | extern int16_t compassHeading; |
97 | extern int16_t compassHeading; |
98 | extern int16_t compassCourse; |
98 | extern int16_t compassCourse; |
99 | extern int16_t compassOffCourse; |
99 | extern int16_t compassOffCourse; |
100 | extern uint8_t compassCalState; |
100 | extern uint8_t compassCalState; |
101 | extern int32_t yawGyroHeading; |
101 | extern int32_t yawGyroHeading; |
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; |
- | 105 | ||
- | 106 | void updateCompass(void); |
|
105 | 107 | ||
106 | /* |
108 | /* |
107 | * Interval wrap-over values for attitude integrals |
109 | * Interval wrap-over values for attitude integrals |
108 | */ |
110 | */ |
109 | extern long turnOver180Pitch, turnOver180Roll; |
111 | extern long turnOver180Pitch, turnOver180Roll; |
110 | 112 | ||
111 | // No longer used. |
113 | // No longer used. |
112 | // extern uint8_t FunnelCourse; |
114 | // extern uint8_t FunnelCourse; |
113 | 115 | ||
114 | /* |
116 | /* |
115 | * Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements, |
117 | * Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements, |
116 | * to help cancelling out drift and vibration noise effects. The dynamic offsets themselves |
118 | * to help cancelling out drift and vibration noise effects. The dynamic offsets themselves |
117 | * can be updated in flight by different ways, for example: |
119 | * can be updated in flight by different ways, for example: |
118 | * - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool |
120 | * - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool |
119 | * - Summing up how much acc. meter correction was done to the gyro integrals over the last n |
121 | * - Summing up how much acc. meter correction was done to the gyro integrals over the last n |
120 | * integration, and then adding the sum / n to the dynamic offset |
122 | * integration, and then adding the sum / n to the dynamic offset |
121 | * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
123 | * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
122 | * - Invent your own... |
124 | * - Invent your own... |
123 | */ |
125 | */ |
124 | extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
126 | extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
125 | 127 | ||
126 | /* |
128 | /* |
- | 129 | * For NaviCtrl use. |
|
- | 130 | */ |
|
- | 131 | extern int16_t averageAcc[2], averageAccCount; |
|
- | 132 | ||
- | 133 | /* |
|
127 | * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
134 | * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
128 | * To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right). |
135 | * To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right). |
129 | */ |
136 | */ |
130 | void attitude_setNeutral(void); |
137 | void attitude_setNeutral(void); |
131 | 138 | ||
132 | /* |
139 | /* |
133 | * Experiment. |
140 | * Experiment. |
134 | */ |
141 | */ |
135 | // void attitude_startDynamicCalibration(void); |
142 | // void attitude_startDynamicCalibration(void); |
136 | // void attitude_continueDynamicCalibration(void); |
143 | // void attitude_continueDynamicCalibration(void); |
- | 144 | ||
- | 145 | int32_t getAngleEstimateFromAcc(uint8_t axis); |
|
137 | 146 | ||
138 | /* |
147 | /* |
139 | * Main routine, called from the flight loop. |
148 | * Main routine, called from the flight loop. |
140 | */ |
149 | */ |
141 | void calculateFlightAttitude(void); |
150 | void calculateFlightAttitude(void); |
142 | #endif //_ATTITUDE_H |
151 | #endif //_ATTITUDE_H |
143 | 152 |