Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
2108 - 1
#include <avr/io.h>
2
#include <avr/interrupt.h>
3
#include <avr/pgmspace.h>
4
#include <stdlib.h>
5
 
6
#include "analog.h"
7
#include "configuration.h"
8
#include "attitude.h"
9
#include "printf_P.h"
10
#include "isqrt.h"
11
#include "twimaster.h"
12
 
13
// for Delay functions
14
#include "timer0.h"
15
 
16
// For reading and writing acc. meter offsets.
17
#include "eeprom.h"
18
 
19
// For debugOut
20
#include "output.h"
21
 
22
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
23
#define startADC() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
24
 
25
const char* recal = ", recalibration needed.";
26
volatile uint16_t ADSensorInputs[8];
27
 
28
/*
29
 * These 4 exported variables are zero-offset. The "PID" ones are used
30
 * in the attitude control as rotation rates. The "ATT" ones are for
31
 * integration to angles.
32
 */
33
int16_t gyro_PID[3];
34
int16_t gyro_ATT[3];
35
int16_t gyroD[3];
36
int16_t gyroDWindow[3][GYRO_D_WINDOW_LENGTH];
37
uint8_t gyroDWindowIdx = 0;
38
 
39
/*
40
 * Airspeed
41
 */
2132 - 42
//int16_t airpressure;
43
//uint16_t airspeedVelocity = 0;
2109 - 44
//int16_t airpressureWindow[AIRPRESSURE_WINDOW_LENGTH];
45
//uint8_t airpressureWindowIdx = 0;
2108 - 46
 
47
/*
48
 * Offset values. These are the raw gyro and acc. meter sums when the copter is
49
 * standing still. They are used for adjusting the gyro and acc. meter values
50
 * to be centered on zero.
51
 */
52
sensorOffset_t gyroOffset;
53
uint16_t airpressureOffset;
54
 
55
/*
56
 * 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
 * an opposite sign, PR_ORIENTATION_REVERSED is set to 1 (true).
59
 * Transform:
60
 * pitch <- pp*pitch + pr*roll
61
 * roll  <- rp*pitch + rr*roll
62
 * Not reversed, GYRO_QUADRANT:
63
 * 0: pp=1, pr=0, rp=0, rr=1  // 0    degrees
64
 * 1: pp=1, pr=-1,rp=1, rr=1  // +45  degrees
65
 * 2: pp=0, pr=-1,rp=1, rr=0  // +90  degrees
66
 * 3: pp=-1,pr=-1,rp=1, rr=1  // +135 degrees
67
 * 4: pp=-1,pr=0, rp=0, rr=-1 // +180 degrees
68
 * 5: pp=-1,pr=1, rp=-1,rr=-1 // +225 degrees
69
 * 6: pp=0, pr=1, rp=-1,rr=0  // +270 degrees
70
 * 7: pp=1, pr=1, rp=-1,rr=1  // +315 degrees
71
 * Reversed, GYRO_QUADRANT:
72
 * 0: pp=-1,pr=0, rp=0, rr=1  // 0    degrees with pitch reversed
73
 * 1: pp=-1,pr=-1,rp=-1,rr=1  // +45  degrees with pitch reversed
74
 * 2: pp=0, pr=-1,rp=-1,rr=0  // +90  degrees with pitch reversed
75
 * 3: pp=1, pr=-1,rp=-1,rr=1  // +135 degrees with pitch reversed
76
 * 4: pp=1, pr=0, rp=0, rr=-1 // +180 degrees with pitch reversed
77
 * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed
78
 * 6: pp=0, pr=1, rp=1, rr=0  // +270 degrees with pitch reversed
79
 * 7: pp=-1,pr=1, rp=1, rr=1  // +315 degrees with pitch reversed
80
 */
81
 
82
void rotate(int16_t* result, uint8_t quadrant, uint8_t reversePR, uint8_t reverseYaw) {
83
  static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1};
84
  // Pitch to Pitch part
85
  int8_t xx = reversePR ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant];
86
  // Roll to Pitch part
87
  int8_t xy = rotationTab[(quadrant+2)%8];
88
  // Pitch to Roll part
89
  int8_t yx = reversePR ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8];
90
  // Roll to Roll part
91
  int8_t yy = rotationTab[quadrant];
92
 
93
  int16_t xIn = result[0];
94
  result[0] = xx*xIn + xy*result[1];
95
  result[1] = yx*xIn + yy*result[1];
96
 
97
  if (quadrant & 1) {
98
        // A rotation was used above, where the factors were too large by sqrt(2).
99
        // So, we multiply by 2^n/sqt(2) and right shift n bits, as to divide by sqrt(2).
100
        // A suitable value for n: Sample is 11 bits. After transformation it is the sum
101
        // of 2 11 bit numbers, so 12 bits. We have 4 bits left...
102
        result[0] = (result[0]*11) >> 4;
103
        result[1] = (result[1]*11) >> 4;
104
  }
105
 
106
  if (reverseYaw)
107
    result[3] =-result[3];
108
}
109
 
110
/*
111
 * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
112
 * That is divided by 3 below, for a final 10.34 per volt.
113
 * So the initial value of 100 is for 9.7 volts.
114
 */
115
uint16_t UBat = 100;
116
 
117
/*
118
 * Control and status.
119
 */
120
volatile uint8_t sensorDataReady = ALL_DATA_READY;
121
 
122
/*
123
 * Experiment: Measuring vibration-induced sensor noise.
124
 */
125
uint16_t gyroNoisePeak[3];
126
 
127
volatile uint8_t adState;
128
volatile uint8_t adChannel;
129
 
130
// ADC channels
131
#define AD_UBAT           6
2132 - 132
//#define AD_AIRPRESSURE    7
2108 - 133
 
134
/*
135
 * Table of AD converter inputs for each state.
136
 * The number of samples summed for each channel is equal to
137
 * the number of times the channel appears in the array.
138
 * The max. number of samples that can be taken in 2 ms is:
139
 * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control
140
 * loop needs a little time between reading AD values and
141
 * re-enabling ADC, the real limit is (how much?) lower.
142
 * The acc. sensor is sampled even if not used - or installed
143
 * at all. The cost is not significant.
144
 */
145
 
146
const uint8_t channelsForStates[] PROGMEM = {
2132 - 147
  //AD_AIRPRESSURE,
148
  AD_UBAT
149
  //AD_AIRPRESSURE,
150
  //AD_AIRPRESSURE,
151
  //AD_AIRPRESSURE,
2108 - 152
};
153
 
154
// Feature removed. Could be reintroduced later - but should work for all gyro types then.
155
// uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
156
 
157
void analog_init(void) {
158
        uint8_t sreg = SREG;
159
        // disable all interrupts before reconfiguration
160
        cli();
161
 
162
        // ADC0 ... ADC7 is connected to PortA pin 0 ... 7
163
        // DDRA = 0x00;
164
        // PORTA = 0x00;
165
        // Digital Input Disable Register 0
166
        // Disable digital input buffer for analog adc_channel pins
167
        // DIDR0 = 0xFF;
168
        // external reference, adjust data to the right
169
        ADMUX &= ~((1<<REFS1)|(1<<REFS0)|(1<<ADLAR));
170
        // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
171
        ADMUX = (ADMUX & 0xE0);
172
        //Set ADC Control and Status Register A
173
        //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
174
        ADCSRA = (1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
175
        //Set ADC Control and Status Register B
176
        //Trigger Source to Free Running Mode
177
        ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0));
178
 
179
        startAnalogConversionCycle();
180
 
2133 - 181
        twimaster_init();
182
 
2108 - 183
        // restore global interrupt flags
184
        SREG = sreg;
185
}
186
 
187
/*
2135 - 188
 * The sensor is installed vertically and the axes are moved around a little...
2108 - 189
 */
2133 - 190
int16_t rawGyroValue(uint8_t axis) {
2135 - 191
  switch(axis) {
192
  case PITCH:
193
    return ITG3200SensorInputs[3];
194
  case ROLL:
195
    return ITG3200SensorInputs[1];
196
  case YAW:
197
    return ITG3200SensorInputs[2];
198
  }
199
  return 0;
2108 - 200
}
201
 
202
/*
203
uint16_t rawAccValue(uint8_t axis) {
204
        return sensorInputs[AD_ACC_PITCH-axis];
205
}
206
*/
207
 
208
void measureNoise(const int16_t sensor,
209
                volatile uint16_t* const noiseMeasurement, const uint8_t damping) {
210
        if (sensor > (int16_t) (*noiseMeasurement)) {
211
                *noiseMeasurement = sensor;
212
        } else if (-sensor > (int16_t) (*noiseMeasurement)) {
213
                *noiseMeasurement = -sensor;
214
        } else if (*noiseMeasurement > damping) {
215
                *noiseMeasurement -= damping;
216
        } else {
217
                *noiseMeasurement = 0;
218
        }
219
}
220
 
221
void startAnalogConversionCycle(void) {
2129 - 222
  twimaster_startCycle();
2108 - 223
  // Stop the sampling. Cycle is over.
224
  for (uint8_t i = 0; i<8; i++) {
225
    ADSensorInputs[i] = 0;
226
  }
227
 
228
  adState = 0;
2132 - 229
  adChannel = AD_UBAT;
2108 - 230
  ADMUX = (ADMUX & 0xE0) | adChannel;
231
  startADC();
2109 - 232
  sensorDataReady = 0;
2108 - 233
}
234
 
235
/*****************************************************
236
 * Interrupt Service Routine for ADC
237
 * Runs at 312.5 kHz or 3.2 �s. When all states are
238
 * processed further conversions are stopped.
239
 *****************************************************/
240
ISR(ADC_vect) {
241
  ADSensorInputs[adChannel] += ADC;
242
  // set up for next state.
243
  adState++;
244
  if (adState < sizeof(channelsForStates)) {
245
    adChannel = pgm_read_byte(&channelsForStates[adState]);
246
    // set adc muxer to next adChannel
247
    ADMUX = (ADMUX & 0xE0) | adChannel;
248
    // after full cycle stop further interrupts
249
    startADC();
250
  } else {
251
    sensorDataReady |= ADC_DATA_READY;
252
    // do not restart ADC converter. 
253
  }
254
}
255
 
256
void analog_updateGyros(void) {
257
  // for various filters...
258
  int16_t tempOffsetGyro[3], tempGyro;
259
 
260
  for (uint8_t axis=0; axis<3; axis++) {
261
    tempGyro = rawGyroValue(axis);
262
    /*
263
     * Process the gyro data for the PID controller.
264
     */
265
 
266
    // Saturation prevention was removed. No airplane rotates more than 2000 deg/s.
267
 
268
    // 2) Apply sign and offset, scale before filtering.
269
    tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]);
270
  }
271
 
272
  // 2.1: Transform axes.
273
  rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW);
274
 
275
  for (uint8_t axis=0; axis<3; axis++) {
276
        // 3) Filter.
277
    tempOffsetGyro[axis] = (gyro_PID[axis] * (IMUConfig.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / IMUConfig.gyroPIDFilterConstant;
278
 
279
    // 4) Measure noise.
280
    measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
281
 
282
    // 5) Differential measurement.
283
    // TODO: Examine effects of overruns here, they are quite possible.
284
    int16_t diff = tempOffsetGyro[axis] - gyro_PID[axis];
285
    gyroD[axis] -= gyroDWindow[axis][gyroDWindowIdx];
286
    gyroD[axis] += diff;
287
    gyroDWindow[axis][gyroDWindowIdx] = diff;
2141 - 288
    debugOut.analog[9+axis] = gyroD[axis];
2108 - 289
 
290
    // 6) Done.
291
    gyro_PID[axis] = tempOffsetGyro[axis];
292
 
293
    // Prepare tempOffsetGyro for next calculation below...
294
    tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]);
295
  }
296
 
297
  /*
298
   * Now process the data for attitude angles.
299
   */
300
  rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW);
301
 
302
  // dampenGyroActivity();
303
  gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
304
  gyro_ATT[ROLL] = tempOffsetGyro[ROLL];
305
  gyro_ATT[YAW] = tempOffsetGyro[YAW];
306
 
307
  if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) {
308
      gyroDWindowIdx = 0;
309
  }
310
}
311
 
2132 - 312
/*
2108 - 313
// probably wanna aim at 1/10 m/s/unit.
2129 - 314
#define LOG_AIRSPEED_FACTOR 0
2108 - 315
 
316
void analog_updateAirspeed(void) {
317
  uint16_t rawAirpressure = ADSensorInputs[AD_AIRPRESSURE];
2132 - 318
  int16_t temp = airpressureOffset - rawAirPressure;
2109 - 319
//   airpressure -= airpressureWindow[airpressureWindowIdx];
320
//  airpressure += temp;
321
//  airpressureWindow[airpressureWindowIdx] = temp;
322
//  airpressureWindowIdx++;
323
//  if (airpressureWindowIdx == AIRPRESSURE_WINDOW_LENGTH) {
324
//        airpressureWindowIdx = 0;
325
//  }
2108 - 326
 
2109 - 327
#define AIRPRESSURE_FILTER 16
328
  airpressure = ((int32_t)airpressure * (AIRPRESSURE_FILTER-1) + (AIRPRESSURE_FILTER/2) + temp) / AIRPRESSURE_FILTER;
329
 
330
  uint16_t p2 = (airpressure<0) ? 0 : airpressure;
331
  airspeedVelocity = (staticParams.airspeedCorrection * isqrt16(p2)) >> LOG_AIRSPEED_FACTOR;
332
 
333
  debugOut.analog[17] = airpressure;
334
  debugOut.analog[18] = airpressureOffset;
335
  debugOut.analog[19] = airspeedVelocity;
336
 
337
  isFlying = 0; //(airspeedVelocity >= staticParams.isFlyingThreshold);
2108 - 338
}
2132 - 339
*/
2108 - 340
 
341
void analog_updateBatteryVoltage(void) {
342
  // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
343
  // This is divided by 3 --> 10.34 counts per volt.
344
  UBat = (3 * UBat + ADSensorInputs[AD_UBAT] / 3) / 4;
345
}
346
 
347
void analog_update(void) {
348
  analog_updateGyros();
349
  // analog_updateAccelerometers();
2132 - 350
  // analog_updateAirspeed();
2108 - 351
  analog_updateBatteryVoltage();
352
#ifdef USE_MK3MAG
353
  magneticHeading = volatileMagneticHeading;
354
#endif
355
}
356
 
357
void analog_setNeutral() {
358
        twimaster_setNeutral();
359
 
360
  if (gyroOffset_readFromEEProm()) {
361
    printf("gyro offsets invalid%s",recal);
2133 - 362
    gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 0;
363
    gyroOffset.offsets[YAW] = 0;
2108 - 364
  }
365
 
366
  // Noise is relative to offset. So, reset noise measurements when changing offsets.
367
  for (uint8_t i=PITCH; i<=YAW; i++) {
368
          gyroNoisePeak[i] = 0;
369
          gyroD[i] = 0;
370
          for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) {
371
                  gyroDWindow[i][j] = 0;
372
          }
373
  }
374
 
375
  // Setting offset values has an influence in the analog.c ISR
376
  // Therefore run measurement for 100ms to achive stable readings
377
  delay_ms_with_adc_measurement(100, 0);
378
 
379
  // gyroActivity = 0;
380
}
381
 
382
void analog_calibrate(void) {
2129 - 383
#define OFFSET_CYCLES 120
2108 - 384
  uint8_t i, axis;
385
  int32_t offsets[4] = { 0, 0, 0, 0};
386
 
387
  // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
388
  for (i = 0; i < OFFSET_CYCLES; i++) {
389
    delay_ms_with_adc_measurement(10, 1);
390
    for (axis = PITCH; axis <= YAW; axis++) {
391
      offsets[axis] += rawGyroValue(axis);
392
    }
2132 - 393
    // offsets[3] += ADSensorInputs[AD_AIRPRESSURE];
2108 - 394
  }
395
 
396
  for (axis = PITCH; axis <= YAW; axis++) {
397
    gyroOffset.offsets[axis] = (offsets[axis] + OFFSET_CYCLES / 2) / OFFSET_CYCLES;
398
  }
399
 
2132 - 400
  /*
2108 - 401
  airpressureOffset = (offsets[3] + OFFSET_CYCLES / 2) / OFFSET_CYCLES;
402
  int16_t min = 200;
2109 - 403
  int16_t max = 1024-200;
404
 
2108 - 405
  if(airpressureOffset < min || airpressureOffset > max)
406
    versionInfo.hardwareErrors[0] |= FC_ERROR0_PRESSURE;
2132 - 407
  */
2108 - 408
 
409
  gyroOffset_writeToEEProm();
2132 - 410
  airpressureOffset_writeToEEProm();
2108 - 411
 
412
  startAnalogConversionCycle();
413
}