Rev 1960 | Rev 1964 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1612 | dongfang | 1 | #ifndef _ANALOG_H |
2 | #define _ANALOG_H |
||
3 | #include <inttypes.h> |
||
4 | |||
1646 | - | 5 | //#include "invenSense.h" |
6 | //#include "ENC-03_FC1.3.h" |
||
1775 | - | 7 | //#include "ADXRS610_FC2.0.h" |
1612 | dongfang | 8 | |
9 | /* |
||
1645 | - | 10 | * How much low pass filtering to apply for gyro_PID. |
1612 | dongfang | 11 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
12 | * Temporarily replaced by userparam-configurable variable. |
||
13 | */ |
||
1646 | - | 14 | // #define GYROS_PID_FILTER 1 |
1612 | dongfang | 15 | |
16 | /* |
||
1645 | - | 17 | * How much low pass filtering to apply for gyro_ATT. |
1612 | dongfang | 18 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
19 | * Temporarily replaced by userparam-configurable variable. |
||
20 | */ |
||
1646 | - | 21 | // #define GYROS_ATT_FILTER 1 |
1612 | dongfang | 22 | |
23 | // Temporarily replaced by userparam-configurable variable. |
||
1646 | - | 24 | // #define ACC_FILTER 4 |
1612 | dongfang | 25 | |
26 | /* |
||
1821 | - | 27 | About setting constants for different gyros: |
28 | Main parameters are positive directions and voltage/angular speed gain. |
||
29 | The "Positive direction" is the rotation direction around an axis where |
||
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 |
||
32 | direction is: |
||
33 | - Nose down for pitch |
||
34 | - Left hand side down for roll |
||
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. |
||
52 | |||
53 | Assuming constant ADCValue, in the H&I code: |
||
1612 | dongfang | 54 | |
1821 | - | 55 | gyro = I * ADCValue. |
1612 | dongfang | 56 | |
1821 | - | 57 | where I is 8 for FC1.0 and 16 for all others. |
1612 | dongfang | 58 | |
1821 | - | 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] |
||
1612 | dongfang | 66 | |
1821 | - | 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] |
||
1612 | dongfang | 74 | |
1821 | - | 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) |
||
82 | (only about half as sensitive as V1.3. But it will take about twice the |
||
83 | rotation rate!) |
||
1612 | dongfang | 84 | |
1821 | - | 85 | All together: gyro = I * H * rotation rate [units / (deg/s)]. |
86 | */ |
||
1612 | dongfang | 87 | |
88 | /* |
||
89 | * A factor that the raw gyro values are multiplied by, |
||
1645 | - | 90 | * before being filtered and passed to the attitude module. |
1612 | dongfang | 91 | * A value of 1 would cause a little bit of loss of precision in the |
92 | * filtering (on the other hand the values are so noisy in flight that |
||
93 | * it will not really matter - but when testing on the desk it might be |
||
94 | * noticeable). 4 is fine for the default filtering. |
||
1645 | - | 95 | * Experiment: Set it to 1. |
1612 | dongfang | 96 | */ |
1874 | - | 97 | #define GYRO_FACTOR_PITCHROLL 1 |
1612 | dongfang | 98 | |
99 | /* |
||
100 | * How many samples are summed in one ADC loop, for pitch&roll and yaw, |
||
101 | * respectively. This is = the number of occurences of each channel in the |
||
102 | * channelsForStates array in analog.c. |
||
103 | */ |
||
104 | #define GYRO_SUMMATION_FACTOR_PITCHROLL 4 |
||
105 | #define GYRO_SUMMATION_FACTOR_YAW 2 |
||
106 | |||
1646 | - | 107 | #define ACC_SUMMATION_FACTOR_PITCHROLL 2 |
108 | #define ACC_SUMMATION_FACTOR_Z 1 |
||
109 | |||
1612 | dongfang | 110 | /* |
1821 | - | 111 | 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 |
||
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 |
||
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 |
||
118 | constant is there. |
||
1612 | dongfang | 119 | |
1821 | - | 120 | The control loop executes every 2ms, and for each iteration |
121 | gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL]. |
||
122 | Assuming a constant rotation rate v and a zero initial gyroIntegral |
||
123 | (for this explanation), we get: |
||
1612 | dongfang | 124 | |
1821 | - | 125 | gyroIntegral = |
126 | N * gyro / HIRES_GYRO_INTEGRATION_FACTOR = |
||
127 | N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
||
1612 | dongfang | 128 | |
1821 | - | 129 | where N is the number of summations; N = t/2ms. |
1612 | dongfang | 130 | |
1821 | - | 131 | For one degree of rotation: t*v = 1: |
1612 | dongfang | 132 | |
1821 | - | 133 | gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR. |
1612 | dongfang | 134 | |
1821 | - | 135 | This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor. |
136 | |||
137 | Examples: |
||
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 |
||
140 | My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365 |
||
141 | */ |
||
142 | |||
1612 | dongfang | 143 | /* |
1645 | - | 144 | * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor. |
1612 | dongfang | 145 | * Will be about 10 or so for InvenSense, and about 33 for ADXRS610. |
146 | */ |
||
147 | #define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL) |
||
148 | #define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW) |
||
149 | |||
150 | /* |
||
1645 | - | 151 | * Gyro saturation prevention. |
152 | */ |
||
153 | // How far from the end of its range a gyro is considered near-saturated. |
||
154 | #define SENSOR_MIN_PITCHROLL 32 |
||
155 | // Other end of the range (calculated) |
||
156 | #define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL) |
||
157 | // Max. boost to add "virtually" to gyro signal at total saturation. |
||
158 | #define EXTRAPOLATION_LIMIT 2500 |
||
159 | // Slope of the boost (calculated) |
||
160 | #define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL) |
||
161 | |||
162 | /* |
||
1612 | dongfang | 163 | * This value is subtracted from the gyro noise measurement in each iteration, |
164 | * making it return towards zero. |
||
165 | */ |
||
166 | #define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
||
167 | |||
1645 | - | 168 | #define PITCH 0 |
169 | #define ROLL 1 |
||
1646 | - | 170 | #define YAW 2 |
171 | #define Z 2 |
||
1612 | dongfang | 172 | /* |
173 | * The values that this module outputs |
||
1645 | - | 174 | * These first 2 exported arrays are zero-offset. The "PID" ones are used |
175 | * in the attitude control as rotation rates. The "ATT" ones are for |
||
176 | * integration to angles. For the same axis, the PID and ATT variables |
||
177 | * generally have about the same values. There are just some differences |
||
178 | * in filtering, and when a gyro becomes near saturated. |
||
179 | * Maybe this distinction is not really necessary. |
||
1612 | dongfang | 180 | */ |
1645 | - | 181 | extern volatile int16_t gyro_PID[2]; |
182 | extern volatile int16_t gyro_ATT[2]; |
||
183 | extern volatile int16_t gyroD[2]; |
||
1775 | - | 184 | extern volatile int16_t yawGyro; |
1612 | dongfang | 185 | extern volatile uint16_t ADCycleCount; |
186 | extern volatile int16_t UBat; |
||
187 | |||
1775 | - | 188 | // 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3. |
1869 | - | 189 | #define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3)) |
1775 | - | 190 | |
1960 | - | 191 | typedef struct { |
192 | uint8_t checksum; |
||
193 | uint8_t revisionNumber; |
||
194 | int16_t offsets[3]; |
||
195 | } sensorOffset_t; |
||
196 | |||
197 | extern sensorOffset_t gyroOffset; |
||
198 | extern sensorOffset_t accOffset; |
||
199 | |||
1612 | dongfang | 200 | /* |
201 | * This is not really for external use - but the ENC-03 gyro modules needs it. |
||
202 | */ |
||
1646 | - | 203 | extern volatile int16_t rawGyroSum[3]; |
1612 | dongfang | 204 | |
205 | /* |
||
1645 | - | 206 | * The acceleration values that this module outputs. They are zero based. |
1612 | dongfang | 207 | */ |
1646 | - | 208 | extern volatile int16_t acc[3]; |
1645 | - | 209 | extern volatile int16_t filteredAcc[2]; |
1872 | - | 210 | // extern volatile int32_t stronglyFilteredAcc[3]; |
1612 | dongfang | 211 | |
212 | /* |
||
1775 | - | 213 | * Diagnostics: Gyro noise level because of motor vibrations. The variables |
214 | * only really reflect the noise level when the copter stands still but with |
||
215 | * its motors running. |
||
216 | */ |
||
217 | extern volatile uint16_t gyroNoisePeak[2]; |
||
218 | extern volatile uint16_t accNoisePeak[2]; |
||
219 | |||
220 | /* |
||
221 | * Air pressure. |
||
1961 | - | 222 | * The sensor has a sensitivity of 45 mV/kPa. |
1796 | - | 223 | * An approximate p(h) formula is = p(h[m])[Pa] = p_0 - 11.95 * 10^-3 * h |
1961 | - | 224 | * That is: dV = 45 mV * 11.95 * 10^-3 dh = 0.53775 mV / m. |
225 | * That is, with 1.024 / 3 steps per mV: 0.183552 steps / m |
||
1775 | - | 226 | */ |
1961 | - | 227 | #define AIRPRESSURE_SUMMATION_FACTOR 54 |
1775 | - | 228 | #define AIRPRESSURE_FILTER 8 |
229 | // Minimum A/D value before a range change is performed. |
||
230 | #define MIN_RAWPRESSURE (200 * 2) |
||
231 | // Maximum A/D value before a range change is performed. |
||
232 | #define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE) |
||
233 | |||
1796 | - | 234 | #define MIN_RANGES_EXTRAPOLATION 15 |
235 | #define MAX_RANGES_EXTRAPOLATION 240 |
||
1775 | - | 236 | |
237 | #define PRESSURE_EXTRAPOLATION_COEFF 25L |
||
238 | #define AUTORANGE_WAIT_FACTOR 1 |
||
239 | |||
240 | extern volatile uint16_t simpleAirPressure; |
||
241 | /* |
||
242 | * At saturation, the filteredAirPressure may actually be (simulated) negative. |
||
243 | */ |
||
244 | extern volatile int32_t filteredAirPressure; |
||
245 | |||
246 | /* |
||
1612 | dongfang | 247 | * Flag: Interrupt handler has done all A/D conversion and processing. |
248 | */ |
||
249 | extern volatile uint8_t analogDataReady; |
||
250 | |||
251 | void analog_init(void); |
||
252 | |||
1952 | - | 253 | /* |
254 | * Start the conversion cycle. It will stop automatically. |
||
255 | */ |
||
256 | void startAnalogConversionCycle(void); |
||
1612 | dongfang | 257 | |
1952 | - | 258 | /* |
259 | * Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used. |
||
260 | */ |
||
1955 | - | 261 | void analog_update(void); |
1612 | dongfang | 262 | |
263 | /* |
||
1961 | - | 264 | * Read gyro and acc.meter calibration from EEPROM. |
1612 | dongfang | 265 | */ |
1961 | - | 266 | void analog_setNeutral(void); |
1612 | dongfang | 267 | |
268 | /* |
||
1961 | - | 269 | * Zero-offset gyros and write the calibration data to EEPROM. |
1612 | dongfang | 270 | */ |
1961 | - | 271 | void analog_calibrateGyros(void); |
272 | |||
273 | /* |
||
274 | * Zero-offset accelerometers and write the calibration data to EEPROM. |
||
275 | */ |
||
1612 | dongfang | 276 | void analog_calibrateAcc(void); |
277 | #endif //_ANALOG_H |