Rev 2116 | Rev 2122 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2116 | Rev 2119 | ||
|---|---|---|---|
| Line 5... | Line 5... | ||
| 5 | 5 | ||
| 6 | #include "analog.h" |
6 | #include "analog.h" |
| 7 | #include "attitude.h" |
7 | #include "attitude.h" |
| 8 | #include "printf_P.h" |
8 | #include "printf_P.h" |
| - | 9 | #include "isqrt.h" |
|
| Line 9... | Line 10... | ||
| 9 | #include "isqrt.h" |
10 | #include "sensors.h" |
| 10 | 11 | ||
| Line 11... | Line 12... | ||
| 11 | // for Delay functions |
12 | // for Delay functions |
| Line 402... | Line 403... | ||
| 402 | 403 | ||
| 403 | // gyroActivity = 0; |
404 | // gyroActivity = 0; |
| Line 404... | Line 405... | ||
| 404 | } |
405 | } |
| 405 | 406 | ||
| 406 | void analog_calibrate(void) { |
407 | void analog_calibrate(void) { |
| 407 | #define OFFSET_CYCLES 64 |
408 | #define OFFSET_CYCLES 250 |
| 408 | uint8_t i, axis; |
409 | uint8_t i, axis; |
| Line 409... | Line 410... | ||
| 409 | int32_t offsets[4] = { 0, 0, 0, 0}; |
410 | int32_t offsets[4] = { 0, 0, 0, 0 }; |
| 410 | gyro_calibrate(); |
411 | gyro_calibrate(); |
| 411 | 412 | ||
| 412 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
413 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
| 413 | for (i = 0; i < OFFSET_CYCLES; i++) { |
414 | for (i = 0; i < OFFSET_CYCLES; i++) { |
| 414 | delay_ms_with_adc_measurement(10, 1); |
415 | delay_ms_with_adc_measurement(2, 1); |
| 415 | for (axis = PITCH; axis <= YAW; axis++) { |
416 | for (axis = PITCH; axis <= YAW; axis++) { |
| 416 | offsets[axis] += rawGyroValue(axis); |
417 | offsets[axis] += rawGyroValue(axis); |
| Line 417... | Line 418... | ||
| 417 | } |
418 | } |
| 418 | offsets[3] += sensorInputs[AD_AIRPRESSURE]; |
419 | offsets[3] += sensorInputs[AD_AIRPRESSURE]; |
| 419 | } |
420 | } |
| 420 | 421 | ||
| 421 | for (axis = PITCH; axis <= YAW; axis++) { |
422 | for (axis = PITCH; axis <= YAW; axis++) { |
| 422 | gyroOffset.offsets[axis] = (offsets[axis] + OFFSET_CYCLES / 2) / OFFSET_CYCLES; |
423 | gyroOffset.offsets[axis] = (offsets[axis] /*+ OFFSET_CYCLES/2 */) / OFFSET_CYCLES; |
| 423 | int16_t min = (512-200) * GYRO_OVERSAMPLING; |
424 | int16_t min = (512-200) * GYRO_OVERSAMPLING; |
| Line 424... | Line 425... | ||
| 424 | int16_t max = (512+200) * GYRO_OVERSAMPLING; |
425 | int16_t max = (512+200) * GYRO_OVERSAMPLING; |
| 425 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
426 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
| 426 | versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
427 | versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
| 427 | } |
428 | } |
| 428 | 429 | ||