Subversion Repositories FlightCtrl

Rev

Rev 2073 | Rev 2088 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2073 Rev 2086
Line 40... Line 40...
40 
 * integration to angles.
 40 
 * integration to angles.
41 
 */
 41 
 */
42 
int16_t gyro_PID[2];
 42 
int16_t gyro_PID[2];
43 
int16_t gyro_ATT[2];
 43 
int16_t gyro_ATT[2];
44 
int16_t gyroD[2];
 44 
int16_t gyroD[2];
- 
 
 45 
int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH];
- 
 
 46 
uint8_t gyroDWindowIdx = 0;
45 
int16_t yawGyro;
 47 
int16_t yawGyro;
46 
int16_t magneticHeading;
 48 
int16_t magneticHeading;
Line 47... Line 49...
47 
 
 49 
 
- 
 
 50 
int32_t groundPressure;
Line 48... Line 51...
48 
int32_t groundPressure;
 51 
int16_t dHeight;
49 
 
 52 
 
50 
/*
 53 
/*
51 
 * Offset values. These are the raw gyro and acc. meter sums when the copter is
 54 
 * Offset values. These are the raw gyro and acc. meter sums when the copter is
Line 358... Line 361...
358 
 
 361 
 
359 
    // 4) Measure noise.
 362 
    // 4) Measure noise.
Line 360... Line 363...
360 
    measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
 363 
    measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
361 
 
 364 
 
- 
 
 365 
    // 5) Differential measurement.
- 
 
 366 
    // gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant;
- 
 
 367 
    int16_t diff = tempOffsetGyro[axis] - gyro_PID[axis];
- 
 
 368 
    gyroD[axis] -= gyroDWindow[axis][gyroDWindowIdx];
Line 362... Line 369...
362 
    // 5) Differential measurement.
 369 
    gyroD[axis] += diff;
363 
    gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant;
 370 
    gyroDWindow[axis][gyroDWindowIdx] = diff;
Line 364... Line 371...
364 
 
 371 
 
365 
    // 6) Done.
 372 
    // 6) Done.
366 
    gyro_PID[axis] = tempOffsetGyro[axis];
 373 
    gyro_PID[axis] = tempOffsetGyro[axis];
367 
 
 374 
 
- 
 
 375 
    // Prepare tempOffsetGyro for next calculation below...
- 
 
 376 
    tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
- 
 
 377 
  }
- 
 
 378 
 
368 
    // Prepare tempOffsetGyro for next calculation below...
 379 
  if (gyroDWindowIdx >= GYRO_D_WINDOW_LENGTH) {
369 
    tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
 380 
          gyroDWindowIdx = 0;
370 
  }
 381 
  }
371 
 
 382 
 
Line 490... Line 501...
490 
          airPressureWindow[windowPtr++] = simpleAirPressure;
 501 
          airPressureWindow[windowPtr++] = simpleAirPressure;
491 
          if (windowPtr >= staticParams.airpressureWindowLength) windowPtr = 0;
 502 
          if (windowPtr >= staticParams.airpressureWindowLength) windowPtr = 0;
492 
          filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;
 503 
          filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;
493 
      }
 504 
      }
Line -... Line 505...
- 
 
 505 
 
494 
 
 506 
      // positive diff of pressure
- 
 
 507 
      int16_t diff = filteredAirPressure - lastFilteredAirPressure;
- 
 
 508 
      // is a negative diff of height.
- 
 
 509 
      dHeight -= diff;
- 
 
 510 
      // remove old sample (fifo) from window.
- 
 
 511 
      dHeight += dAirPressureWindow[dWindowPtr];
495 
      dAirPressureWindow[dWindowPtr++] = (int16_t)(filteredAirPressure - lastFilteredAirPressure);
 512 
      dAirPressureWindow[dWindowPtr++] = diff;
496 
      if (dWindowPtr >= staticParams.airpressureDWindowLength) dWindowPtr = 0;
 - 
 
497 
 
 513 
      if (dWindowPtr >= staticParams.airpressureDWindowLength) dWindowPtr = 0;
498 
      pressureMeasurementCount = airPressureSum = 0;
 514 
      pressureMeasurementCount = airPressureSum = 0;
499 
    }
 515 
    }
500 
  }
 516 
  }
Line 530... Line 546...
530 
    accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY;
 546 
    accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY;
531 
    accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z;
 547 
    accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z;
532 
  }
 548 
  }
Line 533... Line 549...
533 
 
 549 
 
- 
 
 550 
  // Noise is relative to offset. So, reset noise measurements when changing offsets.
534 
  // Noise is relative to offset. So, reset noise measurements when changing offsets.
 551 
  for (uint8_t i=PITCH; i<=ROLL; i++) {
535 
  gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
 552 
          gyroNoisePeak[i] = 0;
- 
 
 553 
          accNoisePeak[i] = 0;
- 
 
 554 
          gyroD[i] = 0;
- 
 
 555 
          for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) {
- 
 
 556 
                  gyroDWindow[i][j] = 0;
536 
  accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0;
 557 
          }
537 
 
 558 
  }
538 
  // Setting offset values has an influence in the analog.c ISR
 559 
  // Setting offset values has an influence in the analog.c ISR
539 
  // Therefore run measurement for 100ms to achive stable readings
 560 
  // Therefore run measurement for 100ms to achive stable readings
Line 540... Line 561...
540 
  delay_ms_with_adc_measurement(100, 0);
 561 
  delay_ms_with_adc_measurement(100, 0);
Line 621... Line 642...
621 
int32_t analog_getHeight(void) {
 642 
int32_t analog_getHeight(void) {
622 
  return groundPressure - filteredAirPressure;
 643 
  return groundPressure - filteredAirPressure;
623 
}
 644 
}
Line 624... Line 645...
624 
 
 645 
 
625 
int16_t analog_getDHeight(void) {
 - 
 
626 
        int16_t result = 0;
 - 
 
627 
        for (int i=0; i<staticParams.airpressureDWindowLength; i++) {
 - 
 
628 
                result -= dAirPressureWindow[i]; // minus pressure is plus height.
 - 
 
629 
        }
 - 
 
630 
 
 - 
 
631 
        debugOut.analog[30] = result;
 - 
 
632 
  // dHeight = -dPressure, so here it is the old pressure minus the current, not opposite.
 646 
int16_t analog_getDHeight(void) {
633 
  return result;
 647 
  return dHeight;