Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1910 - 1
#ifndef _ANALOG_H
2
#define _ANALOG_H
3
#include <inttypes.h>
4
 
5
//#include "invenSense.h"
6
//#include "ENC-03_FC1.3.h"
7
//#include "ADXRS610_FC2.0.h"
8
 
9
/*
10
 About setting constants for different gyros:
11
 Main parameters are positive directions and voltage/angular speed gain.
12
 The "Positive direction" is the rotation direction around an axis where
13
 the corresponding gyro outputs a voltage > the no-rotation voltage.
14
 A gyro is considered, in this code, to be "forward" if its positive
15
 direction is:
16
 - Nose down for pitch
17
 - Left hand side down for roll
18
 - Clockwise seen from above for yaw.
19
 
20
 Setting gyro gain correctly: All sensor measurements in analog.c take
21
 place in a cycle, each cycle comprising all sensors. Some sensors are
22
 sampled more than ones, and the results added. The pitch and roll gyros
23
 are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74
24
 code.
25
 In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
26
 or 4 (other versions), offset to zero, low pass filtered and then assigned
27
 to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
28
 roll.
29
 So:
30
 
31
 gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
32
 where V is 2 for FC1.0 and 4 for all others.
33
 
34
 Assuming constant ADCValue, in the H&I code:
35
 
36
 gyro = I * ADCValue.
37
 
38
 where I is 8 for FC1.0 and 16 for all others.
39
 
40
 The relation between rotation rate and ADCValue:
41
 ADCValue [units] =
42
 rotational speed [deg/s] *
43
 gyro sensitivity [V / deg/s] *
44
 amplifier gain [units] *
45
 1024 [units] /
46
 3V full range [V]
47
 
48
 or: H is the number of steps the ADC value changes with,
49
 for a 1 deg/s change in rotational velocity:
50
 H = ADCValue [units] / rotation rate [deg/s] =
51
 gyro sensitivity [V / deg/s] *
52
 amplifier gain [units] *
53
 1024 [units] /
54
 3V full range [V]
55
 
56
 Examples:
57
 FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
58
 H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
59
 FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
60
 H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
61
 My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
62
 H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
63
 (only about half as sensitive as V1.3. But it will take about twice the
64
 rotation rate!)
65
 
66
 All together: gyro = I * H * rotation rate [units / (deg/s)].
67
 */
68
 
69
/*
70
 * A factor that the raw gyro values are multiplied by,
71
 * before being filtered and passed to the attitude module.
72
 * A value of 1 would cause a little bit of loss of precision in the
73
 * filtering (on the other hand the values are so noisy in flight that
74
 * it will not really matter - but when testing on the desk it might be
75
 * noticeable). 4 is fine for the default filtering.
76
 * Experiment: Set it to 1.
77
 */
78
#define GYRO_FACTOR_PITCHROLL 1
79
 
80
/*
81
 * How many samples are summed in one ADC loop, for pitch&roll and yaw,
82
 * respectively. This is = the number of occurences of each channel in the
83
 * channelsForStates array in analog.c.
84
 */
85
#define GYRO_SUMMATION_FACTOR_PITCHROLL 4
86
#define GYRO_SUMMATION_FACTOR_YAW 2
87
 
88
#define ACC_SUMMATION_FACTOR_PITCHROLL 2
89
#define ACC_SUMMATION_FACTOR_Z 1
90
 
91
/*
92
 Integration:
93
 The HiResXXXX values are divided by 8 (in H&I firmware) before integration.
94
 In the Killagreg rewrite of the H&I firmware, the factor 8 is called
95
 HIRES_GYRO_AMPLIFY. In this code, it is called HIRES_GYRO_INTEGRATION_FACTOR,
96
 and care has been taken that all other constants (gyro to degree factor, and
97
 180 degree flip-over detection limits) are corrected to it. Because the
98
 division by the constant takes place in the flight attitude code, the
99
 constant is there.
100
 
101
 The control loop executes every 2ms, and for each iteration
102
 gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
103
 Assuming a constant rotation rate v and a zero initial gyroIntegral
104
 (for this explanation), we get:
105
 
106
 gyroIntegral =
107
 N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =
108
 N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
109
 
110
 where N is the number of summations; N = t/2ms.
111
 
112
 For one degree of rotation: t*v = 1:
113
 
114
 gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.
115
 
116
 This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.
117
 
118
 Examples:
119
 FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304
120
 FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260
121
 My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365
122
 */
123
 
124
/*
125
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
126
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
127
 */
128
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL)
129
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW)
130
 
131
/*
132
 * Gyro saturation prevention.
133
 */
134
// How far from the end of its range a gyro is considered near-saturated.
135
#define SENSOR_MIN_PITCHROLL 32
136
// Other end of the range (calculated)
137
#define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
138
// Max. boost to add "virtually" to gyro signal at total saturation.
139
#define EXTRAPOLATION_LIMIT 2500
140
// Slope of the boost (calculated)
141
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)
142
 
143
/*
144
 * This value is subtracted from the gyro noise measurement in each iteration,
145
 * making it return towards zero.
146
 */
147
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
148
 
149
#define PITCH 0
150
#define ROLL 1
151
#define YAW 2
152
#define Z 2
153
/*
154
 * The values that this module outputs
155
 * These first 2 exported arrays are zero-offset. The "PID" ones are used
156
 * in the attitude control as rotation rates. The "ATT" ones are for
157
 * integration to angles. For the same axis, the PID and ATT variables
158
 * generally have about the same values. There are just some differences
159
 * in filtering, and when a gyro becomes near saturated.
160
 * Maybe this distinction is not really necessary.
161
 */
162
extern volatile int16_t gyro_PID[2];
163
extern volatile int16_t gyro_ATT[2];
164
extern volatile int16_t gyroD[3];
165
extern volatile int16_t yawGyro;
166
extern volatile uint16_t ADCycleCount;
167
extern volatile int16_t UBat;
168
 
169
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
170
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
171
 
172
/*
173
 * This is not really for external use - but the ENC-03 gyro modules needs it.
174
 */
175
extern volatile int16_t rawGyroSum[3];
176
 
177
/*
178
 * The acceleration values that this module outputs. They are zero based.
179
 */
180
extern volatile int16_t acc[3];
181
extern volatile int16_t filteredAcc[2];
182
// extern volatile int32_t stronglyFilteredAcc[3];
183
 
184
/*
185
 * Diagnostics: Gyro noise level because of motor vibrations. The variables
186
 * only really reflect the noise level when the copter stands still but with
187
 * its motors running.
188
 */
189
extern volatile uint16_t gyroNoisePeak[2];
190
extern volatile uint16_t accNoisePeak[2];
191
 
192
/*
193
 * Air pressure.
194
 * The sensor has a sensitivity of 46 mV/kPa.
195
 * An approximate p(h) formula is = p(h[m])[Pa] = p_0 - 11.95 * 10^-3 * h
196
 * That is: dV = 46 mV * 11.95 * 10^-3 dh = 0.5497 mV / m.
197
 * That is, with 2 * 1.024 / 3 steps per mV: 0.037 steps / m
198
 */
199
#define AIRPRESSURE_SUMMATION_FACTOR 16
200
#define AIRPRESSURE_FILTER 8
201
// Minimum A/D value before a range change is performed.
202
#define MIN_RAWPRESSURE (200 * 2)
203
// Maximum A/D value before a range change is performed.
204
#define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE)
205
 
206
#define MIN_RANGES_EXTRAPOLATION 15
207
#define MAX_RANGES_EXTRAPOLATION 240
208
 
209
#define PRESSURE_EXTRAPOLATION_COEFF 25L
210
#define AUTORANGE_WAIT_FACTOR 1
211
 
212
extern volatile uint16_t simpleAirPressure;
213
/*
214
 * At saturation, the filteredAirPressure may actually be (simulated) negative.
215
 */
216
extern volatile int32_t filteredAirPressure;
217
 
218
/*
219
 * Flag: Interrupt handler has done all A/D conversion and processing.
220
 */
221
extern volatile uint8_t analogDataReady;
222
 
223
void analog_init(void);
224
 
225
// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
226
#define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE)))
227
 
228
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
229
#define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
230
 
231
/*
232
 * "Warm" calibration: Zero-offset gyros.
233
 */
234
void analog_calibrate(void);
235
 
236
/*
237
 * "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM.
238
 */
239
void analog_calibrateAcc(void);
240
#endif //_ANALOG_H