Subversion Repositories FlightCtrl

Rev

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

Rev 2104 Rev 2105
Line 49... Line 49...
49 
 * Offset values. These are the raw gyro and acc. meter sums when the copter is
 49 
 * Offset values. These are the raw gyro and acc. meter sums when the copter is
50 
 * standing still. They are used for adjusting the gyro and acc. meter values
 50 
 * standing still. They are used for adjusting the gyro and acc. meter values
51 
 * to be centered on zero.
 51 
 * to be centered on zero.
52 
 */
 52 
 */
53 
sensorOffset_t gyroOffset;
 53 
sensorOffset_t gyroOffset;
- 
 
 54 
int16_t airpressureOffset;
Line 54... Line 55...
54 
 
 55 
 
55 
/*
 56 
/*
56 
 * In the MK coordinate system, nose-down is positive and left-roll is positive.
 57 
 * In the MK coordinate system, nose-down is positive and left-roll is positive.
57 
 * If a sensor is used in an orientation where one but not both of the axes has
 58 
 * If a sensor is used in an orientation where one but not both of the axes has
Line 109... Line 110...
109 
 
 110 
 
110 
/*
 111 
/*
111 
 * Airspeed
 112 
 * Airspeed
112 
 */
 113 
 */
113 
uint16_t simpleAirPressure;
 - 
 
Line 114... Line 114...
114 
uint16_t airspeedVelocity;
 114 
uint16_t simpleAirPressure;
115 
 
 115 
 
116 
/*
 116 
/*
117 
 * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
 117 
 * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
Line 232... Line 232...
232 
        } else {
 232 
        } else {
233 
                *noiseMeasurement = 0;
 233 
                *noiseMeasurement = 0;
234 
        }
 234 
        }
235 
}
 235 
}
Line 236... Line -...
236 
 
 - 
 
237 
/*
 - 
 
238 
 * Min.: 0
 - 
 
239 
 * Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type.
 - 
 
240 
 */
 - 
 
241 
uint16_t getSimplePressure(int advalue) {
 - 
 
242 
        return advalue;
 - 
 
243 
}
 - 
 
244 
 
 236 
 
245 
void startAnalogConversionCycle(void) {
 237 
void startAnalogConversionCycle(void) {
Line 246... Line 238...
246 
  analogDataReady = 0;
 238 
  analogDataReady = 0;
247 
 
 239 
 
Line 357... Line 349...
357 
  if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) {
 349 
  if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) {
358 
      gyroDWindowIdx = 0;
 350 
      gyroDWindowIdx = 0;
359 
  }
 351 
  }
360 
}
 352 
}
Line -... Line 353...
- 
 
 353 
 
- 
 
 354 
// probably wanna aim at 1/10 m/s/unit.
- 
 
 355 
#define LOG_AIRSPEED_FACTOR 2
361 
 
 356 
 
362 
void analog_updateAirPressure(void) {
 357 
void analog_updateAirspeed(void) {
- 
 
 358 
  uint16_t rawAirPressure = sensorInputs[AD_AIRPRESSURE];
- 
 
 359 
  int16_t temp = rawAirPressure - airpressureOffset;
363 
  uint16_t rawAirPressure = sensorInputs[AD_AIRPRESSURE];
 360 
  if (temp<0) temp = 0;
364 
  simpleAirPressure = rawAirPressure;
 361 
  simpleAirPressure = temp;
365 
  airspeedVelocity = isqrt16(simpleAirPressure);
 362 
  airspeedVelocity = (staticParams.airspeedCorrection * isqrt16(simpleAirPressure)) >> LOG_AIRSPEED_FACTOR;
Line 366... Line 363...
366 
}
 363 
}
367 
 
 364 
 
368 
void analog_updateBatteryVoltage(void) {
 365 
void analog_updateBatteryVoltage(void) {
Line 403... Line 400...
403 
  delay_ms_with_adc_measurement(100, 0);
 400 
  delay_ms_with_adc_measurement(100, 0);
Line 404... Line 401...
404 
 
 401 
 
405 
  // gyroActivity = 0;
 402 
  // gyroActivity = 0;
Line 406... Line 403...
406 
}
 403 
}
407 
 
 404 
 
408 
void analog_calibrateGyros(void) {
 405 
void analog_calibrate(void) {
409 
#define GYRO_OFFSET_CYCLES 64
 406 
#define OFFSET_CYCLES 64
410 
  uint8_t i, axis;
 407 
  uint8_t i, axis;
Line 411... Line 408...
411 
  int32_t offsets[3] = { 0, 0, 0 };
 408 
  int32_t offsets[4] = { 0, 0, 0, 0};
412 
  gyro_calibrate();
 409 
  gyro_calibrate();
413 
 
 410 
 
414 
  // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
 411 
  // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
415 
  for (i = 0; i < GYRO_OFFSET_CYCLES; i++) {
 412 
  for (i = 0; i < OFFSET_CYCLES; i++) {
416 
    delay_ms_with_adc_measurement(10, 1);
 413 
    delay_ms_with_adc_measurement(10, 1);
- 
 
 414 
    for (axis = PITCH; axis <= YAW; axis++) {
417 
    for (axis = PITCH; axis <= YAW; axis++) {
 415 
      offsets[axis] += rawGyroValue(axis);
Line 418... Line 416...
418 
      offsets[axis] += rawGyroValue(axis);
 416 
    }
419 
    }
 417 
    offsets[3] += sensorInputs[AD_AIRPRESSURE];
420 
  }
 - 
 
421 
 
 418 
  }
422 
  for (axis = PITCH; axis <= YAW; axis++) {
 419 
 
423 
    gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
 420 
  for (axis = PITCH; axis <= YAW; axis++) {
424 
 
 421 
    gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
425 
    int16_t min = (512-200) * GYRO_OVERSAMPLING;
 422 
    int16_t min = (512-200) * GYRO_OVERSAMPLING;
Line -... Line 423...
- 
 
 423 
    int16_t max = (512+200) * GYRO_OVERSAMPLING;
- 
 
 424 
    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
- 
 
 425 
      versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis;
- 
 
 426 
  }
- 
 
 427 
 
- 
 
 428 
  airpressureOffset = (offsets[3] + OFFSET_CYCLES / 2) / OFFSET_CYCLES;
426 
    int16_t max = (512+200) * GYRO_OVERSAMPLING;
 429 
  int16_t min = 200;
- 
 
 430 
  int16_t max = (1024-200) * 2;
427 
    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
 431 
  if(airpressure < min || airpressure > max)
428 
      versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis;
 432 
    versionInfo.hardwareErrors[0] |= FC_ERROR0_PRESSURE;