Subversion Repositories FlightCtrl

Rev

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

Rev 2086 Rev 2089
1
#ifndef _ANALOG_H
1
#ifndef _ANALOG_H
2
#define _ANALOG_H
2
#define _ANALOG_H
3
#include <inttypes.h>
3
#include <inttypes.h>
4
#include "configuration.h"
4
#include "configuration.h"
5
 
5
 
6
/*
6
/*
7
 About setting constants for different gyros:
7
 About setting constants for different gyros:
8
 Main parameters are positive directions and voltage/angular speed gain.
8
 Main parameters are positive directions and voltage/angular speed gain.
9
 The "Positive direction" is the rotation direction around an axis where
9
 The "Positive direction" is the rotation direction around an axis where
10
 the corresponding gyro outputs a voltage > the no-rotation voltage.
10
 the corresponding gyro outputs a voltage > the no-rotation voltage.
11
 A gyro is considered, in this code, to be "forward" if its positive
11
 A gyro is considered, in this code, to be "forward" if its positive
12
 direction is:
12
 direction is:
13
 - Nose down for pitch
13
 - Nose down for pitch
14
 - Left hand side down for roll
14
 - Left hand side down for roll
15
 - Clockwise seen from above for yaw.
15
 - Clockwise seen from above for yaw.
16
 
16
 
17
 Setting gyro gain correctly: All sensor measurements in analog.c take
17
 Setting gyro gain correctly: All sensor measurements in analog.c take
18
 place in a cycle, each cycle comprising all sensors. Some sensors are
18
 place in a cycle, each cycle comprising all sensors. Some sensors are
19
 sampled more than once (oversampled), and the results added.
19
 sampled more than once (oversampled), and the results added.
20
 In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
20
 In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
21
 or 4 (other versions), offset to zero, low pass filtered and then assigned
21
 or 4 (other versions), offset to zero, low pass filtered and then assigned
22
 to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
22
 to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
23
 roll. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here.
23
 roll. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here.
24
*/
24
*/
25
#define GYRO_FACTOR_PITCHROLL 1
25
#define GYRO_FACTOR_PITCHROLL 1
26
 
26
 
27
/*
27
/*
28
 GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
28
 GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
29
 ADCValue [units] =
29
 ADCValue [units] =
30
 rotational speed [deg/s] *
30
 rotational speed [deg/s] *
31
 gyro sensitivity [V / deg/s] *
31
 gyro sensitivity [V / deg/s] *
32
 amplifier gain [units] *
32
 amplifier gain [units] *
33
 1024 [units] /
33
 1024 [units] /
34
 3V full range [V]
34
 3V full range [V]
35
 
35
 
36
 GYRO_HW_FACTOR is:
36
 GYRO_HW_FACTOR is:
37
 gyro sensitivity [V / deg/s] *
37
 gyro sensitivity [V / deg/s] *
38
 amplifier gain [units] *
38
 amplifier gain [units] *
39
 1024 [units] /
39
 1024 [units] /
40
 3V full range [V]
40
 3V full range [V]
41
 
41
 
42
 Examples:
42
 Examples:
43
 FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
43
 FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
44
 GYRO_HW_FACTOR = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
44
 GYRO_HW_FACTOR = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
45
 
45
 
46
 FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
46
 FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
47
 GYRO_HW_FACTOR = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
47
 GYRO_HW_FACTOR = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
48
 
48
 
49
 My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
49
 My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
50
 GYRO_HW_FACTOR = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
50
 GYRO_HW_FACTOR = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
51
 (only about half as sensitive as V1.3. But it will take about twice the
51
 (only about half as sensitive as V1.3. But it will take about twice the
52
 rotation rate!)
52
 rotation rate!)
53
 
53
 
54
 GYRO_HW_FACTOR is given in the makefile.
54
 GYRO_HW_FACTOR is given in the makefile.
55
*/
55
*/
56
 
56
 
57
/*
57
/*
58
 * How many samples are added in one ADC loop, for pitch&roll and yaw,
58
 * How many samples are added in one ADC loop, for pitch&roll and yaw,
59
 * respectively. This is = the number of occurences of each channel in the
59
 * respectively. This is = the number of occurences of each channel in the
60
 * channelsForStates array in analog.c.
60
 * channelsForStates array in analog.c.
61
 */
61
 */
62
#define GYRO_OVERSAMPLING_PITCHROLL 4
62
#define GYRO_OVERSAMPLING_PITCHROLL 4
63
#define GYRO_OVERSAMPLING_YAW 2
63
#define GYRO_OVERSAMPLING_YAW 2
64
 
64
 
65
#define ACC_OVERSAMPLING_XY 2
65
#define ACC_OVERSAMPLING_XY 2
66
#define ACC_OVERSAMPLING_Z 1
66
#define ACC_OVERSAMPLING_Z 1
67
 
67
 
68
/*
68
/*
69
 * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.
69
 * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.
70
 */
70
 */
71
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_PITCHROLL * GYRO_FACTOR_PITCHROLL)
71
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_PITCHROLL * GYRO_FACTOR_PITCHROLL)
72
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_YAW)
72
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_YAW)
73
 
73
 
74
/*
74
/*
75
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
75
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
76
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
76
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
77
 */
77
 */
78
 
78
 
79
/*
79
/*
80
 * Gyro saturation prevention.
80
 * Gyro saturation prevention.
81
 */
81
 */
82
// How far from the end of its range a gyro is considered near-saturated.
82
// How far from the end of its range a gyro is considered near-saturated.
83
#define SENSOR_MIN_PITCHROLL 32
83
#define SENSOR_MIN_PITCHROLL 32
84
// Other end of the range (calculated)
84
// Other end of the range (calculated)
85
#define SENSOR_MAX_PITCHROLL (GYRO_OVERSAMPLING_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
85
#define SENSOR_MAX_PITCHROLL (GYRO_OVERSAMPLING_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
86
// Max. boost to add "virtually" to gyro signal at total saturation.
86
// Max. boost to add "virtually" to gyro signal at total saturation.
87
#define EXTRAPOLATION_LIMIT 2500
87
#define EXTRAPOLATION_LIMIT 2500
88
// Slope of the boost (calculated)
88
// Slope of the boost (calculated)
89
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)
89
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)
90
 
90
 
91
/*
91
/*
92
 * This value is subtracted from the gyro noise measurement in each iteration,
92
 * This value is subtracted from the gyro noise measurement in each iteration,
93
 * making it return towards zero.
93
 * making it return towards zero.
94
 */
94
 */
95
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
95
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
96
 
96
 
97
#define PITCH 0
97
#define PITCH 0
98
#define ROLL 1
98
#define ROLL 1
99
#define YAW 2
99
#define YAW 2
100
#define Z 2
100
#define Z 2
101
/*
101
/*
102
 * The values that this module outputs
102
 * The values that this module outputs
103
 * These first 2 exported arrays are zero-offset. The "PID" ones are used
103
 * These first 2 exported arrays are zero-offset. The "PID" ones are used
104
 * in the attitude control as rotation rates. The "ATT" ones are for
104
 * in the attitude control as rotation rates. The "ATT" ones are for
105
 * integration to angles. For the same axis, the PID and ATT variables
105
 * integration to angles. For the same axis, the PID and ATT variables
106
 * generally have about the same values. There are just some differences
106
 * generally have about the same values. There are just some differences
107
 * in filtering, and when a gyro becomes near saturated.
107
 * in filtering, and when a gyro becomes near saturated.
108
 * Maybe this distinction is not really necessary.
108
 * Maybe this distinction is not really necessary.
109
 */
109
 */
110
extern int16_t gyro_PID[2];
110
extern int16_t gyro_PID[2];
111
extern int16_t gyro_ATT[2];
111
extern int16_t gyro_ATT[2];
112
#define GYRO_D_WINDOW_LENGTH 3
112
#define GYRO_D_WINDOW_LENGTH 3
113
extern int16_t gyroD[2];
113
extern int16_t gyroD[2];
114
extern int16_t yawGyro;
114
extern int16_t yawGyro;
115
extern volatile uint16_t ADCycleCount;
115
extern volatile uint16_t ADCycleCount;
116
extern int16_t UBat;
116
extern int16_t UBat;
117
 
117
 
118
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
118
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
119
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
119
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
120
 
120
 
121
extern sensorOffset_t gyroOffset;
121
extern sensorOffset_t gyroOffset;
122
extern sensorOffset_t accOffset;
122
extern sensorOffset_t accOffset;
123
extern sensorOffset_t gyroAmplifierOffset;
123
extern sensorOffset_t gyroAmplifierOffset;
124
 
124
 
125
/*
125
/*
126
 * This is not really for external use - but the ENC-03 gyro modules needs it.
126
 * This is not really for external use - but the ENC-03 gyro modules needs it.
127
 */
127
 */
128
//extern volatile int16_t rawGyroSum[3];
128
//extern volatile int16_t rawGyroSum[3];
129
 
129
 
130
/*
130
/*
131
 * The acceleration values that this module outputs. They are zero based.
131
 * The acceleration values that this module outputs. They are zero based.
132
 */
132
 */
133
extern int16_t acc[3];
133
extern int16_t acc[3];
134
extern int16_t filteredAcc[3];
134
extern int16_t filteredAcc[3];
135
// extern volatile int32_t stronglyFilteredAcc[3];
135
// extern volatile int32_t stronglyFilteredAcc[3];
136
 
136
 
137
/*
137
/*
138
 * Diagnostics: Gyro noise level because of motor vibrations. The variables
138
 * Diagnostics: Gyro noise level because of motor vibrations. The variables
139
 * only really reflect the noise level when the copter stands still but with
139
 * only really reflect the noise level when the copter stands still but with
140
 * its motors running.
140
 * its motors running.
141
 */
141
 */
142
extern uint16_t gyroNoisePeak[3];
142
extern uint16_t gyroNoisePeak[3];
143
extern uint16_t accNoisePeak[3];
143
extern uint16_t accNoisePeak[3];
144
 
144
 
145
/*
145
/*
146
 * Air pressure.
146
 * Air pressure.
147
 * The sensor has a sensitivity of 45 mV/kPa.
147
 * The sensor has a sensitivity of 45 mV/kPa.
148
 * An approximate p(h) formula is = p(h[m])[kPa] = p_0 - 11.95 * 10^-3 * h
148
 * An approximate p(h) formula is = p(h[m])[kPa] = p_0 - 11.95 * 10^-3 * h
149
 * p(h[m])[kPa] = 101.3 - 11.95 * 10^-3 * h
149
 * p(h[m])[kPa] = 101.3 - 11.95 * 10^-3 * h
150
 * 11.95 * 10^-3 * h = 101.3 - p[kPa]
150
 * 11.95 * 10^-3 * h = 101.3 - p[kPa]
151
 * h = (101.3 - p[kPa])/0.01195
151
 * h = (101.3 - p[kPa])/0.01195
152
 * That is: dV = -45 mV * 11.95 * 10^-3 dh = -0.53775 mV / m.
152
 * That is: dV = -45 mV * 11.95 * 10^-3 dh = -0.53775 mV / m.
153
 * That is, with 38.02 * 1.024 / 3 steps per mV: -7 steps / m
153
 * That is, with 38.02 * 1.024 / 3 steps per mV: -7 steps / m
154
 
154
 
155
Display pressures
155
Display pressures
156
4165 mV-->1084.7
156
4165 mV-->1084.7
157
4090 mV-->1602.4   517.7
157
4090 mV-->1602.4   517.7
158
3877 mV-->3107.8  1503.4
158
3877 mV-->3107.8  1503.4
159
 
159
 
160
4165 mV-->1419.1
160
4165 mV-->1419.1
161
3503 mV-->208.1
161
3503 mV-->208.1
162
Diff.:   1211.0
162
Diff.:   1211.0
163
 
163
 
164
Calculated  Vout = 5V(.009P-0.095) --> 5V .009P = Vout + 5V 0.095 --> P = (Vout + 5V 0.095)/(5V 0.009)
164
Calculated  Vout = 5V(.009P-0.095) --> 5V .009P = Vout + 5V 0.095 --> P = (Vout + 5V 0.095)/(5V 0.009)
165
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
165
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
166
4090 mV = 5V(0.009P-0.095)  P = 101.44 kPa  h = -11.7m   139.7m
166
4090 mV = 5V(0.009P-0.095)  P = 101.44 kPa  h = -11.7m   139.7m
167
3877 mV = 5V(0.009P-0.095)  P = 96.7   kPa  h = 385m     396.7m
167
3877 mV = 5V(0.009P-0.095)  P = 96.7   kPa  h = 385m     396.7m
168
 
168
 
169
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
169
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
170
3503 mV = 5V(0.009P-0.095)  P = 88.4   kPa  h = 384m  Diff: 1079.5m
170
3503 mV = 5V(0.009P-0.095)  P = 88.4   kPa  h = 384m  Diff: 1079.5m
171
Pressure at sea level: 101.3 kPa. voltage: 5V * (0.009P-0.095) = 4.0835V
171
Pressure at sea level: 101.3 kPa. voltage: 5V * (0.009P-0.095) = 4.0835V
172
This is OCR2 = 143.15 at 1.5V in --> simple pressure =
172
This is OCR2 = 143.15 at 1.5V in --> simple pressure =
173
*/
173
*/
174
 
174
 
175
#define AIRPRESSURE_OVERSAMPLING 14
175
#define AIRPRESSURE_OVERSAMPLING 14
176
#define AIRPRESSURE_FILTER 8
176
#define AIRPRESSURE_FILTER 8
177
// Minimum A/D value before a range change is performed.
177
// Minimum A/D value before a range change is performed.
178
#define MIN_RAWPRESSURE (200 * 2)
178
#define MIN_RAWPRESSURE (200 * 2)
179
// Maximum A/D value before a range change is performed.
179
// Maximum A/D value before a range change is performed.
180
#define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE)
180
#define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE)
181
 
181
 
182
#define MIN_RANGES_EXTRAPOLATION 15
182
#define MIN_RANGES_EXTRAPOLATION 15
183
#define MAX_RANGES_EXTRAPOLATION 240
183
#define MAX_RANGES_EXTRAPOLATION 240
184
 
184
 
185
#define PRESSURE_EXTRAPOLATION_COEFF 25L
185
#define PRESSURE_EXTRAPOLATION_COEFF 25L
186
#define AUTORANGE_WAIT_FACTOR 1
186
#define AUTORANGE_WAIT_FACTOR 1
187
 
187
 
188
#define ABS_ALTITUDE_OFFSET 108205
188
#define ABS_ALTITUDE_OFFSET 108205
189
 
189
 
190
extern uint16_t simpleAirPressure;
190
extern uint16_t simpleAirPressure;
191
/*
191
/*
192
 * At saturation, the filteredAirPressure may actually be (simulated) negative.
192
 * At saturation, the filteredAirPressure may actually be (simulated) negative.
193
 */
193
 */
194
extern int32_t filteredAirPressure;
194
extern int32_t filteredAirPressure;
195
 
195
 
196
extern int16_t magneticHeading;
196
extern int16_t magneticHeading;
197
 
197
 
198
extern uint16_t gyroActivity;
198
extern uint32_t gyroActivity;
199
 
199
 
200
/*
200
/*
201
 * Flag: Interrupt handler has done all A/D conversion and processing.
201
 * Flag: Interrupt handler has done all A/D conversion and processing.
202
 */
202
 */
203
extern volatile uint8_t analogDataReady;
203
extern volatile uint8_t analogDataReady;
204
 
204
 
205
 
205
 
206
void analog_init(void);
206
void analog_init(void);
207
 
207
 
208
/*
208
/*
209
 * This is really only for use for the ENC-03 code module, which needs to get the raw value
209
 * This is really only for use for the ENC-03 code module, which needs to get the raw value
210
 * for its calibration. The raw value should not be used for anything else.
210
 * for its calibration. The raw value should not be used for anything else.
211
 */
211
 */
212
uint16_t rawGyroValue(uint8_t axis);
212
uint16_t rawGyroValue(uint8_t axis);
213
 
213
 
214
/*
214
/*
215
 * Start the conversion cycle. It will stop automatically.
215
 * Start the conversion cycle. It will stop automatically.
216
 */
216
 */
217
void startAnalogConversionCycle(void);
217
void startAnalogConversionCycle(void);
218
 
218
 
219
/*
219
/*
220
 * Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.
220
 * Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.
221
 */
221
 */
222
void analog_update(void);
222
void analog_update(void);
223
 
223
 
224
/*
224
/*
225
 * Read gyro and acc.meter calibration from EEPROM.
225
 * Read gyro and acc.meter calibration from EEPROM.
226
 */
226
 */
227
void analog_setNeutral(void);
227
void analog_setNeutral(void);
228
 
228
 
229
/*
229
/*
230
 * Zero-offset gyros and write the calibration data to EEPROM.
230
 * Zero-offset gyros and write the calibration data to EEPROM.
231
 */
231
 */
232
void analog_calibrateGyros(void);
232
void analog_calibrateGyros(void);
233
 
233
 
234
/*
234
/*
235
 * Zero-offset accelerometers and write the calibration data to EEPROM.
235
 * Zero-offset accelerometers and write the calibration data to EEPROM.
236
 */
236
 */
237
void analog_calibrateAcc(void);
237
void analog_calibrateAcc(void);
238
 
238
 
239
 
239
 
240
void analog_setGround(void);
240
void analog_setGround(void);
241
 
241
 
242
int32_t analog_getHeight(void);
242
int32_t analog_getHeight(void);
243
int16_t analog_getDHeight(void);
243
int16_t analog_getDHeight(void);
244
 
244
 
245
#endif //_ANALOG_H
245
#endif //_ANALOG_H
246
 
246