Rev 1991 | Rev 2017 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1991 | Rev 2015 | ||
---|---|---|---|
Line 82... | Line 82... | ||
82 | * They are exported in the analog.h file - but please do not use them! The only |
82 | * They are exported in the analog.h file - but please do not use them! The only |
83 | * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating |
83 | * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating |
84 | * the offsets with the DAC. |
84 | * the offsets with the DAC. |
85 | */ |
85 | */ |
86 | volatile uint16_t sensorInputs[8]; |
86 | volatile uint16_t sensorInputs[8]; |
87 | volatile int16_t rawGyroSum[3]; |
- | |
88 | volatile int16_t acc[3]; |
87 | int16_t acc[3]; |
89 | volatile int16_t filteredAcc[3] = { 0,0,0 }; |
88 | int16_t filteredAcc[3] = { 0,0,0 }; |
90 | // volatile int32_t stronglyFilteredAcc[3] = { 0,0,0 }; |
- | |
Line 91... | Line 89... | ||
91 | 89 | ||
92 | /* |
90 | /* |
93 | * These 4 exported variables are zero-offset. The "PID" ones are used |
91 | * These 4 exported variables are zero-offset. The "PID" ones are used |
94 | * in the attitude control as rotation rates. The "ATT" ones are for |
92 | * in the attitude control as rotation rates. The "ATT" ones are for |
95 | * integration to angles. |
93 | * integration to angles. |
96 | */ |
94 | */ |
97 | volatile int16_t gyro_PID[2]; |
95 | int16_t gyro_PID[2]; |
98 | volatile int16_t gyro_ATT[2]; |
96 | int16_t gyro_ATT[2]; |
99 | volatile int16_t gyroD[2]; |
97 | int16_t gyroD[2]; |
Line 100... | Line 98... | ||
100 | volatile int16_t yawGyro; |
98 | int16_t yawGyro; |
101 | 99 | ||
102 | /* |
100 | /* |
103 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
101 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
Line 108... | Line 106... | ||
108 | sensorOffset_t gyroOffset; |
106 | sensorOffset_t gyroOffset; |
109 | sensorOffset_t accOffset; |
107 | sensorOffset_t accOffset; |
110 | sensorOffset_t gyroAmplifierOffset; |
108 | sensorOffset_t gyroAmplifierOffset; |
Line 111... | Line 109... | ||
111 | 109 | ||
- | 110 | /* |
|
112 | /* |
111 | * In the MK coordinate system, nose-down is positive and left-roll is positive. |
- | 112 | * If a sensor is used in an orientation where one but not both of the axes has |
|
- | 113 | * an opposite sign, PR_ORIENTATION_REVERSED is set to 1 (true). |
|
- | 114 | * Transform: |
|
113 | * This allows some experimentation with the gyro filters. |
115 | * pitch <- pp*pitch + pr*roll |
- | 116 | * roll <- rp*pitch + rr*roll |
|
- | 117 | * Not reversed, GYRO_QUADRANT: |
|
- | 118 | * 0: pp=1, pr=0, rp=0, rr=1 // 0 degrees |
|
- | 119 | * 1: pp=1, pr=-1,rp=1, rr=1 // +45 degrees |
|
- | 120 | * 2: pp=0, pr=-1,rp=1, rr=0 // +90 degrees |
|
- | 121 | * 3: pp=-1,pr=-1,rp=1, rr=1 // +135 degrees |
|
- | 122 | * 4: pp=-1,pr=0, rp=0, rr=-1 // +180 degrees |
|
- | 123 | * 5: pp=-1,pr=1, rp=-1,rr=-1 // +225 degrees |
|
- | 124 | * 6: pp=0, pr=1, rp=-1,rr=0 // +270 degrees |
|
- | 125 | * 7: pp=1, pr=1, rp=-1,rr=1 // +315 degrees |
|
- | 126 | * Reversed, GYRO_QUADRANT: |
|
- | 127 | * 0: pp=-1,pr=0, rp=0, rr=1 // 0 degrees with pitch reversed |
|
- | 128 | * 1: pp=-1,pr=-1,rp=-1,rr=1 // +45 degrees with pitch reversed |
|
- | 129 | * 2: pp=0, pr=-1,rp=-1,rr=0 // +90 degrees with pitch reversed |
|
- | 130 | * 3: pp=1, pr=-1,rp=-1,rr=1 // +135 degrees with pitch reversed |
|
- | 131 | * 4: pp=1, pr=0, rp=0, rr=-1 // +180 degrees with pitch reversed |
|
- | 132 | * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed |
|
- | 133 | * 6: pp=0, pr=1, rp=1, rr=0 // +270 degrees with pitch reversed |
|
114 | * Should be replaced by #define's later on... |
134 | * 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed |
- | 135 | */ |
|
- | 136 | ||
- | 137 | void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) { |
|
- | 138 | static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1}; |
|
- | 139 | // Pitch to Pitch part |
|
- | 140 | int8_t xx = (reverse & 1) ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant]; |
|
- | 141 | // Roll to Pitch part |
|
- | 142 | int8_t xy = rotationTab[(quadrant+2)%8]; |
|
- | 143 | // Pitch to Roll part |
|
- | 144 | int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8]; |
|
- | 145 | // Roll to Roll part |
|
- | 146 | int8_t yy = rotationTab[quadrant]; |
|
- | 147 | ||
- | 148 | int16_t xIn = result[0]; |
|
- | 149 | result[0] = xx*result[0] + xy*result[1]; |
|
- | 150 | result[1] = yx*xIn + yy*result[1]; |
|
- | 151 | ||
- | 152 | if (quadrant & 1) { |
|
- | 153 | // A rotation was used above, where the factors were too large by sqrt(2). |
|
- | 154 | // So, we multiply by 2^n/sqt(2) and right shift n bits, as to divide by sqrt(2). |
|
- | 155 | // A suitable value for n: Sample is 11 bits. After transformation it is the sum |
|
- | 156 | // of 2 11 bit numbers, so 12 bits. We have 4 bits left... |
|
- | 157 | result[0] = (result[0]*11) >> 4; |
|
- | 158 | result[1] = (result[1]*11) >> 4; |
|
- | 159 | } |
|
Line 115... | Line 160... | ||
115 | */ |
160 | } |
116 | 161 | ||
117 | /* |
162 | /* |
118 | * Air pressure |
163 | * Air pressure |
Line 119... | Line 164... | ||
119 | */ |
164 | */ |
120 | volatile uint8_t rangewidth = 105; |
165 | volatile uint8_t rangewidth = 105; |
Line 121... | Line 166... | ||
121 | 166 | ||
122 | // Direct from sensor, irrespective of range. |
167 | // Direct from sensor, irrespective of range. |
Line 123... | Line 168... | ||
123 | // volatile uint16_t rawAirPressure; |
168 | // volatile uint16_t rawAirPressure; |
124 | 169 | ||
Line 125... | Line 170... | ||
125 | // Value of 2 samples, with range. |
170 | // Value of 2 samples, with range. |
126 | volatile uint16_t simpleAirPressure; |
171 | uint16_t simpleAirPressure; |
Line 127... | Line 172... | ||
127 | 172 | ||
128 | // Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered. |
173 | // Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered. |
Line 129... | Line 174... | ||
129 | volatile int32_t filteredAirPressure; |
174 | int32_t filteredAirPressure; |
130 | 175 | ||
131 | // Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples. |
176 | // Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples. |
132 | volatile int32_t airPressureSum; |
177 | int32_t airPressureSum; |
133 | 178 | ||
134 | // The number of samples summed into airPressureSum so far. |
179 | // The number of samples summed into airPressureSum so far. |
Line 135... | Line 180... | ||
135 | volatile uint8_t pressureMeasurementCount; |
180 | uint8_t pressureMeasurementCount; |
136 | 181 | ||
137 | /* |
182 | /* |
138 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
183 | * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
139 | * That is divided by 3 below, for a final 10.34 per volt. |
184 | * That is divided by 3 below, for a final 10.34 per volt. |
Line 140... | Line 185... | ||
140 | * So the initial value of 100 is for 9.7 volts. |
185 | * So the initial value of 100 is for 9.7 volts. |
141 | */ |
186 | */ |
142 | volatile int16_t UBat = 100; |
187 | int16_t UBat = 100; |
143 | 188 | ||
144 | /* |
189 | /* |
Line 145... | Line 190... | ||
145 | * Control and status. |
190 | * Control and status. |
146 | */ |
191 | */ |
Line 147... | Line 192... | ||
147 | volatile uint16_t ADCycleCount = 0; |
192 | volatile uint16_t ADCycleCount = 0; |
Line 223... | Line 268... | ||
223 | 268 | ||
224 | // restore global interrupt flags |
269 | // restore global interrupt flags |
225 | SREG = sreg; |
270 | SREG = sreg; |
Line -... | Line 271... | ||
- | 271 | } |
|
- | 272 | ||
- | 273 | uint16_t rawGyroValue(uint8_t axis) { |
|
- | 274 | return sensorInputs[AD_GYRO_PITCH-axis]; |
|
- | 275 | } |
|
- | 276 | ||
- | 277 | uint16_t rawAccValue(uint8_t axis) { |
|
- | 278 | return sensorInputs[AD_ACC_PITCH-axis]; |
|
226 | } |
279 | } |
227 | 280 | ||
228 | void measureNoise(const int16_t sensor, |
281 | void measureNoise(const int16_t sensor, |
229 | volatile uint16_t* const noiseMeasurement, const uint8_t damping) { |
282 | volatile uint16_t* const noiseMeasurement, const uint8_t damping) { |
230 | if (sensor > (int16_t) (*noiseMeasurement)) { |
283 | if (sensor > (int16_t) (*noiseMeasurement)) { |
Line 280... | Line 333... | ||
280 | } |
333 | } |
281 | } |
334 | } |
Line 282... | Line 335... | ||
282 | 335 | ||
283 | void analog_updateGyros(void) { |
336 | void analog_updateGyros(void) { |
284 | // for various filters... |
337 | // for various filters... |
Line 285... | Line 338... | ||
285 | int16_t tempOffsetGyro, tempGyro; |
338 | int16_t tempOffsetGyro[2], tempGyro; |
286 | 339 | ||
287 | debugOut.digital[0] &= ~DEBUG_SENSORLIMIT; |
340 | debugOut.digital[0] &= ~DEBUG_SENSORLIMIT; |
288 | for (uint8_t axis=0; axis<2; axis++) { |
341 | for (uint8_t axis=0; axis<2; axis++) { |
289 | tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis]; |
342 | tempGyro = rawGyroValue(axis); |
290 | 343 | ||
291 | /* |
344 | /* |
292 | * Process the gyro data for the PID controller. |
345 | * Process the gyro data for the PID controller. |
293 | */ |
346 | */ |
Line 294... | Line 347... | ||
294 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
347 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
295 | // gyro with a wider range, and helps counter saturation at full control. |
348 | // gyro with a wider range, and helps counter saturation at full control. |
296 | 349 | ||
297 | if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) { |
350 | if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) { |
298 | if (tempGyro < SENSOR_MIN_PITCHROLL) { |
351 | if (tempGyro < SENSOR_MIN_PITCHROLL) { |
299 | debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
352 | debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
300 | tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
353 | tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
301 | } else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
- | |
302 | debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
354 | } else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
303 | tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE |
355 | debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
304 | + SENSOR_MAX_PITCHROLL; |
356 | tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL; |
305 | } |
357 | } |
306 | } |
- | |
307 | 358 | } |
|
- | 359 | ||
- | 360 | // 2) Apply sign and offset, scale before filtering. |
|
308 | // 2) Apply sign and offset, scale before filtering. |
361 | tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
309 | if (GYRO_REVERSED[axis]) { |
362 | } |
310 | tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
- | |
311 | } else { |
363 | |
- | 364 | // 2.1: Transform axes. |
|
312 | tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
365 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1); |
313 | } |
366 | |
314 | 367 | for (uint8_t axis=0; axis<2; axis++) { |
|
315 | // 3) Scale and filter. |
368 | // 3) Filter. |
316 | tempOffsetGyro = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroPIDFilterConstant; |
369 | tempOffsetGyro[axis] = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / staticParams.gyroPIDFilterConstant; |
317 | 370 | ||
318 | // 4) Measure noise. |
371 | // 4) Measure noise. |
319 | measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
372 | measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
320 | 373 | ||
321 | // 5) Differential measurement. |
374 | // 5) Differential measurement. |
322 | gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro - gyro_PID[axis])) / staticParams.gyroDFilterConstant; |
375 | gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant; |
323 | - | ||
324 | // 6) Done. |
- | |
325 | gyro_PID[axis] = tempOffsetGyro; |
- | |
326 | - | ||
327 | /* |
- | |
328 | * Now process the data for attitude angles. |
376 | |
329 | */ |
377 | // 6) Done. |
330 | tempGyro = rawGyroSum[axis]; |
- | |
331 | - | ||
332 | // 1) Apply sign and offset, scale before filtering. |
- | |
333 | if (GYRO_REVERSED[axis]) { |
378 | gyro_PID[axis] = tempOffsetGyro[axis]; |
334 | tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
- | |
335 | } else { |
- | |
336 | tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
- | |
337 | } |
- | |
338 | 379 | ||
Line -... | Line 380... | ||
- | 380 | // Prepare tempOffsetGyro for next calculation below... |
|
- | 381 | tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
|
- | 382 | } |
|
- | 383 | ||
- | 384 | /* |
|
- | 385 | * Now process the data for attitude angles. |
|
- | 386 | */ |
|
- | 387 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1); |
|
- | 388 | ||
339 | // 2) Filter. |
389 | // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else. |
340 | gyro_ATT[axis] = (gyro_ATT[axis] * (staticParams.gyroATTFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroATTFilterConstant; |
- | |
341 | } |
390 | // gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter; |
342 | 391 | // gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter; |
|
343 | // Yaw gyro. |
392 | |
344 | rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
393 | // Yaw gyro. |
345 | if (GYRO_REVERSED[YAW]) |
394 | if (staticParams.imuReversedFlags & 2) |
Line 346... | Line 395... | ||
346 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
395 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
347 | else |
- | |
348 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
- | |
349 | } |
- | |
350 | - | ||
351 | void analog_updateAccelerometers(void) { |
- | |
352 | // Z acc. |
- | |
353 | if (ACC_REVERSED[Z]) |
396 | else |
354 | acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z]; |
397 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
355 | else |
- | |
356 | acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z]; |
- | |
357 | - | ||
358 | // Pitch and roll axis accelerations. |
398 | } |
359 | for (uint8_t axis=0; axis<2; axis++) { |
399 | |
360 | if (ACC_REVERSED[axis]) |
400 | void analog_updateAccelerometers(void) { |
- | 401 | // Pitch and roll axis accelerations. |
|
- | 402 | for (uint8_t axis=0; axis<2; axis++) { |
|
361 | acc[axis] = accOffset.offsets[axis] - sensorInputs[AD_ACC_PITCH-axis]; |
403 | acc[axis] = rawAccValue(axis) - accOffset.offsets[axis]; |
362 | else |
404 | } |
363 | acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset.offsets[axis]; |
405 | |
364 | } |
406 | rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & 4); |
- | 407 | ||
- | 408 | for(uint8_t axis=0; axis<3; axis++) { |
|
- | 409 | filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant; |
|
- | 410 | measureNoise(acc[axis], &accNoisePeak[axis], 1); |
|
- | 411 | } |
|
- | 412 | ||
365 | 413 | // Z acc. |
|
Line 366... | Line 414... | ||
366 | for (uint8_t axis=0; axis<3; axis++) { |
414 | if (staticParams.imuReversedFlags & 8) |
367 | filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant; |
415 | acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z]; |
368 | measureNoise(acc[axis], &accNoisePeak[axis], 1); |
416 | else |
Line 486... | Line 534... | ||
486 | gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
534 | gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
487 | accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
535 | accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
Line 488... | Line 536... | ||
488 | 536 | ||
489 | // Setting offset values has an influence in the analog.c ISR |
537 | // Setting offset values has an influence in the analog.c ISR |
490 | // Therefore run measurement for 100ms to achive stable readings |
538 | // Therefore run measurement for 100ms to achive stable readings |
Line 491... | Line 539... | ||
491 | delay_ms_with_adc_measurement(100); |
539 | delay_ms_with_adc_measurement(100, 0); |
492 | 540 | ||
493 | // Rough estimate. Hmm no nothing happens at calibration anyway. |
541 | // Rough estimate. Hmm no nothing happens at calibration anyway. |
494 | // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
542 | // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
Line 501... | Line 549... | ||
501 | int32_t offsets[3] = { 0, 0, 0 }; |
549 | int32_t offsets[3] = { 0, 0, 0 }; |
502 | gyro_calibrate(); |
550 | gyro_calibrate(); |
Line 503... | Line 551... | ||
503 | 551 | ||
504 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
552 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
505 | for (i = 0; i < GYRO_OFFSET_CYCLES; i++) { |
553 | for (i = 0; i < GYRO_OFFSET_CYCLES; i++) { |
506 | delay_ms_with_adc_measurement(20); |
554 | delay_ms_with_adc_measurement(10, 1); |
507 | for (axis = PITCH; axis <= YAW; axis++) { |
555 | for (axis = PITCH; axis <= YAW; axis++) { |
508 | offsets[axis] += rawGyroSum[axis]; |
556 | offsets[axis] += rawGyroValue(axis); |
509 | } |
557 | } |
Line 510... | Line 558... | ||
510 | } |
558 | } |
511 | 559 | ||
512 | for (axis = PITCH; axis <= YAW; axis++) { |
560 | for (axis = PITCH; axis <= YAW; axis++) { |
Line 513... | Line 561... | ||
513 | gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
561 | gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
- | 562 | } |
|
514 | } |
563 | |
Line 515... | Line 564... | ||
515 | 564 | gyroOffset_writeToEEProm(); |
|
516 | gyroOffset_writeToEEProm(); |
565 | startAnalogConversionCycle(); |
517 | } |
566 | } |
518 | 567 | ||
519 | /* |
568 | /* |
520 | * Find acc. offsets for a neutral reading, and write them to EEPROM. |
569 | * Find acc. offsets for a neutral reading, and write them to EEPROM. |
521 | * Does not (!} update the local variables. This must be done with a |
570 | * Does not (!} update the local variables. This must be done with a |
522 | * call to analog_calibrate() - this always (?) is done by the caller |
571 | * call to analog_calibrate() - this always (?) is done by the caller |
523 | * anyway. There would be nothing wrong with updating the variables |
572 | * anyway. There would be nothing wrong with updating the variables |
524 | * directly from here, though. |
573 | * directly from here, though. |
525 | */ |
574 | */ |
526 | void analog_calibrateAcc(void) { |
575 | void analog_calibrateAcc(void) { |
527 | #define ACC_OFFSET_CYCLES 10 |
576 | #define ACC_OFFSET_CYCLES 32 |
528 | uint8_t i, axis; |
577 | uint8_t i, axis; |
529 | int32_t deltaOffset[3] = { 0, 0, 0 }; |
578 | int32_t offsets[3] = { 0, 0, 0 }; |
530 | int16_t filteredDelta; |
579 | int16_t filteredDelta; |
531 | 580 | ||
532 | for (i = 0; i < ACC_OFFSET_CYCLES; i++) { |
581 | for (i = 0; i < ACC_OFFSET_CYCLES; i++) { |
533 | delay_ms_with_adc_measurement(10); |
582 | delay_ms_with_adc_measurement(10, 1); |
534 | for (axis = PITCH; axis <= YAW; axis++) { |
583 | for (axis = PITCH; axis <= YAW; axis++) { |
535 | deltaOffset[axis] += acc[axis]; |
584 | offsets[axis] += rawAccValue(axis); |
536 | } |
585 | } |
537 | } |
- | |
538 | - | ||
539 | for (axis = PITCH; axis <= YAW; axis++) { |
586 | } |
Line 540... | Line 587... | ||
540 | filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) |
587 | |
- | 588 | for (axis = PITCH; axis <= YAW; axis++) { |
|
541 | / ACC_OFFSET_CYCLES; |
589 | accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |