Subversion Repositories FlightCtrl

Rev

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

Rev 2116 Rev 2119
Line 5... Line 5...
5
 
5
 
6
#include "analog.h"
6
#include "analog.h"
7
#include "attitude.h"
7
#include "attitude.h"
8
#include "printf_P.h"
8
#include "printf_P.h"
-
 
9
#include "isqrt.h"
Line 9... Line 10...
9
#include "isqrt.h"
10
#include "sensors.h"
10
 
11
 
Line 11... Line 12...
11
// for Delay functions
12
// for Delay functions
Line 402... Line 403...
402
 
403
 
403
  // gyroActivity = 0;
404
  // gyroActivity = 0;
Line 404... Line 405...
404
}
405
}
405
 
406
 
406
void analog_calibrate(void) {
407
void analog_calibrate(void) {
407
#define OFFSET_CYCLES 64
408
#define OFFSET_CYCLES 250
408
  uint8_t i, axis;
409
  uint8_t i, axis;
Line 409... Line 410...
409
  int32_t offsets[4] = { 0, 0, 0, 0};
410
  int32_t offsets[4] = { 0, 0, 0, 0 };
410
  gyro_calibrate();
411
  gyro_calibrate();
411
 
412
 
412
  // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
413
  // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
413
  for (i = 0; i < OFFSET_CYCLES; i++) {
414
  for (i = 0; i < OFFSET_CYCLES; i++) {
414
    delay_ms_with_adc_measurement(10, 1);
415
    delay_ms_with_adc_measurement(2, 1);
415
    for (axis = PITCH; axis <= YAW; axis++) {
416
    for (axis = PITCH; axis <= YAW; axis++) {
416
      offsets[axis] += rawGyroValue(axis);
417
      offsets[axis] += rawGyroValue(axis);
Line 417... Line 418...
417
    }
418
    }
418
    offsets[3] += sensorInputs[AD_AIRPRESSURE];
419
    offsets[3] += sensorInputs[AD_AIRPRESSURE];
419
  }
420
  }
420
 
421
 
421
  for (axis = PITCH; axis <= YAW; axis++) {
422
  for (axis = PITCH; axis <= YAW; axis++) {
422
    gyroOffset.offsets[axis] = (offsets[axis] + OFFSET_CYCLES / 2) / OFFSET_CYCLES;
423
    gyroOffset.offsets[axis] = (offsets[axis]  /*+ OFFSET_CYCLES/2 */) / OFFSET_CYCLES;
423
    int16_t min = (512-200) * GYRO_OVERSAMPLING;
424
    int16_t min = (512-200) * GYRO_OVERSAMPLING;
Line 424... Line 425...
424
    int16_t max = (512+200) * GYRO_OVERSAMPLING;
425
    int16_t max = (512+200) * GYRO_OVERSAMPLING;
425
    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
426
    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
426
      versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis;
427
      versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis;
427
  }
428
  }
428
 
429