Subversion Repositories FlightCtrl

Rev

Rev 2035 | Rev 2052 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2035 Rev 2036
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
2
// + Copyright (c) 04.2007 Holger Buss
-
 
3
// + Nur f�r den privaten Gebrauch
-
 
4
// + www.MikroKopter.com
-
 
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
6
// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
-
 
7
// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist.
-
 
8
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
-
 
9
// + bzgl. der Nutzungsbedingungen aufzunehmen.
-
 
10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
-
 
11
// + Verkauf von Luftbildaufnahmen, usw.
-
 
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
13
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
-
 
14
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
-
 
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
-
 
17
// + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
-
 
18
// + eindeutig als Ursprung verlinkt und genannt werden
-
 
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
20
// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
-
 
21
// + Benutzung auf eigene Gefahr
-
 
22
// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
-
 
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
24
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
-
 
25
// + mit unserer Zustimmung zul�ssig
-
 
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
27
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
-
 
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
29
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
-
 
30
// + this list of conditions and the following disclaimer.
-
 
31
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
-
 
32
// +     from this software without specific prior written permission.
-
 
33
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
-
 
34
// +     for non-commercial use (directly or indirectly)
-
 
35
// +     Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
-
 
36
// +     with our written permission
-
 
37
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
-
 
38
// +     clearly linked as origin
-
 
39
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
-
 
40
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-
 
41
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-
 
42
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-
 
43
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-
 
44
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-
 
45
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-
 
46
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-
 
47
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-
 
48
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-
 
49
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-
 
50
// +  POSSIBILITY OF SUCH DAMAGE.
-
 
51
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
52
#include <avr/io.h>
1
#include <avr/io.h>
53
#include <avr/interrupt.h>
2
#include <avr/interrupt.h>
54
#include <avr/pgmspace.h>
3
#include <avr/pgmspace.h>
55
 
4
 
56
#include "analog.h"
5
#include "analog.h"
57
#include "attitude.h"
6
#include "attitude.h"
58
#include "sensors.h"
7
#include "sensors.h"
59
#include "printf_P.h"
8
#include "printf_P.h"
60
 
9
 
61
// for Delay functions
10
// for Delay functions
62
#include "timer0.h"
11
#include "timer0.h"
63
 
12
 
64
// For debugOut
13
// For debugOut
65
#include "uart0.h"
14
#include "uart0.h"
66
 
15
 
67
// For reading and writing acc. meter offsets.
16
// For reading and writing acc. meter offsets.
68
#include "eeprom.h"
17
#include "eeprom.h"
69
 
18
 
70
// For debugOut.digital
19
// For debugOut.digital
71
#include "output.h"
20
#include "output.h"
72
 
21
 
73
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
22
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
74
#define startADC() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
23
#define startADC() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
75
 
24
 
76
const char* recal = ", recalibration needed.";
25
const char* recal = ", recalibration needed.";
77
 
26
 
78
/*
27
/*
79
 * For each A/D conversion cycle, each analog channel is sampled a number of times
28
 * For each A/D conversion cycle, each analog channel is sampled a number of times
80
 * (see array channelsForStates), and the results for each channel are summed.
29
 * (see array channelsForStates), and the results for each channel are summed.
81
 * Here are those for the gyros and the acc. meters. They are not zero-offset.
30
 * Here are those for the gyros and the acc. meters. They are not zero-offset.
82
 * They are exported in the analog.h file - but please do not use them! The only
31
 * They are exported in the analog.h file - but please do not use them! The only
83
 * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
32
 * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
84
 * the offsets with the DAC.
33
 * the offsets with the DAC.
85
 */
34
 */
86
volatile uint16_t sensorInputs[8];
35
volatile uint16_t sensorInputs[8];
87
int16_t acc[3];
36
int16_t acc[3];
88
int16_t filteredAcc[3] = { 0,0,0 };
37
int16_t filteredAcc[3] = { 0,0,0 };
89
 
38
 
90
/*
39
/*
91
 * These 4 exported variables are zero-offset. The "PID" ones are used
40
 * These 4 exported variables are zero-offset. The "PID" ones are used
92
 * in the attitude control as rotation rates. The "ATT" ones are for
41
 * in the attitude control as rotation rates. The "ATT" ones are for
93
 * integration to angles.
42
 * integration to angles.
94
 */
43
 */
95
int16_t gyro_PID[2];
44
int16_t gyro_PID[2];
96
int16_t gyro_ATT[2];
45
int16_t gyro_ATT[2];
97
int16_t gyroD[2];
46
int16_t gyroD[2];
98
int16_t yawGyro;
47
int16_t yawGyro;
99
 
48
 
100
int32_t groundPressure;
49
int32_t groundPressure;
101
 
50
 
102
/*
51
/*
103
 * Offset values. These are the raw gyro and acc. meter sums when the copter is
52
 * Offset values. These are the raw gyro and acc. meter sums when the copter is
104
 * standing still. They are used for adjusting the gyro and acc. meter values
53
 * standing still. They are used for adjusting the gyro and acc. meter values
105
 * to be centered on zero.
54
 * to be centered on zero.
106
 */
55
 */
107
 
56
 
108
sensorOffset_t gyroOffset;
57
sensorOffset_t gyroOffset;
109
sensorOffset_t accOffset;
58
sensorOffset_t accOffset;
110
sensorOffset_t gyroAmplifierOffset;
59
sensorOffset_t gyroAmplifierOffset;
111
 
60
 
112
/*
61
/*
113
 * In the MK coordinate system, nose-down is positive and left-roll is positive.
62
 * In the MK coordinate system, nose-down is positive and left-roll is positive.
114
 * If a sensor is used in an orientation where one but not both of the axes has
63
 * If a sensor is used in an orientation where one but not both of the axes has
115
 * an opposite sign, PR_ORIENTATION_REVERSED is set to 1 (true).
64
 * an opposite sign, PR_ORIENTATION_REVERSED is set to 1 (true).
116
 * Transform:
65
 * Transform:
117
 * pitch <- pp*pitch + pr*roll
66
 * pitch <- pp*pitch + pr*roll
118
 * roll  <- rp*pitch + rr*roll
67
 * roll  <- rp*pitch + rr*roll
119
 * Not reversed, GYRO_QUADRANT:
68
 * Not reversed, GYRO_QUADRANT:
120
 * 0: pp=1, pr=0, rp=0, rr=1  // 0    degrees
69
 * 0: pp=1, pr=0, rp=0, rr=1  // 0    degrees
121
 * 1: pp=1, pr=-1,rp=1, rr=1  // +45  degrees
70
 * 1: pp=1, pr=-1,rp=1, rr=1  // +45  degrees
122
 * 2: pp=0, pr=-1,rp=1, rr=0  // +90  degrees
71
 * 2: pp=0, pr=-1,rp=1, rr=0  // +90  degrees
123
 * 3: pp=-1,pr=-1,rp=1, rr=1  // +135 degrees
72
 * 3: pp=-1,pr=-1,rp=1, rr=1  // +135 degrees
124
 * 4: pp=-1,pr=0, rp=0, rr=-1 // +180 degrees
73
 * 4: pp=-1,pr=0, rp=0, rr=-1 // +180 degrees
125
 * 5: pp=-1,pr=1, rp=-1,rr=-1 // +225 degrees
74
 * 5: pp=-1,pr=1, rp=-1,rr=-1 // +225 degrees
126
 * 6: pp=0, pr=1, rp=-1,rr=0  // +270 degrees
75
 * 6: pp=0, pr=1, rp=-1,rr=0  // +270 degrees
127
 * 7: pp=1, pr=1, rp=-1,rr=1  // +315 degrees
76
 * 7: pp=1, pr=1, rp=-1,rr=1  // +315 degrees
128
 * Reversed, GYRO_QUADRANT:
77
 * Reversed, GYRO_QUADRANT:
129
 * 0: pp=-1,pr=0, rp=0, rr=1  // 0    degrees with pitch reversed
78
 * 0: pp=-1,pr=0, rp=0, rr=1  // 0    degrees with pitch reversed
130
 * 1: pp=-1,pr=-1,rp=-1,rr=1  // +45  degrees with pitch reversed
79
 * 1: pp=-1,pr=-1,rp=-1,rr=1  // +45  degrees with pitch reversed
131
 * 2: pp=0, pr=-1,rp=-1,rr=0  // +90  degrees with pitch reversed
80
 * 2: pp=0, pr=-1,rp=-1,rr=0  // +90  degrees with pitch reversed
132
 * 3: pp=1, pr=-1,rp=-1,rr=1  // +135 degrees with pitch reversed
81
 * 3: pp=1, pr=-1,rp=-1,rr=1  // +135 degrees with pitch reversed
133
 * 4: pp=1, pr=0, rp=0, rr=-1 // +180 degrees with pitch reversed
82
 * 4: pp=1, pr=0, rp=0, rr=-1 // +180 degrees with pitch reversed
134
 * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed
83
 * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed
135
 * 6: pp=0, pr=1, rp=1, rr=0  // +270 degrees with pitch reversed
84
 * 6: pp=0, pr=1, rp=1, rr=0  // +270 degrees with pitch reversed
136
 * 7: pp=-1,pr=1, rp=1, rr=1  // +315 degrees with pitch reversed
85
 * 7: pp=-1,pr=1, rp=1, rr=1  // +315 degrees with pitch reversed
137
 */
86
 */
138
 
87
 
139
void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {
88
void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {
140
  static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1};
89
  static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1};
141
  // Pitch to Pitch part
90
  // Pitch to Pitch part
142
  int8_t xx = reverse ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant];
91
  int8_t xx = reverse ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant];
143
  // Roll to Pitch part
92
  // Roll to Pitch part
144
  int8_t xy = rotationTab[(quadrant+2)%8];
93
  int8_t xy = rotationTab[(quadrant+2)%8];
145
  // Pitch to Roll part
94
  // Pitch to Roll part
146
  int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8];
95
  int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8];
147
  // Roll to Roll part
96
  // Roll to Roll part
148
  int8_t yy = rotationTab[quadrant];
97
  int8_t yy = rotationTab[quadrant];
149
 
98
 
150
  int16_t xIn = result[0];
99
  int16_t xIn = result[0];
151
  result[0] = xx*xIn + xy*result[1];
100
  result[0] = xx*xIn + xy*result[1];
152
  result[1] = yx*xIn + yy*result[1];
101
  result[1] = yx*xIn + yy*result[1];
153
 
102
 
154
  if (quadrant & 1) {
103
  if (quadrant & 1) {
155
        // A rotation was used above, where the factors were too large by sqrt(2).
104
        // A rotation was used above, where the factors were too large by sqrt(2).
156
        // So, we multiply by 2^n/sqt(2) and right shift n bits, as to divide by sqrt(2).
105
        // So, we multiply by 2^n/sqt(2) and right shift n bits, as to divide by sqrt(2).
157
        // A suitable value for n: Sample is 11 bits. After transformation it is the sum
106
        // A suitable value for n: Sample is 11 bits. After transformation it is the sum
158
        // of 2 11 bit numbers, so 12 bits. We have 4 bits left...
107
        // of 2 11 bit numbers, so 12 bits. We have 4 bits left...
159
        result[0] = (result[0]*11) >> 4;
108
        result[0] = (result[0]*11) >> 4;
160
        result[1] = (result[1]*11) >> 4;
109
        result[1] = (result[1]*11) >> 4;
161
  }
110
  }
162
}
111
}
163
 
112
 
164
/*
113
/*
165
 * Air pressure
114
 * Air pressure
166
 */
115
 */
167
volatile uint8_t rangewidth = 105;
116
volatile uint8_t rangewidth = 105;
168
 
117
 
169
// Direct from sensor, irrespective of range.
118
// Direct from sensor, irrespective of range.
170
// volatile uint16_t rawAirPressure;
119
// volatile uint16_t rawAirPressure;
171
 
120
 
172
// Value of 2 samples, with range.
121
// Value of 2 samples, with range.
173
uint16_t simpleAirPressure;
122
uint16_t simpleAirPressure;
174
 
123
 
175
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered.
124
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered.
176
int32_t filteredAirPressure;
125
int32_t filteredAirPressure;
177
int32_t lastFilteredAirPressure;
126
int32_t lastFilteredAirPressure;
178
 
127
 
179
#define MAX_AIRPRESSURE_WINDOW_LENGTH 64
128
#define MAX_AIRPRESSURE_WINDOW_LENGTH 32
180
int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];
129
int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];
181
int32_t windowedAirPressure;
130
int32_t windowedAirPressure;
182
uint8_t windowPtr;
131
uint8_t windowPtr;
183
 
132
 
184
// Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples.
133
// Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples.
185
int32_t airPressureSum;
134
int32_t airPressureSum;
186
 
135
 
187
// The number of samples summed into airPressureSum so far.
136
// The number of samples summed into airPressureSum so far.
188
uint8_t pressureMeasurementCount;
137
uint8_t pressureMeasurementCount;
189
 
138
 
190
/*
139
/*
191
 * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
140
 * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
192
 * That is divided by 3 below, for a final 10.34 per volt.
141
 * That is divided by 3 below, for a final 10.34 per volt.
193
 * So the initial value of 100 is for 9.7 volts.
142
 * So the initial value of 100 is for 9.7 volts.
194
 */
143
 */
195
int16_t UBat = 100;
144
int16_t UBat = 100;
196
 
145
 
197
/*
146
/*
198
 * Control and status.
147
 * Control and status.
199
 */
148
 */
200
volatile uint16_t ADCycleCount = 0;
149
volatile uint16_t ADCycleCount = 0;
201
volatile uint8_t analogDataReady = 1;
150
volatile uint8_t analogDataReady = 1;
202
 
151
 
203
/*
152
/*
204
 * Experiment: Measuring vibration-induced sensor noise.
153
 * Experiment: Measuring vibration-induced sensor noise.
205
 */
154
 */
206
uint16_t gyroNoisePeak[3];
155
uint16_t gyroNoisePeak[3];
207
uint16_t accNoisePeak[3];
156
uint16_t accNoisePeak[3];
208
 
157
 
209
volatile uint8_t adState;
158
volatile uint8_t adState;
210
volatile uint8_t adChannel;
159
volatile uint8_t adChannel;
211
 
160
 
212
// ADC channels
161
// ADC channels
213
#define AD_GYRO_YAW       0
162
#define AD_GYRO_YAW       0
214
#define AD_GYRO_ROLL      1
163
#define AD_GYRO_ROLL      1
215
#define AD_GYRO_PITCH     2
164
#define AD_GYRO_PITCH     2
216
#define AD_AIRPRESSURE    3
165
#define AD_AIRPRESSURE    3
217
#define AD_UBAT           4
166
#define AD_UBAT           4
218
#define AD_ACC_Z          5
167
#define AD_ACC_Z          5
219
#define AD_ACC_ROLL       6
168
#define AD_ACC_ROLL       6
220
#define AD_ACC_PITCH      7
169
#define AD_ACC_PITCH      7
221
 
170
 
222
/*
171
/*
223
 * Table of AD converter inputs for each state.
172
 * Table of AD converter inputs for each state.
224
 * The number of samples summed for each channel is equal to
173
 * The number of samples summed for each channel is equal to
225
 * the number of times the channel appears in the array.
174
 * the number of times the channel appears in the array.
226
 * The max. number of samples that can be taken in 2 ms is:
175
 * The max. number of samples that can be taken in 2 ms is:
227
 * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control
176
 * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control
228
 * loop needs a little time between reading AD values and
177
 * loop needs a little time between reading AD values and
229
 * re-enabling ADC, the real limit is (how much?) lower.
178
 * re-enabling ADC, the real limit is (how much?) lower.
230
 * The acc. sensor is sampled even if not used - or installed
179
 * The acc. sensor is sampled even if not used - or installed
231
 * at all. The cost is not significant.
180
 * at all. The cost is not significant.
232
 */
181
 */
233
 
182
 
234
const uint8_t channelsForStates[] PROGMEM = {
183
const uint8_t channelsForStates[] PROGMEM = {
235
  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW,
184
  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW,
236
  AD_ACC_PITCH, AD_ACC_ROLL, AD_AIRPRESSURE,
185
  AD_ACC_PITCH, AD_ACC_ROLL, AD_AIRPRESSURE,
237
 
186
 
238
  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_ACC_Z, // at 8, measure Z acc.
187
  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_ACC_Z, // at 8, measure Z acc.
239
  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, // at 11, finish yaw gyro
188
  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, // at 11, finish yaw gyro
240
 
189
 
241
  AD_ACC_PITCH,   // at 12, finish pitch axis acc.
190
  AD_ACC_PITCH,   // at 12, finish pitch axis acc.
242
  AD_ACC_ROLL,    // at 13, finish roll axis acc.
191
  AD_ACC_ROLL,    // at 13, finish roll axis acc.
243
  AD_AIRPRESSURE, // at 14, finish air pressure.
192
  AD_AIRPRESSURE, // at 14, finish air pressure.
244
 
193
 
245
  AD_GYRO_PITCH,  // at 15, finish pitch gyro
194
  AD_GYRO_PITCH,  // at 15, finish pitch gyro
246
  AD_GYRO_ROLL,   // at 16, finish roll gyro
195
  AD_GYRO_ROLL,   // at 16, finish roll gyro
247
  AD_UBAT         // at 17, measure battery.
196
  AD_UBAT         // at 17, measure battery.
248
};
197
};
249
 
198
 
250
// Feature removed. Could be reintroduced later - but should work for all gyro types then.
199
// Feature removed. Could be reintroduced later - but should work for all gyro types then.
251
// uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
200
// uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
252
 
201
 
253
void analog_init(void) {
202
void analog_init(void) {
254
        uint8_t sreg = SREG;
203
        uint8_t sreg = SREG;
255
        // disable all interrupts before reconfiguration
204
        // disable all interrupts before reconfiguration
256
        cli();
205
        cli();
257
 
206
 
258
        //ADC0 ... ADC7 is connected to PortA pin 0 ... 7
207
        //ADC0 ... ADC7 is connected to PortA pin 0 ... 7
259
        DDRA = 0x00;
208
        DDRA = 0x00;
260
        PORTA = 0x00;
209
        PORTA = 0x00;
261
        // Digital Input Disable Register 0
210
        // Digital Input Disable Register 0
262
        // Disable digital input buffer for analog adc_channel pins
211
        // Disable digital input buffer for analog adc_channel pins
263
        DIDR0 = 0xFF;
212
        DIDR0 = 0xFF;
264
        // external reference, adjust data to the right
213
        // external reference, adjust data to the right
265
        ADMUX &= ~((1<<REFS1)|(1<<REFS0)|(1<<ADLAR));
214
        ADMUX &= ~((1<<REFS1)|(1<<REFS0)|(1<<ADLAR));
266
        // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
215
        // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
267
        ADMUX = (ADMUX & 0xE0);
216
        ADMUX = (ADMUX & 0xE0);
268
        //Set ADC Control and Status Register A
217
        //Set ADC Control and Status Register A
269
        //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
218
        //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
270
        ADCSRA = (1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
219
        ADCSRA = (1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
271
        //Set ADC Control and Status Register B
220
        //Set ADC Control and Status Register B
272
        //Trigger Source to Free Running Mode
221
        //Trigger Source to Free Running Mode
273
        ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0));
222
        ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0));
274
 
223
 
275
        for (uint8_t i=0; i<MAX_AIRPRESSURE_WINDOW_LENGTH; i++) {
224
        for (uint8_t i=0; i<MAX_AIRPRESSURE_WINDOW_LENGTH; i++) {
276
          airPressureWindow[i] = 0;
225
          airPressureWindow[i] = 0;
277
        }
226
        }
278
 
-
 
279
        windowedAirPressure = 0;
227
    windowedAirPressure = 0;
280
 
228
 
281
        startAnalogConversionCycle();
229
        startAnalogConversionCycle();
282
 
230
 
283
        // restore global interrupt flags
231
        // restore global interrupt flags
284
        SREG = sreg;
232
        SREG = sreg;
285
}
233
}
286
 
234
 
287
uint16_t rawGyroValue(uint8_t axis) {
235
uint16_t rawGyroValue(uint8_t axis) {
288
        return sensorInputs[AD_GYRO_PITCH-axis];
236
        return sensorInputs[AD_GYRO_PITCH-axis];
289
}
237
}
290
 
238
 
291
uint16_t rawAccValue(uint8_t axis) {
239
uint16_t rawAccValue(uint8_t axis) {
292
        return sensorInputs[AD_ACC_PITCH-axis];
240
        return sensorInputs[AD_ACC_PITCH-axis];
293
}
241
}
294
 
242
 
295
void measureNoise(const int16_t sensor,
243
void measureNoise(const int16_t sensor,
296
                volatile uint16_t* const noiseMeasurement, const uint8_t damping) {
244
                volatile uint16_t* const noiseMeasurement, const uint8_t damping) {
297
        if (sensor > (int16_t) (*noiseMeasurement)) {
245
        if (sensor > (int16_t) (*noiseMeasurement)) {
298
                *noiseMeasurement = sensor;
246
                *noiseMeasurement = sensor;
299
        } else if (-sensor > (int16_t) (*noiseMeasurement)) {
247
        } else if (-sensor > (int16_t) (*noiseMeasurement)) {
300
                *noiseMeasurement = -sensor;
248
                *noiseMeasurement = -sensor;
301
        } else if (*noiseMeasurement > damping) {
249
        } else if (*noiseMeasurement > damping) {
302
                *noiseMeasurement -= damping;
250
                *noiseMeasurement -= damping;
303
        } else {
251
        } else {
304
                *noiseMeasurement = 0;
252
                *noiseMeasurement = 0;
305
        }
253
        }
306
}
254
}
307
 
255
 
308
/*
256
/*
309
 * Min.: 0
257
 * Min.: 0
310
 * Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type.
258
 * Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type.
311
 */
259
 */
312
uint16_t getSimplePressure(int advalue) {
260
uint16_t getSimplePressure(int advalue) {
313
        uint16_t result = (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
261
        uint16_t result = (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
314
        result += (acc[Z] * (staticParams.airpressureAccZCorrection-128)) >> 10;
262
        result += (acc[Z] * (staticParams.airpressureAccZCorrection-128)) >> 10;
315
        return result;
263
        return result;
316
}
264
}
317
 
265
 
318
void startAnalogConversionCycle(void) {
266
void startAnalogConversionCycle(void) {
319
  analogDataReady = 0;
267
  analogDataReady = 0;
320
 
268
 
321
  // Stop the sampling. Cycle is over.
269
  // Stop the sampling. Cycle is over.
322
  for (uint8_t i = 0; i < 8; i++) {
270
  for (uint8_t i = 0; i < 8; i++) {
323
    sensorInputs[i] = 0;
271
    sensorInputs[i] = 0;
324
  }
272
  }
325
  adState = 0;
273
  adState = 0;
326
  adChannel = AD_GYRO_PITCH;
274
  adChannel = AD_GYRO_PITCH;
327
  ADMUX = (ADMUX & 0xE0) | adChannel;
275
  ADMUX = (ADMUX & 0xE0) | adChannel;
328
  startADC();
276
  startADC();
329
}
277
}
330
 
278
 
331
/*****************************************************
279
/*****************************************************
332
 * Interrupt Service Routine for ADC
280
 * Interrupt Service Routine for ADC
333
 * Runs at 312.5 kHz or 3.2 �s. When all states are
281
 * Runs at 312.5 kHz or 3.2 �s. When all states are
334
 * processed further conversions are stopped.
282
 * processed further conversions are stopped.
335
 *****************************************************/
283
 *****************************************************/
336
ISR(ADC_vect) {
284
ISR(ADC_vect) {
337
  sensorInputs[adChannel] += ADC;
285
  sensorInputs[adChannel] += ADC;
338
  // set up for next state.
286
  // set up for next state.
339
  adState++;
287
  adState++;
340
  if (adState < sizeof(channelsForStates)) {
288
  if (adState < sizeof(channelsForStates)) {
341
    adChannel = pgm_read_byte(&channelsForStates[adState]);
289
    adChannel = pgm_read_byte(&channelsForStates[adState]);
342
    // set adc muxer to next adChannel
290
    // set adc muxer to next adChannel
343
    ADMUX = (ADMUX & 0xE0) | adChannel;
291
    ADMUX = (ADMUX & 0xE0) | adChannel;
344
    // after full cycle stop further interrupts
292
    // after full cycle stop further interrupts
345
    startADC();
293
    startADC();
346
  } else {
294
  } else {
347
    ADCycleCount++;
295
    ADCycleCount++;
348
    analogDataReady = 1;
296
    analogDataReady = 1;
349
    // do not restart ADC converter. 
297
    // do not restart ADC converter. 
350
  }
298
  }
351
}
299
}
352
 
300
 
353
void analog_updateGyros(void) {
301
void analog_updateGyros(void) {
354
  // for various filters...
302
  // for various filters...
355
  int16_t tempOffsetGyro[2], tempGyro;
303
  int16_t tempOffsetGyro[2], tempGyro;
356
 
304
 
357
  debugOut.digital[0] &= ~DEBUG_SENSORLIMIT;
305
  debugOut.digital[0] &= ~DEBUG_SENSORLIMIT;
358
  for (uint8_t axis=0; axis<2; axis++) {
306
  for (uint8_t axis=0; axis<2; axis++) {
359
    tempGyro = rawGyroValue(axis);
307
    tempGyro = rawGyroValue(axis);
360
    /*
308
    /*
361
     * Process the gyro data for the PID controller.
309
     * Process the gyro data for the PID controller.
362
     */
310
     */
363
    // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a
311
    // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a
364
    //    gyro with a wider range, and helps counter saturation at full control.
312
    //    gyro with a wider range, and helps counter saturation at full control.
365
   
313
   
366
    if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) {
314
    if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) {
367
      if (tempGyro < SENSOR_MIN_PITCHROLL) {
315
      if (tempGyro < SENSOR_MIN_PITCHROLL) {
368
                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
316
                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
369
                tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
317
                tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
370
      } else if (tempGyro > SENSOR_MAX_PITCHROLL) {
318
      } else if (tempGyro > SENSOR_MAX_PITCHROLL) {
371
                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
319
                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
372
                tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL;
320
                tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL;
373
      }
321
      }
374
    }
322
    }
375
 
323
 
376
    // 2) Apply sign and offset, scale before filtering.
324
    // 2) Apply sign and offset, scale before filtering.
377
    tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
325
    tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
378
  }
326
  }
379
 
327
 
380
  // 2.1: Transform axes.
328
  // 2.1: Transform axes.
381
  rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
329
  rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
382
 
330
 
383
  for (uint8_t axis=0; axis<2; axis++) {
331
  for (uint8_t axis=0; axis<2; axis++) {
384
        // 3) Filter.
332
        // 3) Filter.
385
    tempOffsetGyro[axis] = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / staticParams.gyroPIDFilterConstant;
333
    tempOffsetGyro[axis] = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / staticParams.gyroPIDFilterConstant;
386
 
334
 
387
    // 4) Measure noise.
335
    // 4) Measure noise.
388
    measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
336
    measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
389
 
337
 
390
    // 5) Differential measurement.
338
    // 5) Differential measurement.
391
    gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant;
339
    gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant;
392
 
340
 
393
    // 6) Done.
341
    // 6) Done.
394
    gyro_PID[axis] = tempOffsetGyro[axis];
342
    gyro_PID[axis] = tempOffsetGyro[axis];
395
 
343
 
396
    // Prepare tempOffsetGyro for next calculation below...
344
    // Prepare tempOffsetGyro for next calculation below...
397
    tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
345
    tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
398
  }
346
  }
399
 
347
 
400
  /*
348
  /*
401
   * Now process the data for attitude angles.
349
   * Now process the data for attitude angles.
402
   */
350
   */
403
   rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
351
   rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
404
 
352
 
405
   gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
353
   gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
406
   gyro_ATT[ROLL] = tempOffsetGyro[ROLL];
354
   gyro_ATT[ROLL] = tempOffsetGyro[ROLL];
407
 
355
 
408
   debugOut.analog[22 + 0] = gyro_PID[0];
356
   debugOut.analog[22 + 0] = gyro_PID[0];
409
   debugOut.analog[22 + 1] = gyro_PID[1];
357
   debugOut.analog[22 + 1] = gyro_PID[1];
410
 
358
 
411
   debugOut.analog[24 + 0] = gyro_ATT[0];
359
   debugOut.analog[24 + 0] = gyro_ATT[0];
412
   debugOut.analog[24 + 1] = gyro_ATT[1];
360
   debugOut.analog[24 + 1] = gyro_ATT[1];
413
 
361
 
414
  // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else.
362
  // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else.
415
  // gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter;
363
  // gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter;
416
  // gyro_ATT[ROLL]  = (gyro_ATT[ROLL]  * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL])  / staticParams.attitudeGyroFilter;
364
  // gyro_ATT[ROLL]  = (gyro_ATT[ROLL]  * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL])  / staticParams.attitudeGyroFilter;
417
 
365
 
418
  // Yaw gyro.
366
  // Yaw gyro.
419
  if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW)
367
  if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW)
420
    yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
368
    yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
421
  else
369
  else
422
    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
370
    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
423
}
371
}
424
 
372
 
425
void analog_updateAccelerometers(void) {
373
void analog_updateAccelerometers(void) {
426
  // Pitch and roll axis accelerations.
374
  // Pitch and roll axis accelerations.
427
  for (uint8_t axis=0; axis<2; axis++) {
375
  for (uint8_t axis=0; axis<2; axis++) {
428
    acc[axis] = rawAccValue(axis) - accOffset.offsets[axis];
376
    acc[axis] = rawAccValue(axis) - accOffset.offsets[axis];
429
  }
377
  }
430
 
378
 
431
  rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_ACC_XY);
379
  rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_ACC_XY);
432
  for(uint8_t axis=0; axis<3; axis++) {
380
  for(uint8_t axis=0; axis<3; axis++) {
433
    filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
381
    filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
434
    measureNoise(acc[axis], &accNoisePeak[axis], 1);
382
    measureNoise(acc[axis], &accNoisePeak[axis], 1);
435
  }
383
  }
436
 
384
 
437
  // Z acc.
385
  // Z acc.
438
  if (staticParams.imuReversedFlags & 8)
386
  if (staticParams.imuReversedFlags & 8)
439
    acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
387
    acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
440
  else
388
  else
441
    acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z];
389
    acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z];
442
}
390
}
443
 
391
 
444
void analog_updateAirPressure(void) {
392
void analog_updateAirPressure(void) {
445
  static uint16_t pressureAutorangingWait = 25;
393
  static uint16_t pressureAutorangingWait = 25;
446
  uint16_t rawAirPressure;
394
  uint16_t rawAirPressure;
447
  int16_t newrange;
395
  int16_t newrange;
448
  // air pressure
396
  // air pressure
449
  if (pressureAutorangingWait) {
397
  if (pressureAutorangingWait) {
450
    //A range switch was done recently. Wait for steadying.
398
    //A range switch was done recently. Wait for steadying.
451
    pressureAutorangingWait--;
399
    pressureAutorangingWait--;
452
  } else {
400
  } else {
453
    rawAirPressure = sensorInputs[AD_AIRPRESSURE];
401
    rawAirPressure = sensorInputs[AD_AIRPRESSURE];
454
    if (rawAirPressure < MIN_RAWPRESSURE) {
402
    if (rawAirPressure < MIN_RAWPRESSURE) {
455
      // value is too low, so decrease voltage on the op amp minus input, making the value higher.
403
      // value is too low, so decrease voltage on the op amp minus input, making the value higher.
456
      newrange = OCR0A - (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (MAX_RAWPRESSURE - rawAirPressure) / (rangewidth * 2) + 1;
404
      newrange = OCR0A - (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (MAX_RAWPRESSURE - rawAirPressure) / (rangewidth * 2) + 1;
457
      if (newrange > MIN_RANGES_EXTRAPOLATION) {
405
      if (newrange > MIN_RANGES_EXTRAPOLATION) {
458
        pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; // = OCRA0 - OCRA0 +
406
        pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; // = OCRA0 - OCRA0 +
459
        OCR0A = newrange;
407
        OCR0A = newrange;
460
      } else {
408
      } else {
461
        if (OCR0A) {
409
        if (OCR0A) {
462
          OCR0A--;
410
          OCR0A--;
463
          pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;
411
          pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;
464
        }
412
        }
465
      }
413
      }
466
    } else if (rawAirPressure > MAX_RAWPRESSURE) {
414
    } else if (rawAirPressure > MAX_RAWPRESSURE) {
467
      // value is too high, so increase voltage on the op amp minus input, making the value lower.
415
      // value is too high, so increase voltage on the op amp minus input, making the value lower.
468
      // If near the end, make a limited increase
416
      // If near the end, make a limited increase
469
      newrange = OCR0A + (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4;  // (rawAirPressure - MIN_RAWPRESSURE) / (rangewidth * 2) - 1;
417
      newrange = OCR0A + (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4;  // (rawAirPressure - MIN_RAWPRESSURE) / (rangewidth * 2) - 1;
470
      if (newrange < MAX_RANGES_EXTRAPOLATION) {
418
      if (newrange < MAX_RANGES_EXTRAPOLATION) {
471
        pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR;
419
        pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR;
472
        OCR0A = newrange;
420
        OCR0A = newrange;
473
      } else {
421
      } else {
474
        if (OCR0A < 254) {
422
        if (OCR0A < 254) {
475
          OCR0A++;
423
          OCR0A++;
476
          pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;
424
          pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;
477
        }
425
        }
478
      }
426
      }
479
    }
427
    }
480
   
428
   
481
    // Even if the sample is off-range, use it.
429
    // Even if the sample is off-range, use it.
482
    simpleAirPressure = getSimplePressure(rawAirPressure);
430
    simpleAirPressure = getSimplePressure(rawAirPressure);
483
   
431
   
484
    if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
432
    if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
485
      // Danger: pressure near lower end of range. If the measurement saturates, the
433
      // Danger: pressure near lower end of range. If the measurement saturates, the
486
      // copter may climb uncontrolledly... Simulate a drastic reduction in pressure.
434
      // copter may climb uncontrolledly... Simulate a drastic reduction in pressure.
487
      debugOut.digital[1] |= DEBUG_SENSORLIMIT;
435
      debugOut.digital[1] |= DEBUG_SENSORLIMIT;
488
      airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth
436
      airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth
489
        + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION
437
        + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION
490
           * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
438
           * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
491
    } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) {
439
    } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) {
492
      // Danger: pressure near upper end of range. If the measurement saturates, the
440
      // Danger: pressure near upper end of range. If the measurement saturates, the
493
      // copter may descend uncontrolledly... Simulate a drastic increase in pressure.
441
      // copter may descend uncontrolledly... Simulate a drastic increase in pressure.
494
      debugOut.digital[1] |= DEBUG_SENSORLIMIT;
442
      debugOut.digital[1] |= DEBUG_SENSORLIMIT;
495
      airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth
443
      airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth
496
        + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION
444
        + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION
497
           * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
445
           * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
498
    } else {
446
    } else {
499
      // normal case.
447
      // normal case.
500
      // If AIRPRESSURE_OVERSAMPLING is an odd number we only want to add half the double sample.
448
      // If AIRPRESSURE_OVERSAMPLING is an odd number we only want to add half the double sample.
501
      // The 2 cases above (end of range) are ignored for this.
449
      // The 2 cases above (end of range) are ignored for this.
502
      debugOut.digital[1] &= ~DEBUG_SENSORLIMIT;
450
      debugOut.digital[1] &= ~DEBUG_SENSORLIMIT;
503
          airPressureSum += simpleAirPressure;
451
          airPressureSum += simpleAirPressure;
504
    }
452
    }
505
   
453
   
506
    // 2 samples were added.
454
    // 2 samples were added.
507
    pressureMeasurementCount += 2;
455
    pressureMeasurementCount += 2;
508
    // Assumption here: AIRPRESSURE_OVERSAMPLING is even (well we all know it's 14 haha...)
456
    // Assumption here: AIRPRESSURE_OVERSAMPLING is even (well we all know it's 14 haha...)
509
    if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING) {
457
    if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING) {
510
 
458
 
511
      // The best oversampling count is 14.5. We add a quarter of the double ADC value to get the final half.
459
      // The best oversampling count is 14.5. We add a quarter of the double ADC value to get the final half.
512
      airPressureSum += simpleAirPressure >> 2;
460
      airPressureSum += simpleAirPressure >> 2;
513
 
461
 
514
      lastFilteredAirPressure = filteredAirPressure;
462
      lastFilteredAirPressure = filteredAirPressure;
-
 
463
 
515
 
464
 
516
      if (!staticParams.airpressureWindowLength) {
465
      if (!staticParams.airpressureWindowLength) {
517
          filteredAirPressure = (filteredAirPressure * (staticParams.airpressureFilterConstant - 1)
466
          filteredAirPressure = (filteredAirPressure * (staticParams.airpressureFilterConstant - 1)
518
                          + airPressureSum + staticParams.airpressureFilterConstant / 2) / staticParams.airpressureFilterConstant;
467
                          + airPressureSum + staticParams.airpressureFilterConstant / 2) / staticParams.airpressureFilterConstant;
519
      } else {
468
      } else {
520
          // use windowed.
469
          // use windowed.
-
 
470
          windowedAirPressure += simpleAirPressure;
-
 
471
          windowedAirPressure -= airPressureWindow[windowPtr];
-
 
472
          airPressureWindow[windowPtr] = simpleAirPressure;
-
 
473
          windowPtr = (windowPtr+1) % staticParams.airpressureWindowLength;
521
          filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;
474
          filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;
522
      }
475
      }
-
 
476
 
523
      pressureMeasurementCount = airPressureSum = 0;
477
      pressureMeasurementCount = airPressureSum = 0;
524
    }
478
    }
525
    //int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];
-
 
526
    //int32_t windowedAirPressure = 0;
-
 
527
    //uint8_t windowPtr;
-
 
528
    windowedAirPressure += simpleAirPressure;
-
 
529
    windowedAirPressure -= airPressureWindow[windowPtr];
-
 
530
    airPressureWindow[windowPtr] = simpleAirPressure;
-
 
531
    windowPtr = (windowPtr+1) % staticParams.airpressureWindowLength;
-
 
532
  }
479
  }
533
}
480
}
534
 
481
 
535
void analog_updateBatteryVoltage(void) {
482
void analog_updateBatteryVoltage(void) {
536
  // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
483
  // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
537
  // This is divided by 3 --> 10.34 counts per volt.
484
  // This is divided by 3 --> 10.34 counts per volt.
538
  UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;
485
  UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;
539
}
486
}
540
 
487
 
541
void analog_update(void) {
488
void analog_update(void) {
542
  analog_updateGyros();
489
  analog_updateGyros();
543
  analog_updateAccelerometers();
490
  analog_updateAccelerometers();
544
  analog_updateAirPressure();
491
  analog_updateAirPressure();
545
  analog_updateBatteryVoltage();
492
  analog_updateBatteryVoltage();
546
}
493
}
547
 
494
 
548
void analog_setNeutral() {
495
void analog_setNeutral() {
549
  gyro_init();
496
  gyro_init();
550
 
497
 
551
  if (gyroOffset_readFromEEProm()) {
498
  if (gyroOffset_readFromEEProm()) {
552
    printf("gyro offsets invalid%s",recal);
499
    printf("gyro offsets invalid%s",recal);
553
    gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING_PITCHROLL;
500
    gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING_PITCHROLL;
554
    gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING_YAW;
501
    gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING_YAW;
555
  }
502
  }
556
 
503
 
557
  if (accOffset_readFromEEProm()) {
504
  if (accOffset_readFromEEProm()) {
558
    printf("acc. meter offsets invalid%s",recal);
505
    printf("acc. meter offsets invalid%s",recal);
559
    accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY;
506
    accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY;
560
    accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z;
507
    accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z;
561
  }
508
  }
562
 
509
 
563
  // Noise is relative to offset. So, reset noise measurements when changing offsets.
510
  // Noise is relative to offset. So, reset noise measurements when changing offsets.
564
  gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
511
  gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
565
  accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0;
512
  accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0;
566
 
513
 
567
  // Setting offset values has an influence in the analog.c ISR
514
  // Setting offset values has an influence in the analog.c ISR
568
  // Therefore run measurement for 100ms to achive stable readings
515
  // Therefore run measurement for 100ms to achive stable readings
569
  delay_ms_with_adc_measurement(100, 0);
516
  delay_ms_with_adc_measurement(100, 0);
570
 
517
 
571
  // Rough estimate. Hmm no nothing happens at calibration anyway.
518
  // Rough estimate. Hmm no nothing happens at calibration anyway.
572
  // airPressureSum = simpleAirPressure * (AIRPRESSURE_OVERSAMPLING/2);
519
  // airPressureSum = simpleAirPressure * (AIRPRESSURE_OVERSAMPLING/2);
573
  // pressureMeasurementCount = 0;
520
  // pressureMeasurementCount = 0;
574
}
521
}
575
 
522
 
576
void analog_calibrateGyros(void) {
523
void analog_calibrateGyros(void) {
577
#define GYRO_OFFSET_CYCLES 32
524
#define GYRO_OFFSET_CYCLES 32
578
  uint8_t i, axis;
525
  uint8_t i, axis;
579
  int32_t offsets[3] = { 0, 0, 0 };
526
  int32_t offsets[3] = { 0, 0, 0 };
580
  gyro_calibrate();
527
  gyro_calibrate();
581
 
528
 
582
  // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
529
  // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
583
  for (i = 0; i < GYRO_OFFSET_CYCLES; i++) {
530
  for (i = 0; i < GYRO_OFFSET_CYCLES; i++) {
584
    delay_ms_with_adc_measurement(10, 1);
531
    delay_ms_with_adc_measurement(10, 1);
585
    for (axis = PITCH; axis <= YAW; axis++) {
532
    for (axis = PITCH; axis <= YAW; axis++) {
586
      offsets[axis] += rawGyroValue(axis);
533
      offsets[axis] += rawGyroValue(axis);
587
    }
534
    }
588
  }
535
  }
589
 
536
 
590
  for (axis = PITCH; axis <= YAW; axis++) {
537
  for (axis = PITCH; axis <= YAW; axis++) {
591
    gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
538
    gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
592
 
539
 
593
    int16_t min = (512-200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL;
540
    int16_t min = (512-200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL;
594
    int16_t max = (512+200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL;
541
    int16_t max = (512+200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL;
595
    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
542
    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
596
      versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis;
543
      versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis;
597
  }
544
  }
598
 
545
 
599
  gyroOffset_writeToEEProm();  
546
  gyroOffset_writeToEEProm();  
600
  startAnalogConversionCycle();
547
  startAnalogConversionCycle();
601
}
548
}
602
 
549
 
603
/*
550
/*
604
 * Find acc. offsets for a neutral reading, and write them to EEPROM.
551
 * Find acc. offsets for a neutral reading, and write them to EEPROM.
605
 * Does not (!} update the local variables. This must be done with a
552
 * Does not (!} update the local variables. This must be done with a
606
 * call to analog_calibrate() - this always (?) is done by the caller
553
 * call to analog_calibrate() - this always (?) is done by the caller
607
 * anyway. There would be nothing wrong with updating the variables
554
 * anyway. There would be nothing wrong with updating the variables
608
 * directly from here, though.
555
 * directly from here, though.
609
 */
556
 */
610
void analog_calibrateAcc(void) {
557
void analog_calibrateAcc(void) {
611
#define ACC_OFFSET_CYCLES 32
558
#define ACC_OFFSET_CYCLES 32
612
  uint8_t i, axis;
559
  uint8_t i, axis;
613
  int32_t offsets[3] = { 0, 0, 0 };
560
  int32_t offsets[3] = { 0, 0, 0 };
614
 
561
 
615
  for (i = 0; i < ACC_OFFSET_CYCLES; i++) {
562
  for (i = 0; i < ACC_OFFSET_CYCLES; i++) {
616
    delay_ms_with_adc_measurement(10, 1);
563
    delay_ms_with_adc_measurement(10, 1);
617
    for (axis = PITCH; axis <= YAW; axis++) {
564
    for (axis = PITCH; axis <= YAW; axis++) {
618
      offsets[axis] += rawAccValue(axis);
565
      offsets[axis] += rawAccValue(axis);
619
    }
566
    }
620
  }
567
  }
621
 
568
 
622
  for (axis = PITCH; axis <= YAW; axis++) {
569
  for (axis = PITCH; axis <= YAW; axis++) {
623
    accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
570
    accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
624
    int16_t min,max;
571
    int16_t min,max;
625
    if (axis==Z) {
572
    if (axis==Z) {
626
        if (staticParams.imuReversedFlags & IMU_REVERSE_ACC_Z) {
573
        if (staticParams.imuReversedFlags & IMU_REVERSE_ACC_Z) {
627
        // TODO: This assumes a sensitivity of +/- 2g.
574
        // TODO: This assumes a sensitivity of +/- 2g.
628
                min = (256-200) * ACC_OVERSAMPLING_Z;
575
                min = (256-200) * ACC_OVERSAMPLING_Z;
629
                        max = (256+200) * ACC_OVERSAMPLING_Z;
576
                        max = (256+200) * ACC_OVERSAMPLING_Z;
630
        } else {
577
        } else {
631
        // TODO: This assumes a sensitivity of +/- 2g.
578
        // TODO: This assumes a sensitivity of +/- 2g.
632
                min = (768-200) * ACC_OVERSAMPLING_Z;
579
                min = (768-200) * ACC_OVERSAMPLING_Z;
633
                        max = (768+200) * ACC_OVERSAMPLING_Z;
580
                        max = (768+200) * ACC_OVERSAMPLING_Z;
634
        }
581
        }
635
    } else {
582
    } else {
636
        min = (512-200) * ACC_OVERSAMPLING_XY;
583
        min = (512-200) * ACC_OVERSAMPLING_XY;
637
        max = (512+200) * ACC_OVERSAMPLING_XY;
584
        max = (512+200) * ACC_OVERSAMPLING_XY;
638
    }
585
    }
639
    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) {
586
    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) {
640
      versionInfo.hardwareErrors[0] |= FC_ERROR0_ACC_X << axis;
587
      versionInfo.hardwareErrors[0] |= FC_ERROR0_ACC_X << axis;
641
    }
588
    }
642
  }
589
  }
643
 
590
 
644
  accOffset_writeToEEProm();
591
  accOffset_writeToEEProm();
645
  startAnalogConversionCycle();
592
  startAnalogConversionCycle();
646
}
593
}
647
 
594
 
648
void analog_setGround() {
595
void analog_setGround() {
649
  groundPressure = filteredAirPressure;
596
  groundPressure = filteredAirPressure;
650
}
597
}
651
 
598
 
652
int32_t analog_getHeight(void) {
599
int32_t analog_getHeight(void) {
653
  return groundPressure - filteredAirPressure;
600
  return groundPressure - filteredAirPressure;
654
}
601
}
655
 
602
 
656
int16_t analog_getDHeight(void) {
603
int16_t analog_getDHeight(void) {
657
  // dHeight = -dPressure, so here it is the old pressure minus the current, not opposite.
604
  // dHeight = -dPressure, so here it is the old pressure minus the current, not opposite.
658
  return lastFilteredAirPressure - filteredAirPressure;
605
  return lastFilteredAirPressure - filteredAirPressure;
659
}
606
}
660
 
607