Subversion Repositories FlightCtrl

Rev

Rev 2164 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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