Subversion Repositories FlightCtrl

Rev

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

Rev 1971 Rev 1979
Line 84... Line 84...
84 
 * the offsets with the DAC.
 84 
 * the offsets with the DAC.
85 
 */
 85 
 */
86 
volatile uint16_t sensorInputs[8];
 86 
volatile uint16_t sensorInputs[8];
87 
volatile int16_t rawGyroSum[3];
 87 
volatile int16_t rawGyroSum[3];
88 
volatile int16_t acc[3];
 88 
volatile int16_t acc[3];
89 
volatile int16_t filteredAcc[2] = { 0,0 };
 89 
volatile int16_t filteredAcc[3] = { 0,0,0 };
90 
// volatile int32_t stronglyFilteredAcc[3] = { 0,0,0 };
 90 
// volatile int32_t stronglyFilteredAcc[3] = { 0,0,0 };
Line 91... Line 91...
91 
 
 91 
 
92 
/*
 92 
/*
93 
 * These 4 exported variables are zero-offset. The "PID" ones are used
 93 
 * These 4 exported variables are zero-offset. The "PID" ones are used
Line 148... Line 148...
148 
volatile uint8_t analogDataReady = 1;
 148 
volatile uint8_t analogDataReady = 1;
Line 149... Line 149...
149 
 
 149 
 
150 
/*
 150 
/*
151 
 * Experiment: Measuring vibration-induced sensor noise.
 151 
 * Experiment: Measuring vibration-induced sensor noise.
152 
 */
 152 
 */
153 
volatile uint16_t gyroNoisePeak[2];
 153 
volatile uint16_t gyroNoisePeak[3];
Line 154... Line 154...
154 
volatile uint16_t accNoisePeak[2];
 154 
volatile uint16_t accNoisePeak[3];
155 
 
 155 
 
156 
// ADC channels
 156 
// ADC channels
157 
#define AD_GYRO_YAW       0
 157 
#define AD_GYRO_YAW       0
Line 345... Line 345...
345 
  else
 345 
  else
346 
    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
 346 
    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
347 
}
 347 
}
Line 348... Line 348...
348 
 
 348 
 
- 
 
 349 
void analog_updateAccelerometers(void) {
- 
 
 350 
  // Z acc.
- 
 
 351 
  if (ACC_REVERSED[Z])
- 
 
 352 
    acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
- 
 
 353 
  else
- 
 
 354 
    acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z];
349 
void analog_updateAccelerometers(void) {
 355 
 
350 
  // Pitch and roll axis accelerations.
 356 
  // Pitch and roll axis accelerations.
351 
  for (uint8_t axis=0; axis<2; axis++) {
 357 
  for (uint8_t axis=0; axis<2; axis++) {
352 
    if (ACC_REVERSED[axis])
 358 
    if (ACC_REVERSED[axis])
353 
      acc[axis] = accOffset.offsets[axis] - sensorInputs[AD_ACC_PITCH-axis];
 359 
      acc[axis] = accOffset.offsets[axis] - sensorInputs[AD_ACC_PITCH-axis];
354 
    else
 360 
    else
- 
 
 361 
      acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset.offsets[axis];
355 
      acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset.offsets[axis];
 362 
  }
- 
 
 363 
 
356 
   
 364 
  for (uint8_t axis=0; axis<3; axis++) {
357 
    filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
 - 
 
358 
   
 - 
 
359 
    /*
 - 
 
360 
      stronglyFilteredAcc[PITCH] =
 - 
 
361 
      (stronglyFilteredAcc[PITCH] * 99 + acc[PITCH] * 10) / 100;
 - 
 
362 
    */
 - 
 
363 
   
 365 
    filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
364 
    measureNoise(acc[axis], &accNoisePeak[axis], 1);
 366 
    measureNoise(acc[axis], &accNoisePeak[axis], 1);
365 
  }
 - 
 
366 
 
 - 
 
367 
  // Z acc.
 - 
 
368 
  if (ACC_REVERSED[Z])
 - 
 
369 
    acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
 - 
 
370 
  else
 - 
 
371 
    acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z];
 - 
 
372 
 
 - 
 
373 
  /*
 - 
 
374 
    stronglyFilteredAcc[Z] =
 - 
 
375 
    (stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100;
 - 
 
376 
  */
 367 
  }
Line 377... Line 368...
377 
}
 368 
}
378 
 
 369 
 
379 
void analog_updateAirPressure(void) {
 370 
void analog_updateAirPressure(void) {
Line 480... Line 471...
480 
    printf("gyro offsets invalid%s",recal);
 471 
    printf("gyro offsets invalid%s",recal);
481 
    gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL;
 472 
    gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL;
482 
    gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW;
 473 
    gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW;
483 
  }
 474 
  }
Line 484... Line -...
484 
 
 - 
 
485 
  debugOut.analog[6] = gyroOffset.offsets[PITCH];
 - 
 
486 
  debugOut.analog[7] = gyroOffset.offsets[ROLL];
 - 
 
487 
 
 475 
 
488 
  if (accOffset_readFromEEProm()) {
 476 
  if (accOffset_readFromEEProm()) {
489 
    printf("acc. meter offsets invalid%s",recal);
 477 
    printf("acc. meter offsets invalid%s",recal);
490 
    accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_SUMMATION_FACTOR_PITCHROLL;
 478 
    accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_SUMMATION_FACTOR_PITCHROLL;
491 
    accOffset.offsets[Z] = 512 * ACC_SUMMATION_FACTOR_Z;
 479 
    accOffset.offsets[Z] = 717 * ACC_SUMMATION_FACTOR_Z;
Line 492... Line 480...
492 
  }
 480 
  }
493 
 
 481 
 
494 
  // Noise is relative to offset. So, reset noise measurements when changing offsets.
 482 
  // Noise is relative to offset. So, reset noise measurements when changing offsets.