Rev 1796 | Rev 1869 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1796 | Rev 1821 | ||
---|---|---|---|
Line 22... | Line 22... | ||
22 | 22 | ||
23 | // Temporarily replaced by userparam-configurable variable. |
23 | // Temporarily replaced by userparam-configurable variable. |
Line 24... | Line 24... | ||
24 | // #define ACC_FILTER 4 |
24 | // #define ACC_FILTER 4 |
25 | 25 | ||
26 | /* |
26 | /* |
27 | About setting constants for different gyros: |
27 | About setting constants for different gyros: |
28 | Main parameters are positive directions and voltage/angular speed gain. |
28 | Main parameters are positive directions and voltage/angular speed gain. |
29 | The "Positive direction" is the rotation direction around an axis where |
29 | The "Positive direction" is the rotation direction around an axis where |
30 | the corresponding gyro outputs a voltage > the no-rotation voltage. |
30 | the corresponding gyro outputs a voltage > the no-rotation voltage. |
31 | A gyro is considered, in this code, to be "forward" if its positive |
31 | A gyro is considered, in this code, to be "forward" if its positive |
32 | direction is: |
32 | direction is: |
33 | - Nose down for pitch |
33 | - Nose down for pitch |
34 | - Left hand side down for roll |
34 | - Left hand side down for roll |
35 | - Clockwise seen from above for yaw. |
35 | - Clockwise seen from above for yaw. |
- | 36 | Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and |
|
- | 37 | GYRO_REVERSE_PITCH #define's if the respective gyros are reverse. |
|
- | 38 | ||
- | 39 | Setting gyro gain correctly: All sensor measurements in analog.c take |
|
- | 40 | place in a cycle, each cycle comprising all sensors. Some sensors are |
|
- | 41 | sampled more than ones, and the results added. The pitch and roll gyros |
|
- | 42 | are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74 |
|
- | 43 | code. |
|
- | 44 | In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0) |
|
- | 45 | or 4 (other versions), offset to zero, low pass filtered and then assigned |
|
- | 46 | to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or |
|
- | 47 | roll. |
|
- | 48 | So: |
|
- | 49 | ||
- | 50 | gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4), |
|
- | 51 | where V is 2 for FC1.0 and 4 for all others. |
|
Line 36... | Line -... | ||
36 | Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and |
- | |
37 | GYRO_REVERSE_PITCH #define's if the respective gyros are reverse. |
- | |
38 | |
- | |
39 | Setting gyro gain correctly: All sensor measurements in analog.c take |
- | |
40 | place in a cycle, each cycle comprising all sensors. Some sensors are |
- | |
41 | sampled more than ones, and the results added. The pitch and roll gyros |
- | |
42 | are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74 |
- | |
43 | code. |
- | |
44 | In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0) |
- | |
45 | or 4 (other versions), offset to zero, low pass filtered and then assigned |
- | |
46 | to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or |
- | |
47 | roll. |
- | |
48 | So: |
- | |
49 | |
- | |
50 | gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4), |
- | |
51 | where V is 2 for FC1.0 and 4 for all others. |
- | |
52 | |
52 | |
53 | Assuming constant ADCValue, in the H&I code: |
- | |
54 | |
- | |
55 | gyro = I * ADCValue. |
- | |
56 | - | ||
57 | where I is 8 for FC1.0 and 16 for all others. |
- | |
58 | - | ||
59 | The relation between rotation rate and ADCValue: |
- | |
60 | ADCValue [units] = |
- | |
61 | rotational speed [deg/s] * |
- | |
62 | gyro sensitivity [V / deg/s] * |
- | |
63 | amplifier gain [units] * |
- | |
64 | 1024 [units] / |
- | |
65 | 3V full range [V] |
- | |
66 | - | ||
67 | or: H is the number of steps the ADC value changes with, |
- | |
68 | for a 1 deg/s change in rotational velocity: |
- | |
69 | H = ADCValue [units] / rotation rate [deg/s] = |
- | |
70 | gyro sensitivity [V / deg/s] * |
- | |
71 | amplifier gain [units] * |
- | |
72 | 1024 [units] / |
- | |
73 | 3V full range [V] |
- | |
74 | - | ||
75 | Examples: |
- | |
76 | FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7: |
- | |
77 | H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s). |
- | |
78 | FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers: |
- | |
79 | H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s). |
- | |
80 | My InvenSense copter has 2mV/deg/s gyros and no amplifiers: |
- | |
Line -... | Line 53... | ||
- | 53 | Assuming constant ADCValue, in the H&I code: |
|
- | 54 | |
|
- | 55 | gyro = I * ADCValue. |
|
- | 56 | ||
- | 57 | where I is 8 for FC1.0 and 16 for all others. |
|
- | 58 | ||
- | 59 | The relation between rotation rate and ADCValue: |
|
- | 60 | ADCValue [units] = |
|
- | 61 | rotational speed [deg/s] * |
|
- | 62 | gyro sensitivity [V / deg/s] * |
|
- | 63 | amplifier gain [units] * |
|
- | 64 | 1024 [units] / |
|
- | 65 | 3V full range [V] |
|
- | 66 | ||
- | 67 | or: H is the number of steps the ADC value changes with, |
|
- | 68 | for a 1 deg/s change in rotational velocity: |
|
- | 69 | H = ADCValue [units] / rotation rate [deg/s] = |
|
- | 70 | gyro sensitivity [V / deg/s] * |
|
- | 71 | amplifier gain [units] * |
|
- | 72 | 1024 [units] / |
|
- | 73 | 3V full range [V] |
|
- | 74 | ||
- | 75 | Examples: |
|
- | 76 | FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7: |
|
- | 77 | H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s). |
|
- | 78 | FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers: |
|
- | 79 | H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s). |
|
- | 80 | My InvenSense copter has 2mV/deg/s gyros and no amplifiers: |
|
81 | H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s) |
81 | H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s) |
82 | (only about half as sensitive as V1.3. But it will take about twice the |
82 | (only about half as sensitive as V1.3. But it will take about twice the |
Line 83... | Line 83... | ||
83 | rotation rate!) |
83 | rotation rate!) |
84 | 84 | ||
85 | All together: gyro = I * H * rotation rate [units / (deg/s)]. |
85 | All together: gyro = I * H * rotation rate [units / (deg/s)]. |
86 | */ |
86 | */ |
Line 106... | Line 106... | ||
106 | 106 | ||
107 | #define ACC_SUMMATION_FACTOR_PITCHROLL 2 |
107 | #define ACC_SUMMATION_FACTOR_PITCHROLL 2 |
Line 108... | Line 108... | ||
108 | #define ACC_SUMMATION_FACTOR_Z 1 |
108 | #define ACC_SUMMATION_FACTOR_Z 1 |
109 | 109 | ||
110 | /* |
110 | /* |
111 | Integration: |
111 | Integration: |
112 | The HiResXXXX values are divided by 8 (in H&I firmware) before integration. |
112 | The HiResXXXX values are divided by 8 (in H&I firmware) before integration. |
113 | In the Killagreg rewrite of the H&I firmware, the factor 8 is called |
113 | In the Killagreg rewrite of the H&I firmware, the factor 8 is called |
114 | HIRES_GYRO_AMPLIFY. In this code, it is called HIRES_GYRO_INTEGRATION_FACTOR, |
114 | HIRES_GYRO_AMPLIFY. In this code, it is called HIRES_GYRO_INTEGRATION_FACTOR, |
115 | and care has been taken that all other constants (gyro to degree factor, and |
115 | and care has been taken that all other constants (gyro to degree factor, and |
116 | 180 degree flip-over detection limits) are corrected to it. Because the |
116 | 180 degree flip-over detection limits) are corrected to it. Because the |
117 | division by the constant takes place in the flight attitude code, the |
117 | division by the constant takes place in the flight attitude code, the |
118 | constant is there. |
118 | constant is there. |
119 | 119 | ||
120 | The control loop executes every 2ms, and for each iteration |
120 | The control loop executes every 2ms, and for each iteration |
121 | gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL]. |
121 | gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL]. |
122 | Assuming a constant rotation rate v and a zero initial gyroIntegral |
122 | Assuming a constant rotation rate v and a zero initial gyroIntegral |
123 | (for this explanation), we get: |
123 | (for this explanation), we get: |
124 | |
124 | |
125 | gyroIntegral = |
125 | gyroIntegral = |
126 | N * gyro / HIRES_GYRO_INTEGRATION_FACTOR = |
126 | N * gyro / HIRES_GYRO_INTEGRATION_FACTOR = |
127 | N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
127 | N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
128 | |
128 | |
129 | where N is the number of summations; N = t/2ms. |
129 | where N is the number of summations; N = t/2ms. |
130 | 130 | ||
131 | For one degree of rotation: t*v = 1: |
131 | For one degree of rotation: t*v = 1: |
132 | 132 | ||
133 | gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR. |
133 | gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR. |
134 | 134 | ||
135 | This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor. |
135 | This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor. |
136 | 136 | ||
137 | Examples: |
137 | Examples: |
138 | FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304 |
138 | FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304 |
139 | FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260 |
139 | FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260 |
Line 140... | Line 140... | ||
140 | My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365 |
140 | My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365 |
141 | */ |
141 | */ |
142 | 142 | ||
143 | /* |
143 | /* |
Line 237... | Line 237... | ||
237 | /* |
237 | /* |
238 | * Flag: Interrupt handler has done all A/D conversion and processing. |
238 | * Flag: Interrupt handler has done all A/D conversion and processing. |
239 | */ |
239 | */ |
240 | extern volatile uint8_t analogDataReady; |
240 | extern volatile uint8_t analogDataReady; |
Line 241... | Line -... | ||
241 | - | ||
242 | 241 | ||
Line 243... | Line 242... | ||
243 | void analog_init(void); |
242 | void analog_init(void); |
244 | 243 |