Rev 1634 | Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1612 | dongfang | 1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 04.2007 Holger Buss |
||
3 | // + Nur für den privaten Gebrauch |
||
4 | // + www.MikroKopter.com |
||
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
6 | // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
||
7 | // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
||
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
||
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
||
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
||
11 | // + Verkauf von Luftbildaufnahmen, usw. |
||
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
||
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
||
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
||
17 | // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
||
18 | // + eindeutig als Ursprung verlinkt werden |
||
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
20 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
||
21 | // + Benutzung auf eigene Gefahr |
||
22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
||
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
||
25 | // + mit unserer Zustimmung zulässig |
||
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
||
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
||
30 | // + this list of conditions and the following disclaimer. |
||
31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
||
32 | // + from this software without specific prior written permission. |
||
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
||
34 | // + for non-commercial use (directly or indirectly) |
||
35 | // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
||
36 | // + with our written permission |
||
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
||
38 | // + clearly linked as origin |
||
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
||
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
||
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
||
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
||
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
||
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
||
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
||
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
||
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
||
48 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
49 | // + POSSIBILITY OF SUCH DAMAGE. |
||
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
51 | #include <avr/io.h> |
||
52 | #include <avr/interrupt.h> |
||
53 | #include <avr/pgmspace.h> |
||
54 | #include "analog.h" |
||
55 | |||
56 | #include "sensors.h" |
||
57 | |||
58 | // for Delay functions |
||
59 | #include "timer0.h" |
||
60 | |||
61 | // For DebugOut |
||
62 | #include "uart0.h" |
||
63 | |||
64 | // For reading and writing acc. meter offsets. |
||
65 | #include "eeprom.h" |
||
66 | |||
67 | /* |
||
68 | * Arrays could have been used arrays for the 2 * 3 axes, but despite some repetition, |
||
69 | * the code is easier to read without. |
||
70 | * |
||
71 | * For each A/D conversion cycle, each channel (eg. the yaw gyro, or the Z axis |
||
72 | * accelerometer) is sampled a number of times (see array channelsForStates), and |
||
73 | * the results for each channel are summed. Here are those for the gyros and the |
||
74 | * acc. meters. They are not zero-offset. |
||
75 | * They are exported in the analog.h file - but please do not use them! The only |
||
76 | * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating |
||
77 | * the offsets with the DAC. |
||
78 | */ |
||
79 | volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum; |
||
80 | volatile int16_t pitchAxisAcc = 0, rollAxisAcc = 0, ZAxisAcc = 0; |
||
81 | volatile int16_t filteredPitchAxisAcc = 0, filteredRollAxisAcc = 0; |
||
82 | |||
83 | // that float one - "Top" - is missing. |
||
84 | |||
85 | /* |
||
86 | * These 4 exported variables are zero-offset. The "filtered" ones are |
||
87 | * (if configured to with the GYROS_SECONDORDERFILTER define) low pass |
||
88 | * filtered versions of the other 2. |
||
89 | * They are derived from the "raw" values above, by zero-offsetting. |
||
90 | */ |
||
91 | volatile int16_t hiResPitchGyro = 0, hiResRollGyro = 0; |
||
92 | volatile int16_t filteredHiResPitchGyro = 0, filteredHiResRollGyro = 0; |
||
93 | volatile int16_t pitchGyroD = 0, rollGyroD = 0; |
||
94 | volatile int16_t yawGyro = 0; |
||
95 | |||
96 | /* |
||
97 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
||
98 | * standing still. They are used for adjusting the gyro and acc. meter values |
||
99 | * to be zero when the copter stands still. |
||
100 | */ |
||
101 | volatile int16_t pitchOffset, rollOffset, yawOffset; |
||
102 | volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset; |
||
103 | |||
104 | /* |
||
105 | * This allows some experimentation with the gyro filters. |
||
106 | * Should be replaced by #define's later on... |
||
107 | */ |
||
108 | volatile uint8_t GYROS_FIRSTORDERFILTER; |
||
109 | volatile uint8_t GYROS_SECONDORDERFILTER; |
||
110 | volatile uint8_t GYROS_DFILTER; |
||
111 | volatile uint8_t ACC_FILTER; |
||
112 | |||
113 | // Air pressure (no support right now). |
||
114 | // volatile int32_t AirPressure = 32000; |
||
115 | // volatile uint8_t average_pressure = 0; |
||
116 | // volatile int16_t StartAirPressure; |
||
117 | // volatile uint16_t ReadingAirPressure = 1023; |
||
118 | // volatile int16_t HeightD = 0; |
||
119 | |||
120 | /* |
||
121 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
||
122 | * That is divided by 3 below, for a final 10.34 per volt. |
||
123 | * So the initial value of 100 is for 9.7 volts. |
||
124 | */ |
||
125 | volatile int16_t UBat = 100; |
||
126 | |||
127 | /* |
||
128 | * Control and status. |
||
129 | */ |
||
130 | volatile uint16_t ADCycleCount = 0; |
||
131 | volatile uint8_t analogDataReady = 1; |
||
132 | |||
133 | /* |
||
134 | * Experiment: Measuring vibration-induced sensor noise. |
||
135 | */ |
||
136 | volatile uint16_t pitchGyroNoisePeak, rollGyroNoisePeak; |
||
137 | volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak; |
||
138 | |||
139 | // ADC channels |
||
140 | #define AD_GYRO_YAW 0 |
||
141 | #define AD_GYRO_ROLL 1 |
||
142 | #define AD_GYRO_PITCH 2 |
||
143 | #define AD_AIRPRESSURE 3 |
||
144 | #define AD_UBAT 4 |
||
145 | #define AD_ACC_Z 5 |
||
146 | #define AD_ACC_ROLL 6 |
||
147 | #define AD_ACC_PITCH 7 |
||
148 | |||
149 | /* |
||
150 | * Table of AD converter inputs for each state. |
||
151 | * The number of samples summed for each channel is equal to |
||
152 | * the number of times the channel appears in the array. |
||
153 | * The max. number of samples that can be taken in 2 ms is: |
||
154 | * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control |
||
155 | * loop needs a little time between reading AD values and |
||
156 | * re-enabling ADC, the real limit is (how much?) lower. |
||
157 | * The acc. sensor is sampled even if not used - or installed |
||
158 | * at all. The cost is not significant. |
||
159 | */ |
||
160 | |||
161 | const uint8_t channelsForStates[] PROGMEM = { |
||
162 | AD_GYRO_PITCH, |
||
163 | AD_GYRO_ROLL, |
||
164 | AD_GYRO_YAW, |
||
165 | |||
166 | AD_ACC_ROLL, |
||
167 | AD_ACC_PITCH, |
||
168 | |||
169 | AD_GYRO_PITCH, |
||
170 | AD_GYRO_ROLL, |
||
171 | |||
172 | AD_ACC_Z, // at 7, finish Z acc. |
||
173 | |||
174 | AD_GYRO_PITCH, |
||
175 | AD_GYRO_ROLL, |
||
176 | AD_GYRO_YAW, // at 10, finish yaw gyro |
||
177 | |||
178 | AD_ACC_PITCH, // at 11, finish pitch axis acc. |
||
179 | AD_ACC_ROLL, // at 12, finish roll axis acc. |
||
180 | |||
181 | AD_GYRO_PITCH, // at 13, finish pitch gyro |
||
182 | AD_GYRO_ROLL, // at 14, finish roll gyro |
||
183 | |||
184 | AD_UBAT // at 15, measure battery. |
||
185 | }; |
||
186 | |||
187 | // Feature removed. Could be reintroduced later - but should work for all gyro types then. |
||
188 | // uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0; |
||
189 | |||
190 | void analog_init(void) { |
||
191 | uint8_t sreg = SREG; |
||
192 | // disable all interrupts before reconfiguration |
||
193 | cli(); |
||
194 | |||
195 | //ADC0 ... ADC7 is connected to PortA pin 0 ... 7 |
||
196 | DDRA = 0x00; |
||
197 | PORTA = 0x00; |
||
198 | // Digital Input Disable Register 0 |
||
199 | // Disable digital input buffer for analog adc_channel pins |
||
200 | DIDR0 = 0xFF; |
||
201 | // external reference, adjust data to the right |
||
202 | ADMUX &= ~((1 << REFS1)|(1 << REFS0)|(1 << ADLAR)); |
||
203 | // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice) |
||
204 | ADMUX = (ADMUX & 0xE0) | AD_GYRO_PITCH; |
||
205 | //Set ADC Control and Status Register A |
||
206 | //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz |
||
207 | ADCSRA = (0<<ADEN)|(0<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(0<<ADIE); |
||
208 | //Set ADC Control and Status Register B |
||
209 | //Trigger Source to Free Running Mode |
||
210 | ADCSRB &= ~((1 << ADTS2)|(1 << ADTS1)|(1 << ADTS0)); |
||
211 | // Start AD conversion |
||
212 | analog_start(); |
||
213 | // restore global interrupt flags |
||
214 | SREG = sreg; |
||
215 | } |
||
216 | |||
217 | void measureNoise(const int16_t sensor, volatile uint16_t* const noiseMeasurement, const uint8_t damping) { |
||
218 | if (sensor > (int16_t)(*noiseMeasurement)) { |
||
219 | *noiseMeasurement = sensor; |
||
220 | } else if (-sensor > (int16_t)(*noiseMeasurement)) { |
||
221 | *noiseMeasurement = -sensor; |
||
222 | } else if (*noiseMeasurement > damping) { |
||
223 | *noiseMeasurement -= damping; |
||
224 | } else { |
||
225 | *noiseMeasurement = 0; |
||
226 | } |
||
227 | } |
||
228 | |||
229 | /*****************************************************/ |
||
230 | /* Interrupt Service Routine for ADC */ |
||
231 | /*****************************************************/ |
||
232 | // Runs at 312.5 kHz or 3.2 µs |
||
233 | // When all states are processed the interrupt is disabled |
||
234 | // and the update of further AD conversions is stopped. |
||
235 | |||
236 | ISR(ADC_vect) { |
||
237 | static uint8_t ad_channel = AD_GYRO_PITCH, state = 0; |
||
238 | static uint16_t sensorInputs[8] = {0,0,0,0,0,0,0,0}; |
||
239 | |||
240 | uint8_t i; |
||
241 | |||
242 | // for various filters... |
||
243 | static int16_t pitchGyroFilter, rollGyroFilter, tempOffsetGyro; |
||
244 | |||
245 | sensorInputs[ad_channel] += ADC; |
||
246 | |||
247 | /* |
||
248 | * Actually we don't need this "switch". We could do all the sampling into the |
||
249 | * sensorInputs array first, and all the processing after the last sample. |
||
250 | */ |
||
251 | switch(state++) { |
||
252 | case 7: // Z acc |
||
253 | #ifdef ACC_REVERSE_ZAXIS |
||
254 | ZAxisAcc = -ZAxisAccOffset - sensorInputs[AD_ACC_Z]; |
||
255 | #else |
||
256 | ZAxisAcc = sensorInputs[AD_ACC_Z] - ZAxisAccOffset; |
||
257 | #endif |
||
258 | break; |
||
259 | |||
260 | case 10: // yaw gyro |
||
261 | rawYawGyroSum = sensorInputs[AD_GYRO_YAW]; |
||
262 | #ifdef GYRO_REVERSE_YAW |
||
263 | yawGyro = rawYawGyroSum - yawOffset; |
||
264 | #else |
||
265 | yawGyro = yawOffset - rawYawGyroSum; // negative is "default" (FC 1.0-1.3). |
||
266 | #endif |
||
267 | break; |
||
268 | |||
269 | case 11: // pitch axis acc. |
||
270 | #ifdef ACC_REVERSE_PITCHAXIS |
||
271 | pitchAxisAcc = -pitchAxisAccOffset - sensorInputs[AD_ACC_PITCH]; |
||
272 | #else |
||
273 | pitchAxisAcc = sensorInputs[AD_ACC_PITCH] - pitchAxisAccOffset; |
||
274 | #endif |
||
275 | filteredPitchAxisAcc = (filteredPitchAxisAcc * (ACC_FILTER-1) + pitchAxisAcc) / ACC_FILTER; |
||
276 | |||
277 | measureNoise(pitchAxisAcc, &pitchAccNoisePeak, 1); |
||
278 | break; |
||
279 | |||
280 | case 12: // roll axis acc. |
||
281 | #ifdef ACC_REVERSE_ROLLAXIS |
||
282 | rollAxisAcc = sensorInputs[AD_ACC_ROLL] - rollAxisAccOffset; |
||
283 | #else |
||
284 | rollAxisAcc = -rollAxisAccOffset - sensorInputs[AD_ACC_ROLL]; |
||
285 | #endif |
||
286 | filteredRollAxisAcc = (filteredRollAxisAcc * (ACC_FILTER-1) + rollAxisAcc) / ACC_FILTER; |
||
287 | measureNoise(rollAxisAcc, &rollAccNoisePeak, 1); |
||
288 | break; |
||
289 | |||
290 | case 13: // pitch gyro |
||
291 | rawPitchGyroSum = sensorInputs[AD_GYRO_PITCH]; |
||
292 | // Filter already before offsetting. The offsetting resolution improvement obtained by divding by |
||
293 | // GYROS_FIRSTORDERFILTER _after_ offsetting is too small to be worth pursuing. |
||
294 | pitchGyroFilter = (pitchGyroFilter * (GYROS_FIRSTORDERFILTER-1) + rawPitchGyroSum * GYRO_FACTOR_PITCHROLL) / GYROS_FIRSTORDERFILTER; |
||
295 | // Offset to 0. |
||
296 | #ifdef GYROS_REVERSE_PITCH |
||
297 | tempOffsetGyro = pitchOffset - pitchGyroFilter; |
||
298 | #else |
||
299 | tempOffsetGyro = pitchGyroFilter - pitchOffset; |
||
300 | #endif |
||
301 | // Calculate the delta from last shot and filter it. |
||
302 | pitchGyroD = (pitchGyroD * (GYROS_DFILTER-1) + (tempOffsetGyro - hiResPitchGyro)) / GYROS_DFILTER; |
||
303 | // How we can overwrite the last value. This value is used for the D part of the PID controller. |
||
304 | hiResPitchGyro = tempOffsetGyro; |
||
305 | // Filter a little more. This value is used in integration to angles. |
||
306 | filteredHiResPitchGyro = (filteredHiResPitchGyro * (GYROS_SECONDORDERFILTER-1) + hiResPitchGyro) / GYROS_SECONDORDERFILTER; |
||
307 | measureNoise(hiResPitchGyro, &pitchGyroNoisePeak, GYRO_NOISE_MEASUREMENT_DAMPING); |
||
308 | break; |
||
309 | |||
310 | case 14: // Roll gyro. Works the same as pitch. |
||
311 | rawRollGyroSum = sensorInputs[AD_GYRO_ROLL]; |
||
312 | rollGyroFilter = (rollGyroFilter * (GYROS_FIRSTORDERFILTER-1) + rawRollGyroSum * GYRO_FACTOR_PITCHROLL) / GYROS_FIRSTORDERFILTER; |
||
313 | #ifdef GYRO_REVERSE_ROLL |
||
314 | tempOffsetGyro = rollOffset - rollGyroFilter; |
||
315 | #else |
||
316 | tempOffsetGyro = rollGyroFilter - rollOffset; |
||
317 | #endif |
||
318 | rollGyroD = (rollGyroD * (GYROS_DFILTER-1) + (tempOffsetGyro - hiResRollGyro)) / GYROS_DFILTER; |
||
319 | hiResRollGyro = tempOffsetGyro; |
||
320 | filteredHiResRollGyro = (filteredHiResRollGyro * (GYROS_SECONDORDERFILTER-1) + hiResRollGyro) / GYROS_SECONDORDERFILTER; |
||
321 | measureNoise(hiResRollGyro, &rollGyroNoisePeak, GYRO_NOISE_MEASUREMENT_DAMPING); |
||
322 | break; |
||
323 | |||
324 | case 15: |
||
325 | // battery |
||
326 | UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
||
327 | analogDataReady = 1; // mark |
||
328 | ADCycleCount++; |
||
329 | // Stop the sampling. Cycle is over. |
||
330 | state = 0; |
||
331 | for (i=0; i<8; i++) { |
||
332 | sensorInputs[i] = 0; |
||
333 | } |
||
334 | break; |
||
335 | default: {} // do nothing. |
||
336 | } |
||
337 | |||
338 | // set up for next state. |
||
339 | ad_channel = pgm_read_byte(&channelsForStates[state]); |
||
340 | // ad_channel = channelsForStates[state]; |
||
341 | |||
342 | // set adc muxer to next ad_channel |
||
343 | ADMUX = (ADMUX & 0xE0) | ad_channel; |
||
344 | // after full cycle stop further interrupts |
||
345 | if(state) analog_start(); |
||
346 | } |
||
347 | |||
348 | void analog_calibrate(void) { |
||
349 | #define GYRO_OFFSET_CYCLES 32 |
||
350 | uint8_t i; |
||
351 | int32_t _pitchOffset = 0, _rollOffset = 0, _yawOffset = 0; |
||
352 | |||
353 | // Set the filters... to be removed again, once some good settings are found. |
||
354 | GYROS_FIRSTORDERFILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1; |
||
355 | GYROS_SECONDORDERFILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1; |
||
356 | GYROS_DFILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1; |
||
357 | ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1; |
||
358 | |||
359 | pitchOffset = rollOffset = yawOffset = 0; |
||
360 | |||
361 | gyro_calibrate(); |
||
362 | |||
363 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
||
364 | for(i=0; i < GYRO_OFFSET_CYCLES; i++) { |
||
365 | Delay_ms_Mess(10); |
||
366 | _pitchOffset += rawPitchGyroSum * GYRO_FACTOR_PITCHROLL; |
||
367 | _rollOffset += rawRollGyroSum * GYRO_FACTOR_PITCHROLL; |
||
368 | _yawOffset += rawYawGyroSum; |
||
369 | } |
||
370 | |||
371 | pitchOffset = (_pitchOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
||
372 | rollOffset = (_rollOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
||
373 | yawOffset = (_yawOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
||
374 | |||
375 | filteredHiResPitchGyro = filteredHiResRollGyro = 0; |
||
376 | |||
377 | pitchAxisAccOffset = (int16_t)GetParamWord(PID_ACC_NICK); |
||
378 | rollAxisAccOffset = (int16_t)GetParamWord(PID_ACC_ROLL); |
||
379 | ZAxisAccOffset = (int16_t)GetParamWord(PID_ACC_TOP); |
||
380 | |||
381 | // Noise is relative to offset. So, reset noise measurements when |
||
382 | // changing offsets. |
||
383 | pitchGyroNoisePeak = rollGyroNoisePeak = 0; |
||
384 | |||
385 | // Setting offset values has an influence in the analog.c ISR |
||
386 | // Therefore run measurement for 100ms to achive stable readings |
||
387 | Delay_ms_Mess(100); |
||
388 | } |
||
389 | |||
390 | /* |
||
391 | * Find acc. offsets for a neutral reading, and write them to EEPROM. |
||
392 | * Does not (!} update the local variables. This must be done with a |
||
393 | * call to analog_calibrate() - this always (?) is done by the caller |
||
394 | * anyway. There would be nothing wrong with updating the variables |
||
395 | * directly from here, though. |
||
396 | */ |
||
397 | void analog_calibrateAcc(void) { |
||
398 | #define ACC_OFFSET_CYCLES 10 |
||
399 | uint8_t i; |
||
400 | int32_t _pitchAxisOffset = 0, _rollAxisOffset = 0, _ZAxisOffset = 0; |
||
401 | |||
402 | pitchAxisAccOffset = rollAxisAccOffset = ZAxisAccOffset = 0; |
||
403 | |||
404 | for(i=0; i < ACC_OFFSET_CYCLES; i++) { |
||
405 | Delay_ms_Mess(10); |
||
406 | _pitchAxisOffset += pitchAxisAcc; |
||
407 | _rollAxisOffset += rollAxisAcc; |
||
408 | _ZAxisOffset += ZAxisAcc; |
||
409 | } |
||
410 | |||
411 | // Save ACC neutral settings to eeprom |
||
412 | SetParamWord(PID_ACC_NICK, (uint16_t)((_pitchAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES)); |
||
413 | SetParamWord(PID_ACC_ROLL, (uint16_t)((_rollAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES)); |
||
414 | SetParamWord(PID_ACC_TOP, (uint16_t)((_ZAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES)); |
||
415 | |||
416 | // Noise is relative to offset. So, reset noise measurements when |
||
417 | // changing offsets. |
||
418 | pitchAccNoisePeak = rollAccNoisePeak = 0; |
||
419 | } |