Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1612 dongfang 1
#ifndef _ANALOG_H
2
#define _ANALOG_H
3
#include <inttypes.h>
1965 - 4
#include "configuration.h"
1612 dongfang 5
 
6
/*
1645 - 7
 * How much low pass filtering to apply for gyro_PID.
1612 dongfang 8
 * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
9
 * Temporarily replaced by userparam-configurable variable.
10
 */
1646 - 11
// #define GYROS_PID_FILTER 1
1612 dongfang 12
 
13
/*
1645 - 14
 * How much low pass filtering to apply for gyro_ATT.
1612 dongfang 15
 * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
16
 * Temporarily replaced by userparam-configurable variable.
17
 */
1646 - 18
// #define GYROS_ATT_FILTER 1
19
// #define ACC_FILTER 4
1612 dongfang 20
 
21
/*
1821 - 22
 About setting constants for different gyros:
23
 Main parameters are positive directions and voltage/angular speed gain.
24
 The "Positive direction" is the rotation direction around an axis where
25
 the corresponding gyro outputs a voltage > the no-rotation voltage.
26
 A gyro is considered, in this code, to be "forward" if its positive
27
 direction is:
28
 - Nose down for pitch
29
 - Left hand side down for roll
30
 - Clockwise seen from above for yaw.
2018 - 31
 
1821 - 32
 Setting gyro gain correctly: All sensor measurements in analog.c take
33
 place in a cycle, each cycle comprising all sensors. Some sensors are
34
 sampled more than ones, and the results added. The pitch and roll gyros
35
 are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74
36
 code.
37
 In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
38
 or 4 (other versions), offset to zero, low pass filtered and then assigned
39
 to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
40
 roll.
41
 So:
42
 
43
 gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
44
 where V is 2 for FC1.0 and 4 for all others.
45
 
46
 Assuming constant ADCValue, in the H&I code:
1612 dongfang 47
 
1821 - 48
 gyro = I * ADCValue.
1612 dongfang 49
 
1821 - 50
 where I is 8 for FC1.0 and 16 for all others.
1612 dongfang 51
 
1821 - 52
 The relation between rotation rate and ADCValue:
53
 ADCValue [units] =
54
 rotational speed [deg/s] *
55
 gyro sensitivity [V / deg/s] *
56
 amplifier gain [units] *
57
 1024 [units] /
58
 3V full range [V]
1612 dongfang 59
 
1821 - 60
 or: H is the number of steps the ADC value changes with,
61
 for a 1 deg/s change in rotational velocity:
62
 H = ADCValue [units] / rotation rate [deg/s] =
63
 gyro sensitivity [V / deg/s] *
64
 amplifier gain [units] *
65
 1024 [units] /
66
 3V full range [V]
1612 dongfang 67
 
1821 - 68
 Examples:
69
 FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
70
 H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
71
 FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
72
 H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
73
 My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
74
 H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
75
 (only about half as sensitive as V1.3. But it will take about twice the
76
 rotation rate!)
1612 dongfang 77
 
1821 - 78
 All together: gyro = I * H * rotation rate [units / (deg/s)].
79
 */
1612 dongfang 80
 
81
/*
82
 * A factor that the raw gyro values are multiplied by,
1645 - 83
 * before being filtered and passed to the attitude module.
1612 dongfang 84
 * A value of 1 would cause a little bit of loss of precision in the
85
 * filtering (on the other hand the values are so noisy in flight that
86
 * it will not really matter - but when testing on the desk it might be
87
 * noticeable). 4 is fine for the default filtering.
1645 - 88
 * Experiment: Set it to 1.
1612 dongfang 89
 */
1874 - 90
#define GYRO_FACTOR_PITCHROLL 1
1612 dongfang 91
 
92
/*
93
 * How many samples are summed in one ADC loop, for pitch&roll and yaw,
94
 * respectively. This is = the number of occurences of each channel in the
95
 * channelsForStates array in analog.c.
96
 */
2019 - 97
#define GYRO_OVERSAMPLING_PITCHROLL 4
98
#define GYRO_OVERSAMPLING_YAW 2
1612 dongfang 99
 
2019 - 100
#define ACC_OVERSAMPLING_XY 2
101
#define ACC_OVERSAMPLING_Z 1
1646 - 102
 
1612 dongfang 103
/*
1821 - 104
 Integration:
105
 The HiResXXXX values are divided by 8 (in H&I firmware) before integration.
106
 In the Killagreg rewrite of the H&I firmware, the factor 8 is called
107
 HIRES_GYRO_AMPLIFY. In this code, it is called HIRES_GYRO_INTEGRATION_FACTOR,
108
 and care has been taken that all other constants (gyro to degree factor, and
109
 180 degree flip-over detection limits) are corrected to it. Because the
110
 division by the constant takes place in the flight attitude code, the
111
 constant is there.
1612 dongfang 112
 
2035 - 113
 The control loop executes at 488Hz, and for each iteration
1821 - 114
 gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
115
 Assuming a constant rotation rate v and a zero initial gyroIntegral
116
 (for this explanation), we get:
1612 dongfang 117
 
1821 - 118
 gyroIntegral =
119
 N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =
120
 N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
1612 dongfang 121
 
2035 - 122
 where N is the number of summations; N = t*488.
1612 dongfang 123
 
1821 - 124
 For one degree of rotation: t*v = 1:
1612 dongfang 125
 
2035 - 126
 gyroIntegralXXXX = t * 488 * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.
1612 dongfang 127
 
1821 - 128
 This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.
129
 
130
 Examples:
2035 - 131
 FC1.3: I=4, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=1 --> integralDegreeFactor = 2545
132
 FC2.0: I=4, H=1.2288, HIRES_GYRO_INTEGRATION_FACTOR=1 --> integralDegreeFactor = 2399
133
 My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1333
1821 - 134
 */
135
 
1612 dongfang 136
/*
1645 - 137
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
1612 dongfang 138
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
139
 */
2019 - 140
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_PITCHROLL * GYRO_FACTOR_PITCHROLL)
141
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_YAW)
1612 dongfang 142
 
143
/*
1645 - 144
 * Gyro saturation prevention.
145
 */
146
// How far from the end of its range a gyro is considered near-saturated.
147
#define SENSOR_MIN_PITCHROLL 32
148
// Other end of the range (calculated)
2019 - 149
#define SENSOR_MAX_PITCHROLL (GYRO_OVERSAMPLING_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
1645 - 150
// Max. boost to add "virtually" to gyro signal at total saturation.
151
#define EXTRAPOLATION_LIMIT 2500
152
// Slope of the boost (calculated)
153
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)
154
 
155
/*
1612 dongfang 156
 * This value is subtracted from the gyro noise measurement in each iteration,
157
 * making it return towards zero.
158
 */
159
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
160
 
1645 - 161
#define PITCH 0
162
#define ROLL 1
1646 - 163
#define YAW 2
164
#define Z 2
1612 dongfang 165
/*
166
 * The values that this module outputs
1645 - 167
 * These first 2 exported arrays are zero-offset. The "PID" ones are used
168
 * in the attitude control as rotation rates. The "ATT" ones are for
169
 * integration to angles. For the same axis, the PID and ATT variables
170
 * generally have about the same values. There are just some differences
171
 * in filtering, and when a gyro becomes near saturated.
172
 * Maybe this distinction is not really necessary.
1612 dongfang 173
 */
2015 - 174
extern int16_t gyro_PID[2];
175
extern int16_t gyro_ATT[2];
176
extern int16_t gyroD[2];
177
extern int16_t yawGyro;
1612 dongfang 178
extern volatile uint16_t ADCycleCount;
2015 - 179
extern int16_t UBat;
1612 dongfang 180
 
1775 - 181
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
1869 - 182
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
1775 - 183
 
1969 - 184
extern sensorOffset_t gyroOffset;
185
extern sensorOffset_t accOffset;
186
extern sensorOffset_t gyroAmplifierOffset;
1960 - 187
 
1612 dongfang 188
/*
189
 * This is not really for external use - but the ENC-03 gyro modules needs it.
190
 */
2015 - 191
//extern volatile int16_t rawGyroSum[3];
1612 dongfang 192
 
193
/*
1645 - 194
 * The acceleration values that this module outputs. They are zero based.
1612 dongfang 195
 */
2015 - 196
extern int16_t acc[3];
197
extern int16_t filteredAcc[3];
1872 - 198
// extern volatile int32_t stronglyFilteredAcc[3];
1612 dongfang 199
 
200
/*
1775 - 201
 * Diagnostics: Gyro noise level because of motor vibrations. The variables
202
 * only really reflect the noise level when the copter stands still but with
203
 * its motors running.
204
 */
2015 - 205
extern uint16_t gyroNoisePeak[3];
206
extern uint16_t accNoisePeak[3];
1775 - 207
 
208
/*
209
 * Air pressure.
1961 - 210
 * The sensor has a sensitivity of 45 mV/kPa.
1970 - 211
 * An approximate p(h) formula is = p(h[m])[kPa] = p_0 - 11.95 * 10^-3 * h
212
 * p(h[m])[kPa] = 101.3 - 11.95 * 10^-3 * h
213
 * 11.95 * 10^-3 * h = 101.3 - p[kPa]
214
 * h = (101.3 - p[kPa])/0.01195
215
 * That is: dV = -45 mV * 11.95 * 10^-3 dh = -0.53775 mV / m.
216
 * That is, with 38.02 * 1.024 / 3 steps per mV: -7 steps / m
217
 
218
Display pressures
219
4165 mV-->1084.7
220
4090 mV-->1602.4   517.7
221
3877 mV-->3107.8  1503.4
222
 
223
4165 mV-->1419.1
224
3503 mV-->208.1
225
Diff.:   1211.0
226
 
227
Calculated  Vout = 5V(.009P-0.095) --> 5V .009P = Vout + 5V 0.095 --> P = (Vout + 5V 0.095)/(5V 0.009)
228
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
229
4090 mV = 5V(0.009P-0.095)  P = 101.44 kPa  h = -11.7m   139.7m
230
3877 mV = 5V(0.009P-0.095)  P = 96.7   kPa  h = 385m     396.7m
231
 
232
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
233
3503 mV = 5V(0.009P-0.095)  P = 88.4   kPa  h = 384m  Diff: 1079.5m
234
Pressure at sea level: 101.3 kPa. voltage: 5V * (0.009P-0.095) = 4.0835V
235
This is OCR2 = 143.15 at 1.5V in --> simple pressure =
236
*/
237
 
2019 - 238
#define AIRPRESSURE_OVERSAMPLING 14
1775 - 239
#define AIRPRESSURE_FILTER 8
240
// Minimum A/D value before a range change is performed.
241
#define MIN_RAWPRESSURE (200 * 2)
242
// Maximum A/D value before a range change is performed.
243
#define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE)
244
 
1796 - 245
#define MIN_RANGES_EXTRAPOLATION 15
246
#define MAX_RANGES_EXTRAPOLATION 240
1775 - 247
 
248
#define PRESSURE_EXTRAPOLATION_COEFF 25L
249
#define AUTORANGE_WAIT_FACTOR 1
250
 
1970 - 251
#define ABS_ALTITUDE_OFFSET 108205
252
 
2015 - 253
extern uint16_t simpleAirPressure;
1775 - 254
/*
255
 * At saturation, the filteredAirPressure may actually be (simulated) negative.
256
 */
2015 - 257
extern int32_t filteredAirPressure;
1775 - 258
 
259
/*
1612 dongfang 260
 * Flag: Interrupt handler has done all A/D conversion and processing.
261
 */
262
extern volatile uint8_t analogDataReady;
263
 
264
void analog_init(void);
265
 
1952 - 266
/*
2015 - 267
 * This is really only for use for the ENC-03 code module, which needs to get the raw value
268
 * for its calibration. The raw value should not be used for anything else.
269
 */
270
uint16_t rawGyroValue(uint8_t axis);
271
 
272
/*
1952 - 273
 * Start the conversion cycle. It will stop automatically.
274
 */
275
void startAnalogConversionCycle(void);
1612 dongfang 276
 
1952 - 277
/*
278
 * Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.
279
 */
1955 - 280
void analog_update(void);
1612 dongfang 281
 
282
/*
1961 - 283
 * Read gyro and acc.meter calibration from EEPROM.
1612 dongfang 284
 */
1961 - 285
void analog_setNeutral(void);
1612 dongfang 286
 
287
/*
1961 - 288
 * Zero-offset gyros and write the calibration data to EEPROM.
1612 dongfang 289
 */
1961 - 290
void analog_calibrateGyros(void);
291
 
292
/*
293
 * Zero-offset accelerometers and write the calibration data to EEPROM.
294
 */
1612 dongfang 295
void analog_calibrateAcc(void);
2033 - 296
 
297
 
2035 - 298
void analog_setGround(void);
2033 - 299
 
300
int32_t analog_getHeight(void);
301
int16_t analog_getDHeight(void);
302
 
1612 dongfang 303
#endif //_ANALOG_H