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.