Subversion Repositories FlightCtrl

Rev

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