Subversion Repositories FlightCtrl

Rev

Rev 2086 | Rev 2089 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1612 dongfang 1
#include <avr/io.h>
2
#include <avr/interrupt.h>
3
#include <avr/pgmspace.h>
1864 - 4
 
1612 dongfang 5
#include "analog.h"
1864 - 6
#include "attitude.h"
1612 dongfang 7
#include "sensors.h"
1964 - 8
#include "printf_P.h"
2051 - 9
#include "mk3mag.h"
1612 dongfang 10
 
11
// for Delay functions
12
#include "timer0.h"
13
 
14
// For reading and writing acc. meter offsets.
15
#include "eeprom.h"
16
 
2052 - 17
// For debugOut
1796 - 18
#include "output.h"
19
 
1952 - 20
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
21
#define startADC() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
22
 
1969 - 23
const char* recal = ", recalibration needed.";
24
 
1854 - 25
/*
26
 * For each A/D conversion cycle, each analog channel is sampled a number of times
27
 * (see array channelsForStates), and the results for each channel are summed.
1645 - 28
 * Here are those for the gyros and the acc. meters. They are not zero-offset.
1612 dongfang 29
 * They are exported in the analog.h file - but please do not use them! The only
30
 * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
31
 * the offsets with the DAC.
32
 */
1952 - 33
volatile uint16_t sensorInputs[8];
2015 - 34
int16_t acc[3];
35
int16_t filteredAcc[3] = { 0,0,0 };
1612 dongfang 36
 
37
/*
1645 - 38
 * These 4 exported variables are zero-offset. The "PID" ones are used
39
 * in the attitude control as rotation rates. The "ATT" ones are for
1854 - 40
 * integration to angles.
1612 dongfang 41
 */
2015 - 42
int16_t gyro_PID[2];
43
int16_t gyro_ATT[2];
44
int16_t gyroD[2];
2086 - 45
int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH];
46
uint8_t gyroDWindowIdx = 0;
2015 - 47
int16_t yawGyro;
2051 - 48
int16_t magneticHeading;
1612 dongfang 49
 
2033 - 50
int32_t groundPressure;
2086 - 51
int16_t dHeight;
2033 - 52
 
1612 dongfang 53
/*
54
 * Offset values. These are the raw gyro and acc. meter sums when the copter is
55
 * standing still. They are used for adjusting the gyro and acc. meter values
1645 - 56
 * to be centered on zero.
1612 dongfang 57
 */
58
 
1969 - 59
sensorOffset_t gyroOffset;
60
sensorOffset_t accOffset;
61
sensorOffset_t gyroAmplifierOffset;
1960 - 62
 
1612 dongfang 63
/*
2015 - 64
 * In the MK coordinate system, nose-down is positive and left-roll is positive.
65
 * If a sensor is used in an orientation where one but not both of the axes has
66
 * an opposite sign, PR_ORIENTATION_REVERSED is set to 1 (true).
67
 * Transform:
68
 * pitch <- pp*pitch + pr*roll
69
 * roll  <- rp*pitch + rr*roll
70
 * Not reversed, GYRO_QUADRANT:
71
 * 0: pp=1, pr=0, rp=0, rr=1  // 0    degrees
72
 * 1: pp=1, pr=-1,rp=1, rr=1  // +45  degrees
73
 * 2: pp=0, pr=-1,rp=1, rr=0  // +90  degrees
74
 * 3: pp=-1,pr=-1,rp=1, rr=1  // +135 degrees
75
 * 4: pp=-1,pr=0, rp=0, rr=-1 // +180 degrees
76
 * 5: pp=-1,pr=1, rp=-1,rr=-1 // +225 degrees
77
 * 6: pp=0, pr=1, rp=-1,rr=0  // +270 degrees
78
 * 7: pp=1, pr=1, rp=-1,rr=1  // +315 degrees
79
 * Reversed, GYRO_QUADRANT:
80
 * 0: pp=-1,pr=0, rp=0, rr=1  // 0    degrees with pitch reversed
81
 * 1: pp=-1,pr=-1,rp=-1,rr=1  // +45  degrees with pitch reversed
82
 * 2: pp=0, pr=-1,rp=-1,rr=0  // +90  degrees with pitch reversed
83
 * 3: pp=1, pr=-1,rp=-1,rr=1  // +135 degrees with pitch reversed
84
 * 4: pp=1, pr=0, rp=0, rr=-1 // +180 degrees with pitch reversed
85
 * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed
86
 * 6: pp=0, pr=1, rp=1, rr=0  // +270 degrees with pitch reversed
87
 * 7: pp=-1,pr=1, rp=1, rr=1  // +315 degrees with pitch reversed
1612 dongfang 88
 */
89
 
2015 - 90
void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {
91
  static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1};
92
  // Pitch to Pitch part
2020 - 93
  int8_t xx = reverse ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant];
2015 - 94
  // Roll to Pitch part
95
  int8_t xy = rotationTab[(quadrant+2)%8];
96
  // Pitch to Roll part
97
  int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8];
98
  // Roll to Roll part
99
  int8_t yy = rotationTab[quadrant];
100
 
101
  int16_t xIn = result[0];
2019 - 102
  result[0] = xx*xIn + xy*result[1];
2015 - 103
  result[1] = yx*xIn + yy*result[1];
104
 
105
  if (quadrant & 1) {
106
        // A rotation was used above, where the factors were too large by sqrt(2).
107
        // So, we multiply by 2^n/sqt(2) and right shift n bits, as to divide by sqrt(2).
108
        // A suitable value for n: Sample is 11 bits. After transformation it is the sum
109
        // of 2 11 bit numbers, so 12 bits. We have 4 bits left...
110
        result[0] = (result[0]*11) >> 4;
111
        result[1] = (result[1]*11) >> 4;
112
  }
113
}
2019 - 114
 
1645 - 115
/*
1775 - 116
 * Air pressure
1645 - 117
 */
1970 - 118
volatile uint8_t rangewidth = 105;
1612 dongfang 119
 
1775 - 120
// Direct from sensor, irrespective of range.
121
// volatile uint16_t rawAirPressure;
122
 
123
// Value of 2 samples, with range.
2015 - 124
uint16_t simpleAirPressure;
1775 - 125
 
2019 - 126
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered.
2015 - 127
int32_t filteredAirPressure;
1775 - 128
 
2073 - 129
#define MAX_D_AIRPRESSURE_WINDOW_LENGTH 32
2071 - 130
//int32_t lastFilteredAirPressure;
131
int16_t dAirPressureWindow[MAX_D_AIRPRESSURE_WINDOW_LENGTH];
2073 - 132
uint8_t dWindowPtr = 0;
2071 - 133
 
2036 - 134
#define MAX_AIRPRESSURE_WINDOW_LENGTH 32
2026 - 135
int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];
136
int32_t windowedAirPressure;
2073 - 137
uint8_t windowPtr = 0;
2026 - 138
 
1775 - 139
// Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples.
2015 - 140
int32_t airPressureSum;
1775 - 141
 
142
// The number of samples summed into airPressureSum so far.
2015 - 143
uint8_t pressureMeasurementCount;
1775 - 144
 
1612 dongfang 145
/*
1854 - 146
 * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
1612 dongfang 147
 * That is divided by 3 below, for a final 10.34 per volt.
148
 * So the initial value of 100 is for 9.7 volts.
149
 */
2015 - 150
int16_t UBat = 100;
1612 dongfang 151
 
152
/*
153
 * Control and status.
154
 */
155
volatile uint16_t ADCycleCount = 0;
156
volatile uint8_t analogDataReady = 1;
157
 
158
/*
159
 * Experiment: Measuring vibration-induced sensor noise.
160
 */
2015 - 161
uint16_t gyroNoisePeak[3];
162
uint16_t accNoisePeak[3];
1612 dongfang 163
 
1986 - 164
volatile uint8_t adState;
1987 - 165
volatile uint8_t adChannel;
1986 - 166
 
1612 dongfang 167
// ADC channels
1645 - 168
#define AD_GYRO_YAW       0
169
#define AD_GYRO_ROLL      1
1634 - 170
#define AD_GYRO_PITCH     2
171
#define AD_AIRPRESSURE    3
1645 - 172
#define AD_UBAT           4
173
#define AD_ACC_Z          5
174
#define AD_ACC_ROLL       6
175
#define AD_ACC_PITCH      7
1612 dongfang 176
 
177
/*
178
 * Table of AD converter inputs for each state.
1854 - 179
 * The number of samples summed for each channel is equal to
1612 dongfang 180
 * the number of times the channel appears in the array.
181
 * The max. number of samples that can be taken in 2 ms is:
1854 - 182
 * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control
183
 * loop needs a little time between reading AD values and
1612 dongfang 184
 * re-enabling ADC, the real limit is (how much?) lower.
185
 * The acc. sensor is sampled even if not used - or installed
186
 * at all. The cost is not significant.
187
 */
188
 
1870 - 189
const uint8_t channelsForStates[] PROGMEM = {
190
  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW,
191
  AD_ACC_PITCH, AD_ACC_ROLL, AD_AIRPRESSURE,
1612 dongfang 192
 
1870 - 193
  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_ACC_Z, // at 8, measure Z acc.
194
  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, // at 11, finish yaw gyro
195
 
196
  AD_ACC_PITCH,   // at 12, finish pitch axis acc.
197
  AD_ACC_ROLL,    // at 13, finish roll axis acc.
198
  AD_AIRPRESSURE, // at 14, finish air pressure.
199
 
200
  AD_GYRO_PITCH,  // at 15, finish pitch gyro
201
  AD_GYRO_ROLL,   // at 16, finish roll gyro
202
  AD_UBAT         // at 17, measure battery.
203
};
1612 dongfang 204
 
205
// Feature removed. Could be reintroduced later - but should work for all gyro types then.
206
// uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
207
 
208
void analog_init(void) {
1821 - 209
        uint8_t sreg = SREG;
210
        // disable all interrupts before reconfiguration
211
        cli();
1612 dongfang 212
 
1821 - 213
        //ADC0 ... ADC7 is connected to PortA pin 0 ... 7
214
        DDRA = 0x00;
215
        PORTA = 0x00;
216
        // Digital Input Disable Register 0
217
        // Disable digital input buffer for analog adc_channel pins
218
        DIDR0 = 0xFF;
219
        // external reference, adjust data to the right
1952 - 220
        ADMUX &= ~((1<<REFS1)|(1<<REFS0)|(1<<ADLAR));
1821 - 221
        // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
1987 - 222
        ADMUX = (ADMUX & 0xE0);
1821 - 223
        //Set ADC Control and Status Register A
224
        //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
1952 - 225
        ADCSRA = (1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
1821 - 226
        //Set ADC Control and Status Register B
227
        //Trigger Source to Free Running Mode
1952 - 228
        ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0));
229
 
2026 - 230
        for (uint8_t i=0; i<MAX_AIRPRESSURE_WINDOW_LENGTH; i++) {
231
          airPressureWindow[i] = 0;
232
        }
2036 - 233
    windowedAirPressure = 0;
2026 - 234
 
1952 - 235
        startAnalogConversionCycle();
236
 
1821 - 237
        // restore global interrupt flags
238
        SREG = sreg;
1612 dongfang 239
}
240
 
2015 - 241
uint16_t rawGyroValue(uint8_t axis) {
242
        return sensorInputs[AD_GYRO_PITCH-axis];
243
}
244
 
245
uint16_t rawAccValue(uint8_t axis) {
246
        return sensorInputs[AD_ACC_PITCH-axis];
247
}
248
 
1821 - 249
void measureNoise(const int16_t sensor,
250
                volatile uint16_t* const noiseMeasurement, const uint8_t damping) {
251
        if (sensor > (int16_t) (*noiseMeasurement)) {
252
                *noiseMeasurement = sensor;
253
        } else if (-sensor > (int16_t) (*noiseMeasurement)) {
254
                *noiseMeasurement = -sensor;
255
        } else if (*noiseMeasurement > damping) {
256
                *noiseMeasurement -= damping;
257
        } else {
258
                *noiseMeasurement = 0;
259
        }
1612 dongfang 260
}
261
 
1796 - 262
/*
263
 * Min.: 0
264
 * Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type.
265
 */
1775 - 266
uint16_t getSimplePressure(int advalue) {
2026 - 267
        uint16_t result = (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
268
        result += (acc[Z] * (staticParams.airpressureAccZCorrection-128)) >> 10;
269
        return result;
1634 - 270
}
271
 
1952 - 272
void startAnalogConversionCycle(void) {
1960 - 273
  analogDataReady = 0;
2017 - 274
 
1952 - 275
  // Stop the sampling. Cycle is over.
276
  for (uint8_t i = 0; i < 8; i++) {
277
    sensorInputs[i] = 0;
278
  }
1986 - 279
  adState = 0;
1987 - 280
  adChannel = AD_GYRO_PITCH;
281
  ADMUX = (ADMUX & 0xE0) | adChannel;
1952 - 282
  startADC();
283
}
284
 
1645 - 285
/*****************************************************
1854 - 286
 * Interrupt Service Routine for ADC
1963 - 287
 * Runs at 312.5 kHz or 3.2 �s. When all states are
1952 - 288
 * processed further conversions are stopped.
1645 - 289
 *****************************************************/
1870 - 290
ISR(ADC_vect) {
1986 - 291
  sensorInputs[adChannel] += ADC;
1952 - 292
  // set up for next state.
1986 - 293
  adState++;
294
  if (adState < sizeof(channelsForStates)) {
295
    adChannel = pgm_read_byte(&channelsForStates[adState]);
296
    // set adc muxer to next adChannel
297
    ADMUX = (ADMUX & 0xE0) | adChannel;
1952 - 298
    // after full cycle stop further interrupts
299
    startADC();
300
  } else {
301
    ADCycleCount++;
302
    analogDataReady = 1;
303
    // do not restart ADC converter. 
304
  }
305
}
1612 dongfang 306
 
2055 - 307
// Experimental gyro shake takeoff detect!
308
uint16_t gyroActivity = 0;
309
void measureGyroActivityAndUpdateGyro(uint8_t axis, int16_t newValue) {
310
  int16_t tmp = gyro_ATT[axis];
311
  gyro_ATT[axis] = newValue;
312
 
313
  tmp -= newValue;
314
  tmp = (tmp*tmp) >> 4;
315
 
316
  if (gyroActivity + (uint16_t)tmp < 0x8000)
317
    gyroActivity += tmp;
318
}
319
 
320
#define GADAMPING 10
321
void dampenGyroActivity(void) {
322
  uint32_t tmp = gyroActivity;
323
  tmp *= ((1<<GADAMPING)-1);
324
  tmp >>= GADAMPING;
325
  gyroActivity = tmp;
326
}
327
 
1952 - 328
void analog_updateGyros(void) {
329
  // for various filters...
2015 - 330
  int16_t tempOffsetGyro[2], tempGyro;
1952 - 331
 
1991 - 332
  debugOut.digital[0] &= ~DEBUG_SENSORLIMIT;
1952 - 333
  for (uint8_t axis=0; axis<2; axis++) {
2015 - 334
    tempGyro = rawGyroValue(axis);
1952 - 335
    /*
336
     * Process the gyro data for the PID controller.
337
     */
338
    // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a
339
    //    gyro with a wider range, and helps counter saturation at full control.
340
 
1960 - 341
    if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) {
1952 - 342
      if (tempGyro < SENSOR_MIN_PITCHROLL) {
2015 - 343
                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
344
                tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
1952 - 345
      } else if (tempGyro > SENSOR_MAX_PITCHROLL) {
2015 - 346
                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
347
                tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL;
1952 - 348
      }
349
    }
2015 - 350
 
1952 - 351
    // 2) Apply sign and offset, scale before filtering.
2015 - 352
    tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
353
  }
354
 
355
  // 2.1: Transform axes.
2020 - 356
  rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
2015 - 357
 
358
  for (uint8_t axis=0; axis<2; axis++) {
359
        // 3) Filter.
360
    tempOffsetGyro[axis] = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / staticParams.gyroPIDFilterConstant;
361
 
1952 - 362
    // 4) Measure noise.
2015 - 363
    measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
364
 
1952 - 365
    // 5) Differential measurement.
2086 - 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];
369
    gyroD[axis] += diff;
370
    gyroDWindow[axis][gyroDWindowIdx] = diff;
2015 - 371
 
1952 - 372
    // 6) Done.
2015 - 373
    gyro_PID[axis] = tempOffsetGyro[axis];
374
 
375
    // Prepare tempOffsetGyro for next calculation below...
376
    tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
1952 - 377
  }
2086 - 378
 
379
  if (gyroDWindowIdx >= GYRO_D_WINDOW_LENGTH) {
380
          gyroDWindowIdx = 0;
381
  }
382
 
2015 - 383
  /*
384
   * Now process the data for attitude angles.
385
   */
2020 - 386
   rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
2015 - 387
 
2055 - 388
   measureGyroActivityAndUpdateGyro(0, tempOffsetGyro[PITCH]);
389
   measureGyroActivityAndUpdateGyro(1, tempOffsetGyro[ROLL]);
390
   dampenGyroActivity();
2017 - 391
 
1952 - 392
  // Yaw gyro.
2020 - 393
  if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW)
1960 - 394
    yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
1952 - 395
  else
1960 - 396
    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
1952 - 397
}
1775 - 398
 
1952 - 399
void analog_updateAccelerometers(void) {
400
  // Pitch and roll axis accelerations.
401
  for (uint8_t axis=0; axis<2; axis++) {
2015 - 402
    acc[axis] = rawAccValue(axis) - accOffset.offsets[axis];
1979 - 403
  }
2015 - 404
 
2020 - 405
  rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_ACC_XY);
2015 - 406
  for(uint8_t axis=0; axis<3; axis++) {
1960 - 407
    filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
1952 - 408
    measureNoise(acc[axis], &accNoisePeak[axis], 1);
409
  }
2015 - 410
 
411
  // Z acc.
412
  if (staticParams.imuReversedFlags & 8)
413
    acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
414
  else
415
    acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z];
2069 - 416
 
417
  debugOut.analog[29] = acc[Z];
1952 - 418
}
1645 - 419
 
1952 - 420
void analog_updateAirPressure(void) {
421
  static uint16_t pressureAutorangingWait = 25;
422
  uint16_t rawAirPressure;
423
  int16_t newrange;
424
  // air pressure
425
  if (pressureAutorangingWait) {
426
    //A range switch was done recently. Wait for steadying.
427
    pressureAutorangingWait--;
428
  } else {
429
    rawAirPressure = sensorInputs[AD_AIRPRESSURE];
430
    if (rawAirPressure < MIN_RAWPRESSURE) {
431
      // value is too low, so decrease voltage on the op amp minus input, making the value higher.
432
      newrange = OCR0A - (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (MAX_RAWPRESSURE - rawAirPressure) / (rangewidth * 2) + 1;
433
      if (newrange > MIN_RANGES_EXTRAPOLATION) {
434
        pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; // = OCRA0 - OCRA0 +
435
        OCR0A = newrange;
436
      } else {
437
        if (OCR0A) {
438
          OCR0A--;
439
          pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;
1821 - 440
        }
1952 - 441
      }
442
    } else if (rawAirPressure > MAX_RAWPRESSURE) {
443
      // value is too high, so increase voltage on the op amp minus input, making the value lower.
444
      // If near the end, make a limited increase
445
      newrange = OCR0A + (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4;  // (rawAirPressure - MIN_RAWPRESSURE) / (rangewidth * 2) - 1;
446
      if (newrange < MAX_RANGES_EXTRAPOLATION) {
447
        pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR;
448
        OCR0A = newrange;
449
      } else {
450
        if (OCR0A < 254) {
451
          OCR0A++;
452
          pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;
453
        }
454
      }
455
    }
456
 
457
    // Even if the sample is off-range, use it.
458
    simpleAirPressure = getSimplePressure(rawAirPressure);
2055 - 459
    debugOut.analog[6] = rawAirPressure;
460
    debugOut.analog[7] = simpleAirPressure;
1952 - 461
 
462
    if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
463
      // Danger: pressure near lower end of range. If the measurement saturates, the
464
      // copter may climb uncontrolledly... Simulate a drastic reduction in pressure.
1955 - 465
      debugOut.digital[1] |= DEBUG_SENSORLIMIT;
1952 - 466
      airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth
467
        + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION
468
           * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
469
    } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) {
470
      // Danger: pressure near upper end of range. If the measurement saturates, the
471
      // copter may descend uncontrolledly... Simulate a drastic increase in pressure.
1955 - 472
      debugOut.digital[1] |= DEBUG_SENSORLIMIT;
1952 - 473
      airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth
474
        + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION
475
           * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
476
    } else {
477
      // normal case.
2026 - 478
      // If AIRPRESSURE_OVERSAMPLING is an odd number we only want to add half the double sample.
1952 - 479
      // The 2 cases above (end of range) are ignored for this.
1955 - 480
      debugOut.digital[1] &= ~DEBUG_SENSORLIMIT;
2035 - 481
          airPressureSum += simpleAirPressure;
1952 - 482
    }
483
 
484
    // 2 samples were added.
485
    pressureMeasurementCount += 2;
2035 - 486
    // Assumption here: AIRPRESSURE_OVERSAMPLING is even (well we all know it's 14 haha...)
487
    if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING) {
488
 
489
      // The best oversampling count is 14.5. We add a quarter of the double ADC value to get the final half.
490
      airPressureSum += simpleAirPressure >> 2;
491
 
2071 - 492
      uint32_t lastFilteredAirPressure = filteredAirPressure;
2035 - 493
 
494
      if (!staticParams.airpressureWindowLength) {
495
          filteredAirPressure = (filteredAirPressure * (staticParams.airpressureFilterConstant - 1)
496
                          + airPressureSum + staticParams.airpressureFilterConstant / 2) / staticParams.airpressureFilterConstant;
497
      } else {
498
          // use windowed.
2036 - 499
          windowedAirPressure += simpleAirPressure;
500
          windowedAirPressure -= airPressureWindow[windowPtr];
2071 - 501
          airPressureWindow[windowPtr++] = simpleAirPressure;
2073 - 502
          if (windowPtr >= staticParams.airpressureWindowLength) windowPtr = 0;
2036 - 503
          filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;
2035 - 504
      }
2036 - 505
 
2086 - 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];
512
      dAirPressureWindow[dWindowPtr++] = diff;
2073 - 513
      if (dWindowPtr >= staticParams.airpressureDWindowLength) dWindowPtr = 0;
1952 - 514
      pressureMeasurementCount = airPressureSum = 0;
515
    }
516
  }
517
}
1821 - 518
 
1952 - 519
void analog_updateBatteryVoltage(void) {
520
  // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
521
  // This is divided by 3 --> 10.34 counts per volt.
522
  UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;
523
}
1821 - 524
 
1952 - 525
void analog_update(void) {
526
  analog_updateGyros();
527
  analog_updateAccelerometers();
528
  analog_updateAirPressure();
529
  analog_updateBatteryVoltage();
2052 - 530
#ifdef USE_MK3MAG
2055 - 531
  magneticHeading = volatileMagneticHeading;
2052 - 532
#endif
1612 dongfang 533
}
534
 
1961 - 535
void analog_setNeutral() {
2018 - 536
  gyro_init();
537
 
1961 - 538
  if (gyroOffset_readFromEEProm()) {
1969 - 539
    printf("gyro offsets invalid%s",recal);
2019 - 540
    gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING_PITCHROLL;
541
    gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING_YAW;
1961 - 542
  }
1964 - 543
 
1961 - 544
  if (accOffset_readFromEEProm()) {
1969 - 545
    printf("acc. meter offsets invalid%s",recal);
2019 - 546
    accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY;
547
    accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z;
1961 - 548
  }
549
 
550
  // Noise is relative to offset. So, reset noise measurements when changing offsets.
2086 - 551
  for (uint8_t i=PITCH; i<=ROLL; i++) {
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;
557
          }
558
  }
1961 - 559
  // Setting offset values has an influence in the analog.c ISR
560
  // Therefore run measurement for 100ms to achive stable readings
2015 - 561
  delay_ms_with_adc_measurement(100, 0);
1961 - 562
 
2055 - 563
  gyroActivity = 0;
1961 - 564
}
565
 
566
void analog_calibrateGyros(void) {
1612 dongfang 567
#define GYRO_OFFSET_CYCLES 32
1952 - 568
  uint8_t i, axis;
1963 - 569
  int32_t offsets[3] = { 0, 0, 0 };
1952 - 570
  gyro_calibrate();
571
 
572
  // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
573
  for (i = 0; i < GYRO_OFFSET_CYCLES; i++) {
2015 - 574
    delay_ms_with_adc_measurement(10, 1);
1952 - 575
    for (axis = PITCH; axis <= YAW; axis++) {
2015 - 576
      offsets[axis] += rawGyroValue(axis);
1952 - 577
    }
578
  }
579
 
580
  for (axis = PITCH; axis <= YAW; axis++) {
1963 - 581
    gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
2018 - 582
 
2019 - 583
    int16_t min = (512-200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL;
584
    int16_t max = (512+200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL;
2018 - 585
    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
586
      versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis;
1952 - 587
  }
1961 - 588
 
589
  gyroOffset_writeToEEProm();  
2015 - 590
  startAnalogConversionCycle();
1612 dongfang 591
}
592
 
593
/*
594
 * Find acc. offsets for a neutral reading, and write them to EEPROM.
595
 * Does not (!} update the local variables. This must be done with a
596
 * call to analog_calibrate() - this always (?) is done by the caller
597
 * anyway. There would be nothing wrong with updating the variables
598
 * directly from here, though.
599
 */
600
void analog_calibrateAcc(void) {
2015 - 601
#define ACC_OFFSET_CYCLES 32
1960 - 602
  uint8_t i, axis;
2015 - 603
  int32_t offsets[3] = { 0, 0, 0 };
604
 
1960 - 605
  for (i = 0; i < ACC_OFFSET_CYCLES; i++) {
2015 - 606
    delay_ms_with_adc_measurement(10, 1);
1960 - 607
    for (axis = PITCH; axis <= YAW; axis++) {
2015 - 608
      offsets[axis] += rawAccValue(axis);
1960 - 609
    }
610
  }
2015 - 611
 
1960 - 612
  for (axis = PITCH; axis <= YAW; axis++) {
2015 - 613
    accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
2018 - 614
    int16_t min,max;
615
    if (axis==Z) {
2020 - 616
        if (staticParams.imuReversedFlags & IMU_REVERSE_ACC_Z) {
2018 - 617
        // TODO: This assumes a sensitivity of +/- 2g.
2019 - 618
                min = (256-200) * ACC_OVERSAMPLING_Z;
619
                        max = (256+200) * ACC_OVERSAMPLING_Z;
2018 - 620
        } else {
621
        // TODO: This assumes a sensitivity of +/- 2g.
2019 - 622
                min = (768-200) * ACC_OVERSAMPLING_Z;
623
                        max = (768+200) * ACC_OVERSAMPLING_Z;
2018 - 624
        }
625
    } else {
2019 - 626
        min = (512-200) * ACC_OVERSAMPLING_XY;
627
        max = (512+200) * ACC_OVERSAMPLING_XY;
2018 - 628
    }
629
    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) {
630
      versionInfo.hardwareErrors[0] |= FC_ERROR0_ACC_X << axis;
631
    }
1960 - 632
  }
1961 - 633
 
2015 - 634
  accOffset_writeToEEProm();
635
  startAnalogConversionCycle();
1612 dongfang 636
}
2033 - 637
 
638
void analog_setGround() {
639
  groundPressure = filteredAirPressure;
640
}
641
 
642
int32_t analog_getHeight(void) {
643
  return groundPressure - filteredAirPressure;
644
}
645
 
646
int16_t analog_getDHeight(void) {
2088 - 647
/*
648
        int16_t result = 0;
649
        for (int i=0; i<staticParams.airpressureDWindowLength; i++) {
650
                result -= dAirPressureWindow[i]; // minus pressure is plus height.
651
        }
652
  // dHeight = -dPressure, so here it is the old pressure minus the current, not opposite.
653
  return result;
654
*/
2086 - 655
  return dHeight;
2033 - 656
}