Rev 1645 | Rev 1775 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1645 | Rev 1646 | ||
|---|---|---|---|
| 1 | #ifndef _ANALOG_H |
1 | #ifndef _ANALOG_H |
| 2 | #define _ANALOG_H |
2 | #define _ANALOG_H |
| 3 | #include <inttypes.h> |
3 | #include <inttypes.h> |
| 4 | 4 | ||
| 5 | // #include "invenSense.h" |
5 | //#include "invenSense.h" |
| - | 6 | //#include "ENC-03_FC1.3.h" |
|
| 6 | #include "ENC-03_FC1.3.h" |
7 | #include "ADXRS610_FC2.0.h" |
| 7 | 8 | ||
| 8 | /* |
9 | /* |
| 9 | * How much low pass filtering to apply for gyro_PID. |
10 | * How much low pass filtering to apply for gyro_PID. |
| 10 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
11 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
| 11 | * Temporarily replaced by userparam-configurable variable. |
12 | * Temporarily replaced by userparam-configurable variable. |
| 12 | */ |
13 | */ |
| 13 | #define GYROS_PIDFILTER 1 |
14 | // #define GYROS_PID_FILTER 1 |
| 14 | 15 | ||
| 15 | /* |
16 | /* |
| 16 | * How much low pass filtering to apply for gyro_ATT. |
17 | * How much low pass filtering to apply for gyro_ATT. |
| 17 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
18 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
| 18 | * Temporarily replaced by userparam-configurable variable. |
19 | * Temporarily replaced by userparam-configurable variable. |
| 19 | */ |
20 | */ |
| 20 | #define GYROS_INTEGRALFILTER 1 |
21 | // #define GYROS_ATT_FILTER 1 |
| 21 | 22 | ||
| 22 | // Temporarily replaced by userparam-configurable variable. |
23 | // Temporarily replaced by userparam-configurable variable. |
| 23 | //#define ACC_FILTER 4 |
24 | // #define ACC_FILTER 4 |
| 24 | 25 | ||
| 25 | /* |
26 | /* |
| 26 | About setting constants for different gyros: |
27 | About setting constants for different gyros: |
| 27 | Main parameters are positive directions and voltage/angular speed gain. |
28 | Main parameters are positive directions and voltage/angular speed gain. |
| 28 | The "Positive direction" is the rotation direction around an axis where |
29 | The "Positive direction" is the rotation direction around an axis where |
| 29 | the corresponding gyro outputs a voltage > the no-rotation voltage. |
30 | the corresponding gyro outputs a voltage > the no-rotation voltage. |
| 30 | 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: |
|
| - | 33 | - Nose down for pitch |
|
| - | 34 | - Left hand side down for roll |
|
| 31 | direction is the same as in FC1.0-1.3, and reverse otherwise. |
35 | - Clockwise seen from above for yaw. |
| 32 | Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and |
36 | Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and |
| 33 | GYRO_REVERSE_PITCH #define's if the respective gyros are reverse. |
37 | GYRO_REVERSE_PITCH #define's if the respective gyros are reverse. |
| 34 | |
38 | |
| 35 | Setting gyro gain correctly: All sensor measurements in analog.c take |
39 | Setting gyro gain correctly: All sensor measurements in analog.c take |
| 36 | place in a cycle, each cycle comprising all sensors. Some sensors are |
40 | place in a cycle, each cycle comprising all sensors. Some sensors are |
| 37 | sampled more than ones, and the results added. The pitch and roll gyros |
41 | sampled more than ones, and the results added. The pitch and roll gyros |
| 38 | are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74 |
42 | are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74 |
| 39 | code. |
43 | code. |
| 40 | In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0) |
44 | In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0) |
| 41 | or 4 (other versions), offset to zero, low pass filtered and then assigned |
45 | or 4 (other versions), offset to zero, low pass filtered and then assigned |
| 42 | to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or |
46 | to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or |
| 43 | roll. |
47 | roll. |
| 44 | So: |
48 | So: |
| 45 | |
49 | |
| 46 | gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4), |
50 | gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4), |
| 47 | where V is 2 for FC1.0 and 4 for all others. |
51 | where V is 2 for FC1.0 and 4 for all others. |
| 48 | |
52 | |
| 49 | Assuming constant ADCValue, in the H&I code: |
53 | Assuming constant ADCValue, in the H&I code: |
| 50 | |
54 | |
| 51 | gyro = I * ADCValue. |
55 | gyro = I * ADCValue. |
| 52 | 56 | ||
| 53 | where I is 8 for FC1.0 and 16 for all others. |
57 | where I is 8 for FC1.0 and 16 for all others. |
| 54 | 58 | ||
| 55 | The relation between rotation rate and ADCValue: |
59 | The relation between rotation rate and ADCValue: |
| 56 | ADCValue [units] = |
60 | ADCValue [units] = |
| 57 | rotational speed [deg/s] * |
61 | rotational speed [deg/s] * |
| 58 | gyro sensitivity [V / deg/s] * |
62 | gyro sensitivity [V / deg/s] * |
| 59 | amplifier gain [units] * |
63 | amplifier gain [units] * |
| 60 | 1024 [units] / |
64 | 1024 [units] / |
| 61 | 3V full range [V] |
65 | 3V full range [V] |
| 62 | 66 | ||
| 63 | or: H is the number of steps the ADC value changes with, |
67 | or: H is the number of steps the ADC value changes with, |
| 64 | for a 1 deg/s change in rotational velocity: |
68 | for a 1 deg/s change in rotational velocity: |
| 65 | H = ADCValue [units] / rotation rate [deg/s] = |
69 | H = ADCValue [units] / rotation rate [deg/s] = |
| 66 | gyro sensitivity [V / deg/s] * |
70 | gyro sensitivity [V / deg/s] * |
| 67 | amplifier gain [units] * |
71 | amplifier gain [units] * |
| 68 | 1024 [units] / |
72 | 1024 [units] / |
| 69 | 3V full range [V] |
73 | 3V full range [V] |
| 70 | 74 | ||
| 71 | Examples: |
75 | Examples: |
| 72 | FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7: |
76 | FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7: |
| 73 | H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s). |
77 | H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s). |
| 74 | FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers: |
78 | FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers: |
| 75 | H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s). |
79 | H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s). |
| 76 | My InvenSense copter has 2mV/deg/s gyros and no amplifiers: |
80 | My InvenSense copter has 2mV/deg/s gyros and no amplifiers: |
| 77 | 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) |
| 78 | (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 |
| 79 | rotation rate!) |
83 | rotation rate!) |
| 80 | 84 | ||
| 81 | All together: gyro = I * H * rotation rate [units / (deg/s)]. |
85 | All together: gyro = I * H * rotation rate [units / (deg/s)]. |
| 82 | */ |
86 | */ |
| 83 | 87 | ||
| 84 | /* |
88 | /* |
| 85 | * A factor that the raw gyro values are multiplied by, |
89 | * A factor that the raw gyro values are multiplied by, |
| 86 | * before being filtered and passed to the attitude module. |
90 | * before being filtered and passed to the attitude module. |
| 87 | * A value of 1 would cause a little bit of loss of precision in the |
91 | * A value of 1 would cause a little bit of loss of precision in the |
| 88 | * filtering (on the other hand the values are so noisy in flight that |
92 | * filtering (on the other hand the values are so noisy in flight that |
| 89 | * it will not really matter - but when testing on the desk it might be |
93 | * it will not really matter - but when testing on the desk it might be |
| 90 | * noticeable). 4 is fine for the default filtering. |
94 | * noticeable). 4 is fine for the default filtering. |
| 91 | * Experiment: Set it to 1. |
95 | * Experiment: Set it to 1. |
| 92 | */ |
96 | */ |
| 93 | #define GYRO_FACTOR_PITCHROLL 4 |
97 | #define GYRO_FACTOR_PITCHROLL 4 |
| 94 | 98 | ||
| 95 | /* |
99 | /* |
| 96 | * How many samples are summed in one ADC loop, for pitch&roll and yaw, |
100 | * How many samples are summed in one ADC loop, for pitch&roll and yaw, |
| 97 | * respectively. This is = the number of occurences of each channel in the |
101 | * respectively. This is = the number of occurences of each channel in the |
| 98 | * channelsForStates array in analog.c. |
102 | * channelsForStates array in analog.c. |
| 99 | */ |
103 | */ |
| 100 | #define GYRO_SUMMATION_FACTOR_PITCHROLL 4 |
104 | #define GYRO_SUMMATION_FACTOR_PITCHROLL 4 |
| 101 | #define GYRO_SUMMATION_FACTOR_YAW 2 |
105 | #define GYRO_SUMMATION_FACTOR_YAW 2 |
| - | 106 | ||
| - | 107 | #define ACC_SUMMATION_FACTOR_PITCHROLL 2 |
|
| - | 108 | #define ACC_SUMMATION_FACTOR_Z 1 |
|
| 102 | 109 | ||
| 103 | /* |
110 | /* |
| 104 | Integration: |
111 | Integration: |
| 105 | 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. |
| 106 | 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 |
| 107 | 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, |
| 108 | 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 |
| 109 | 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 |
| 110 | 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 |
| 111 | constant is there. |
118 | constant is there. |
| 112 | 119 | ||
| 113 | The control loop executes every 2ms, and for each iteration |
120 | The control loop executes every 2ms, and for each iteration |
| 114 | gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL]. |
121 | gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL]. |
| 115 | Assuming a constant rotation rate v and a zero initial gyroIntegral |
122 | Assuming a constant rotation rate v and a zero initial gyroIntegral |
| 116 | (for this explanation), we get: |
123 | (for this explanation), we get: |
| 117 | |
124 | |
| 118 | gyroIntegral = |
125 | gyroIntegral = |
| 119 | N * gyro / HIRES_GYRO_INTEGRATION_FACTOR = |
126 | N * gyro / HIRES_GYRO_INTEGRATION_FACTOR = |
| 120 | N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
127 | N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
| 121 | |
128 | |
| 122 | where N is the number of summations; N = t/2ms. |
129 | where N is the number of summations; N = t/2ms. |
| 123 | 130 | ||
| 124 | For one degree of rotation: t*v = 1: |
131 | For one degree of rotation: t*v = 1: |
| 125 | 132 | ||
| 126 | 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. |
| 127 | 134 | ||
| 128 | 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. |
| 129 | 136 | ||
| 130 | Examples: |
137 | Examples: |
| 131 | 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 |
| 132 | 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 |
| 133 | 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 |
| 134 | */ |
141 | */ |
| 135 | 142 | ||
| 136 | /* |
143 | /* |
| 137 | * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor. |
144 | * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor. |
| 138 | * Will be about 10 or so for InvenSense, and about 33 for ADXRS610. |
145 | * Will be about 10 or so for InvenSense, and about 33 for ADXRS610. |
| 139 | */ |
146 | */ |
| 140 | #define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL) |
147 | #define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL) |
| 141 | #define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW) |
148 | #define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW) |
| 142 | 149 | ||
| 143 | /* |
150 | /* |
| 144 | * Gyro saturation prevention. |
151 | * Gyro saturation prevention. |
| 145 | */ |
152 | */ |
| 146 | // How far from the end of its range a gyro is considered near-saturated. |
153 | // How far from the end of its range a gyro is considered near-saturated. |
| 147 | #define SENSOR_MIN_PITCHROLL 32 |
154 | #define SENSOR_MIN_PITCHROLL 32 |
| 148 | // Other end of the range (calculated) |
155 | // Other end of the range (calculated) |
| 149 | #define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL) |
156 | #define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL) |
| 150 | // Max. boost to add "virtually" to gyro signal at total saturation. |
157 | // Max. boost to add "virtually" to gyro signal at total saturation. |
| 151 | #define EXTRAPOLATION_LIMIT 2500 |
158 | #define EXTRAPOLATION_LIMIT 2500 |
| 152 | // Slope of the boost (calculated) |
159 | // Slope of the boost (calculated) |
| 153 | #define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL) |
160 | #define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL) |
| 154 | 161 | ||
| 155 | /* |
162 | /* |
| 156 | * This value is subtracted from the gyro noise measurement in each iteration, |
163 | * This value is subtracted from the gyro noise measurement in each iteration, |
| 157 | * making it return towards zero. |
164 | * making it return towards zero. |
| 158 | */ |
165 | */ |
| 159 | #define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
166 | #define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
| 160 | 167 | ||
| 161 | #define PITCH 0 |
168 | #define PITCH 0 |
| 162 | #define ROLL 1 |
169 | #define ROLL 1 |
| - | 170 | #define YAW 2 |
|
| - | 171 | #define Z 2 |
|
| 163 | /* |
172 | /* |
| 164 | * The values that this module outputs |
173 | * The values that this module outputs |
| 165 | * These first 2 exported arrays are zero-offset. The "PID" ones are used |
174 | * These first 2 exported arrays are zero-offset. The "PID" ones are used |
| 166 | * in the attitude control as rotation rates. The "ATT" ones are for |
175 | * in the attitude control as rotation rates. The "ATT" ones are for |
| 167 | * integration to angles. For the same axis, the PID and ATT variables |
176 | * integration to angles. For the same axis, the PID and ATT variables |
| 168 | * generally have about the same values. There are just some differences |
177 | * generally have about the same values. There are just some differences |
| 169 | * in filtering, and when a gyro becomes near saturated. |
178 | * in filtering, and when a gyro becomes near saturated. |
| 170 | * Maybe this distinction is not really necessary. |
179 | * Maybe this distinction is not really necessary. |
| 171 | */ |
180 | */ |
| 172 | extern volatile int16_t gyro_PID[2]; |
181 | extern volatile int16_t gyro_PID[2]; |
| 173 | extern volatile int16_t gyro_ATT[2]; |
182 | extern volatile int16_t gyro_ATT[2]; |
| 174 | extern volatile int16_t gyroD[2]; |
183 | extern volatile int16_t gyroD[2]; |
| 175 | 184 | ||
| 176 | extern volatile uint16_t ADCycleCount; |
185 | extern volatile uint16_t ADCycleCount; |
| 177 | extern volatile int16_t UBat; |
186 | extern volatile int16_t UBat; |
| 178 | extern volatile int16_t yawGyro; |
187 | extern volatile int16_t yawGyro; |
| 179 | 188 | ||
| 180 | /* |
189 | /* |
| 181 | * This is not really for external use - but the ENC-03 gyro modules needs it. |
190 | * This is not really for external use - but the ENC-03 gyro modules needs it. |
| 182 | */ |
191 | */ |
| 183 | extern volatile int16_t rawGyroSum[2], rawYawGyroSum; |
192 | extern volatile int16_t rawGyroSum[3]; |
| 184 | 193 | ||
| 185 | /* |
194 | /* |
| 186 | * The acceleration values that this module outputs. They are zero based. |
195 | * The acceleration values that this module outputs. They are zero based. |
| 187 | */ |
196 | */ |
| 188 | extern volatile int16_t acc[2], ZAcc; |
197 | extern volatile int16_t acc[3]; |
| 189 | extern volatile int16_t filteredAcc[2]; |
198 | extern volatile int16_t filteredAcc[2]; |
| 190 | 199 | ||
| 191 | /* |
200 | /* |
| 192 | * Flag: Interrupt handler has done all A/D conversion and processing. |
201 | * Flag: Interrupt handler has done all A/D conversion and processing. |
| 193 | */ |
202 | */ |
| 194 | extern volatile uint8_t analogDataReady; |
203 | extern volatile uint8_t analogDataReady; |
| 195 | 204 | ||
| 196 | // Diagnostics: Gyro noise level because of motor vibrations. The variables |
205 | // Diagnostics: Gyro noise level because of motor vibrations. The variables |
| 197 | // only really reflect the noise level when the copter stands still but with |
206 | // only really reflect the noise level when the copter stands still but with |
| 198 | // its motors running. |
207 | // its motors running. |
| 199 | extern volatile uint16_t gyroNoisePeak[2]; |
208 | extern volatile uint16_t gyroNoisePeak[2]; |
| 200 | extern volatile uint16_t accNoisePeak[2]; |
209 | extern volatile uint16_t accNoisePeak[2]; |
| 201 | 210 | ||
| 202 | // void SearchAirPressureOffset(void); |
211 | // void SearchAirPressureOffset(void); |
| 203 | 212 | ||
| 204 | void analog_init(void); |
213 | void analog_init(void); |
| 205 | 214 | ||
| 206 | // clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
215 | // clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
| 207 | #define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE))) |
216 | #define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE))) |
| 208 | 217 | ||
| 209 | // set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
218 | // set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
| 210 | #define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE)) |
219 | #define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE)) |
| 211 | 220 | ||
| 212 | /* |
221 | /* |
| 213 | * "Warm" calibration: Zero-offset gyros. |
222 | * "Warm" calibration: Zero-offset gyros. |
| 214 | */ |
223 | */ |
| 215 | void analog_calibrate(void); |
224 | void analog_calibrate(void); |
| 216 | 225 | ||
| 217 | /* |
226 | /* |
| 218 | * "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM. |
227 | * "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM. |
| 219 | */ |
228 | */ |
| 220 | void analog_calibrateAcc(void); |
229 | void analog_calibrateAcc(void); |
| 221 | #endif //_ANALOG_H |
230 | #endif //_ANALOG_H |
| 222 | 231 | ||