Rev 2017 | Rev 2019 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2017 | Rev 2018 | ||
---|---|---|---|
Line 522... | Line 522... | ||
522 | analog_updateAirPressure(); |
522 | analog_updateAirPressure(); |
523 | analog_updateBatteryVoltage(); |
523 | analog_updateBatteryVoltage(); |
524 | } |
524 | } |
Line 525... | Line 525... | ||
525 | 525 | ||
526 | void analog_setNeutral() { |
- | |
527 | if (gyroAmplifierOffset_readFromEEProm()) { |
- | |
528 | printf("gyro amp invalid%s",recal); |
526 | void analog_setNeutral() { |
529 | gyro_loadAmplifierOffsets(1); |
- | |
530 | } else |
- | |
531 | gyro_loadAmplifierOffsets(0); |
527 | gyro_init(); |
532 | 528 | ||
533 | if (gyroOffset_readFromEEProm()) { |
529 | if (gyroOffset_readFromEEProm()) { |
534 | printf("gyro offsets invalid%s",recal); |
530 | printf("gyro offsets invalid%s",recal); |
535 | gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL; |
531 | gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL; |
536 | gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW; |
532 | gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW; |
Line 569... | Line 565... | ||
569 | } |
565 | } |
570 | } |
566 | } |
Line 571... | Line 567... | ||
571 | 567 | ||
572 | for (axis = PITCH; axis <= YAW; axis++) { |
568 | for (axis = PITCH; axis <= YAW; axis++) { |
- | 569 | gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
|
- | 570 | ||
- | 571 | int16_t min = (512-200) * (axis==YAW) ? GYRO_SUMMATION_FACTOR_YAW : GYRO_SUMMATION_FACTOR_PITCHROLL; |
|
- | 572 | int16_t max = (512+200) * (axis==YAW) ? GYRO_SUMMATION_FACTOR_YAW : GYRO_SUMMATION_FACTOR_PITCHROLL; |
|
- | 573 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
|
573 | gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
574 | versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
Line 574... | Line 575... | ||
574 | } |
575 | } |
575 | 576 | ||
576 | gyroOffset_writeToEEProm(); |
577 | gyroOffset_writeToEEProm(); |
Line 586... | Line 587... | ||
586 | */ |
587 | */ |
587 | void analog_calibrateAcc(void) { |
588 | void analog_calibrateAcc(void) { |
588 | #define ACC_OFFSET_CYCLES 32 |
589 | #define ACC_OFFSET_CYCLES 32 |
589 | uint8_t i, axis; |
590 | uint8_t i, axis; |
590 | int32_t offsets[3] = { 0, 0, 0 }; |
591 | int32_t offsets[3] = { 0, 0, 0 }; |
591 | int16_t filteredDelta; |
- | |
Line 592... | Line 592... | ||
592 | 592 | ||
593 | for (i = 0; i < ACC_OFFSET_CYCLES; i++) { |
593 | for (i = 0; i < ACC_OFFSET_CYCLES; i++) { |
594 | delay_ms_with_adc_measurement(10, 1); |
594 | delay_ms_with_adc_measurement(10, 1); |
595 | for (axis = PITCH; axis <= YAW; axis++) { |
595 | for (axis = PITCH; axis <= YAW; axis++) { |
596 | offsets[axis] += rawAccValue(axis); |
596 | offsets[axis] += rawAccValue(axis); |
597 | } |
597 | } |
Line 598... | Line 598... | ||
598 | } |
598 | } |
599 | 599 | ||
- | 600 | for (axis = PITCH; axis <= YAW; axis++) { |
|
- | 601 | accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
|
- | 602 | int16_t min,max; |
|
- | 603 | if (axis==Z) { |
|
- | 604 | if (staticParams.imuReversedFlags & 8) { |
|
- | 605 | // TODO: This assumes a sensitivity of +/- 2g. |
|
- | 606 | min = (256-200) * ACC_SUMMATION_FACTOR_Z; |
|
- | 607 | max = (256+200) * ACC_SUMMATION_FACTOR_Z; |
|
- | 608 | } else { |
|
- | 609 | // TODO: This assumes a sensitivity of +/- 2g. |
|
- | 610 | min = (768-200) * ACC_SUMMATION_FACTOR_Z; |
|
- | 611 | max = (768+200) * ACC_SUMMATION_FACTOR_Z; |
|
- | 612 | } |
|
- | 613 | } else { |
|
- | 614 | min = (512-200) * ACC_SUMMATION_FACTOR_PITCHROLL; |
|
- | 615 | max = (512+200) * ACC_SUMMATION_FACTOR_PITCHROLL; |
|
- | 616 | } |
|
- | 617 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) { |
|
600 | for (axis = PITCH; axis <= YAW; axis++) { |
618 | versionInfo.hardwareErrors[0] |= FC_ERROR0_ACC_X << axis; |
Line 601... | Line 619... | ||
601 | accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
619 | } |
602 | } |
620 | } |
603 | 621 |