Rev 2119 | Rev 2124 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2119 | Rev 2122 | ||
|---|---|---|---|
| Line 161... | Line 161... | ||
| 161 | const uint8_t channelsForStates[] PROGMEM = { |
161 | const uint8_t channelsForStates[] PROGMEM = { |
| 162 | AD_GYRO_PITCH, |
162 | AD_GYRO_PITCH, |
| 163 | AD_GYRO_ROLL, |
163 | AD_GYRO_ROLL, |
| 164 | AD_GYRO_YAW, |
164 | AD_GYRO_YAW, |
| Line 165... | Line -... | ||
| 165 | - | ||
| 166 | AD_AIRPRESSURE, |
- | |
| 167 | 165 | ||
| 168 | AD_GYRO_PITCH, |
166 | AD_GYRO_PITCH, |
| 169 | AD_GYRO_ROLL, |
167 | AD_GYRO_ROLL, |
| Line -... | Line 168... | ||
| - | 168 | AD_GYRO_YAW, |
|
| - | 169 | ||
| 170 | AD_GYRO_YAW, |
170 | AD_AIRPRESSURE, |
| Line 171... | Line 171... | ||
| 171 | 171 | ||
| 172 | AD_UBAT, |
172 | AD_UBAT, |
| 173 | 173 | ||
| Line 174... | Line -... | ||
| 174 | AD_GYRO_PITCH, |
- | |
| 175 | AD_GYRO_ROLL, |
- | |
| 176 | AD_GYRO_YAW, |
174 | AD_GYRO_PITCH, |
| 177 | 175 | AD_GYRO_ROLL, |
|
| 178 | AD_AIRPRESSURE, |
176 | AD_GYRO_YAW, |
| - | 177 | ||
| - | 178 | AD_GYRO_PITCH, |
|
| 179 | 179 | AD_GYRO_ROLL, |
|
| Line 180... | Line 180... | ||
| 180 | AD_GYRO_PITCH, |
180 | AD_GYRO_YAW, |
| 181 | AD_GYRO_ROLL, |
181 | |
| Line 403... | Line 403... | ||
| 403 | 403 | ||
| 404 | // gyroActivity = 0; |
404 | // gyroActivity = 0; |
| Line 405... | Line 405... | ||
| 405 | } |
405 | } |
| 406 | 406 | ||
| 407 | void analog_calibrate(void) { |
407 | void analog_calibrate(void) { |
| 408 | #define OFFSET_CYCLES 250 |
408 | #define OFFSET_CYCLES 120 |
| 409 | uint8_t i, axis; |
409 | uint8_t i, axis; |
| Line 410... | Line 410... | ||
| 410 | int32_t offsets[4] = { 0, 0, 0, 0 }; |
410 | int32_t offsets[4] = { 0, 0, 0, 0 }; |
| 411 | gyro_calibrate(); |
411 | gyro_calibrate(); |
| 412 | 412 | ||
| 413 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
413 | // determine gyro bias by averaging (requires that the aircraft does not rotate around any axis!) |
| 414 | for (i = 0; i < OFFSET_CYCLES; i++) { |
414 | for (i = 0; i < OFFSET_CYCLES; i++) { |
| 415 | delay_ms_with_adc_measurement(2, 1); |
415 | delay_ms_with_adc_measurement(2, 1); |
| 416 | for (axis = PITCH; axis <= YAW; axis++) { |
416 | for (axis = PITCH; axis <= YAW; axis++) { |
| 417 | offsets[axis] += rawGyroValue(axis); |
417 | offsets[axis] += rawGyroValue(axis); |
| Line 418... | Line 418... | ||
| 418 | } |
418 | } |
| 419 | offsets[3] += sensorInputs[AD_AIRPRESSURE]; |
419 | offsets[3] += sensorInputs[AD_AIRPRESSURE]; |
| 420 | } |
420 | } |
| 421 | 421 | ||
| 422 | for (axis = PITCH; axis <= YAW; axis++) { |
422 | for (axis = PITCH; axis <= YAW; axis++) { |
| 423 | gyroOffset.offsets[axis] = (offsets[axis] /*+ OFFSET_CYCLES/2 */) / OFFSET_CYCLES; |
423 | gyroOffset.offsets[axis] = (offsets[axis] + OFFSET_CYCLES/2) / OFFSET_CYCLES; |
| 424 | int16_t min = (512-200) * GYRO_OVERSAMPLING; |
424 | int16_t min = (512-200) * GYRO_OVERSAMPLING; |
| Line 425... | Line 425... | ||
| 425 | int16_t max = (512+200) * GYRO_OVERSAMPLING; |
425 | int16_t max = (512+200) * GYRO_OVERSAMPLING; |
| 426 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
426 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
| 427 | versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
427 | versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
| 428 | } |
428 | } |
| 429 | 429 | ||