Subversion Repositories FlightCtrl

Rev

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