Subversion Repositories FlightCtrl

Rev

Rev 2133 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2133 Rev 2142
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
 
25
 
26
/*
26
/*
27
 GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
27
 GYRO_HW_FACTOR is the relation between rotation rate and ADCValue:
28
 ADCValue [units] =
28
 ADCValue [units] =
29
 rotational speed [deg/s] *
29
 rotational speed [deg/s] *
30
 gyro sensitivity [V / deg/s] *
30
 gyro sensitivity [V / deg/s] *
31
 amplifier gain [units] *
31
 amplifier gain [units] *
32
 1024 [units] /
32
 1024 [units] /
33
 3V full range [V]
33
 3V full range [V]
34
 
34
 
35
 GYRO_HW_FACTOR is:
35
 GYRO_HW_FACTOR is:
36
 gyro sensitivity [V / deg/s] *
36
 gyro sensitivity [V / deg/s] *
37
 amplifier gain [units] *
37
 amplifier gain [units] *
38
 1024 [units] /
38
 1024 [units] /
39
 3V full range [V]
39
 3V full range [V]
40
 
40
 
41
 Examples:
41
 Examples:
42
 FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
42
 FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
43
 GYRO_HW_FACTOR = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
43
 GYRO_HW_FACTOR = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
44
 
44
 
45
 FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
45
 FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
46
 GYRO_HW_FACTOR = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
46
 GYRO_HW_FACTOR = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
47
 
47
 
48
 My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
48
 My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
49
 GYRO_HW_FACTOR = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
49
 GYRO_HW_FACTOR = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
50
 (only about half as sensitive as V1.3. But it will take about twice the
50
 (only about half as sensitive as V1.3. But it will take about twice the
51
 rotation rate!)
51
 rotation rate!)
52
 
52
 
53
 GYRO_HW_FACTOR is given in the makefile.
53
 GYRO_HW_FACTOR is given in the makefile.
54
*/
54
*/
55
 
55
 
56
/*
56
/*
57
 * How many samples are added in one ADC loop, for pitch&roll and yaw,
57
 * How many samples are added in one ADC loop, for pitch&roll and yaw,
58
 * respectively. This is = the number of occurences of each channel in the
58
 * respectively. This is = the number of occurences of each channel in the
59
 * channelsForStates array in analog.c.
59
 * channelsForStates array in analog.c.
60
 */
60
 */
61
#define GYRO_OVERSAMPLING 4
61
#define GYRO_OVERSAMPLING 4
62
 
62
 
63
/*
63
/*
64
 * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.
64
 * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.
65
 */
65
 */
66
#define GYRO_RATE_FACTOR (GYRO_HW_FACTOR * GYRO_OVERSAMPLING)
66
#define GYRO_RATE_FACTOR (GYRO_HW_FACTOR * GYRO_OVERSAMPLING)
67
 
67
 
68
/*
68
/*
69
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
69
 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
70
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
70
 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
71
 */
71
 */
72
 
72
 
73
/*
73
/*
74
 * Gyro saturation prevention.
74
 * Gyro saturation prevention.
75
 */
75
 */
76
// How far from the end of its range a gyro is considered near-saturated.
76
// How far from the end of its range a gyro is considered near-saturated.
77
#define SENSOR_MIN 32
77
#define SENSOR_MIN 32
78
// Other end of the range (calculated)
78
// Other end of the range (calculated)
79
#define SENSOR_MAX (GYRO_OVERSAMPLING * 1023 - SENSOR_MIN)
79
#define SENSOR_MAX (GYRO_OVERSAMPLING * 1023 - SENSOR_MIN)
80
// Max. boost to add "virtually" to gyro signal at total saturation.
80
// Max. boost to add "virtually" to gyro signal at total saturation.
81
#define EXTRAPOLATION_LIMIT 2500
81
#define EXTRAPOLATION_LIMIT 2500
82
// Slope of the boost (calculated)
82
// Slope of the boost (calculated)
83
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN)
83
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN)
84
 
84
 
85
/*
85
/*
86
 * This value is subtracted from the gyro noise measurement in each iteration,
86
 * This value is subtracted from the gyro noise measurement in each iteration,
87
 * making it return towards zero.
87
 * making it return towards zero.
88
 */
88
 */
89
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
89
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
90
 
90
 
91
#define PITCH 0
91
#define PITCH 0
92
#define ROLL 1
92
#define ROLL 1
93
#define YAW 2
93
#define YAW 2
94
//#define Z 2
94
//#define Z 2
95
/*
95
/*
96
 * The values that this module outputs
96
 * The values that this module outputs
97
 * These first 2 exported arrays are zero-offset. The "PID" ones are used
97
 * These first 2 exported arrays are zero-offset. The "PID" ones are used
98
 * in the attitude control as rotation rates. The "ATT" ones are for
98
 * in the attitude control as rotation rates. The "ATT" ones are for
99
 * integration to angles. For the same axis, the PID and ATT variables
99
 * integration to angles. For the same axis, the PID and ATT variables
100
 * generally have about the same values. There are just some differences
100
 * generally have about the same values. There are just some differences
101
 * in filtering, and when a gyro becomes near saturated.
101
 * in filtering, and when a gyro becomes near saturated.
102
 * Maybe this distinction is not really necessary.
102
 * Maybe this distinction is not really necessary.
103
 */
103
 */
104
extern int16_t gyro_PID[3];
104
extern int16_t gyro_PID[3];
105
extern int16_t gyro_ATT[3];
105
extern int16_t gyro_ATT[3];
106
extern int16_t gyroD[3];
106
extern int16_t gyroD[3];
107
 
107
 
108
#define GYRO_D_WINDOW_LENGTH 8
108
#define GYRO_D_WINDOW_LENGTH 16
109
 
109
 
110
extern uint16_t UBat;
110
extern uint16_t UBat;
111
extern uint16_t airspeedVelocity;
111
extern uint16_t airspeedVelocity;
112
 
112
 
113
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
113
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
114
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
114
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
115
extern sensorOffset_t gyroOffset;
115
extern sensorOffset_t gyroOffset;
116
extern uint16_t airpressureOffset;
116
extern uint16_t airpressureOffset;
117
 
117
 
118
/*
118
/*
119
 * This is not really for external use - but the ENC-03 gyro modules needs it.
119
 * This is not really for external use - but the ENC-03 gyro modules needs it.
120
 */
120
 */
121
//extern volatile int16_t rawGyroSum[3];
121
//extern volatile int16_t rawGyroSum[3];
122
 
122
 
123
/*
123
/*
124
 * The acceleration values that this module outputs. They are zero based.
124
 * The acceleration values that this module outputs. They are zero based.
125
 */
125
 */
126
//extern int16_t acc[3];
126
//extern int16_t acc[3];
127
//extern int16_t filteredAcc[3];
127
//extern int16_t filteredAcc[3];
128
// extern volatile int32_t stronglyFilteredAcc[3];
128
// extern volatile int32_t stronglyFilteredAcc[3];
129
 
129
 
130
/*
130
/*
131
 * Diagnostics: Gyro noise level because of motor vibrations. The variables
131
 * Diagnostics: Gyro noise level because of motor vibrations. The variables
132
 * only really reflect the noise level when the copter stands still but with
132
 * only really reflect the noise level when the copter stands still but with
133
 * its motors running.
133
 * its motors running.
134
 */
134
 */
135
extern uint16_t gyroNoisePeak[3];
135
extern uint16_t gyroNoisePeak[3];
136
 
136
 
137
/*
137
/*
138
 * Air pressure.
138
 * Air pressure.
139
 * The sensor has a sensitivity of 45 mV/kPa.
139
 * The sensor has a sensitivity of 45 mV/kPa.
140
 * An approximate p(h) formula is = p(h[m])[kPa] = p_0 - 11.95 * 10^-3 * h
140
 * An approximate p(h) formula is = p(h[m])[kPa] = p_0 - 11.95 * 10^-3 * h
141
 * p(h[m])[kPa] = 101.3 - 11.95 * 10^-3 * h
141
 * p(h[m])[kPa] = 101.3 - 11.95 * 10^-3 * h
142
 * 11.95 * 10^-3 * h = 101.3 - p[kPa]
142
 * 11.95 * 10^-3 * h = 101.3 - p[kPa]
143
 * h = (101.3 - p[kPa])/0.01195
143
 * h = (101.3 - p[kPa])/0.01195
144
 * That is: dV = -45 mV * 11.95 * 10^-3 dh = -0.53775 mV / m.
144
 * That is: dV = -45 mV * 11.95 * 10^-3 dh = -0.53775 mV / m.
145
 * That is, with 38.02 * 1.024 / 3 steps per mV: -7 steps / m
145
 * That is, with 38.02 * 1.024 / 3 steps per mV: -7 steps / m
146
 
146
 
147
Display pressures
147
Display pressures
148
4165 mV-->1084.7
148
4165 mV-->1084.7
149
4090 mV-->1602.4   517.7
149
4090 mV-->1602.4   517.7
150
3877 mV-->3107.8  1503.4
150
3877 mV-->3107.8  1503.4
151
 
151
 
152
4165 mV-->1419.1
152
4165 mV-->1419.1
153
3503 mV-->208.1
153
3503 mV-->208.1
154
Diff.:   1211.0
154
Diff.:   1211.0
155
 
155
 
156
Calculated  Vout = 5V(.009P-0.095) --> 5V .009P = Vout + 5V 0.095 --> P = (Vout + 5V 0.095)/(5V 0.009)
156
Calculated  Vout = 5V(.009P-0.095) --> 5V .009P = Vout + 5V 0.095 --> P = (Vout + 5V 0.095)/(5V 0.009)
157
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
157
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
158
4090 mV = 5V(0.009P-0.095)  P = 101.44 kPa  h = -11.7m   139.7m
158
4090 mV = 5V(0.009P-0.095)  P = 101.44 kPa  h = -11.7m   139.7m
159
3877 mV = 5V(0.009P-0.095)  P = 96.7   kPa  h = 385m     396.7m
159
3877 mV = 5V(0.009P-0.095)  P = 96.7   kPa  h = 385m     396.7m
160
 
160
 
161
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
161
4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m
162
3503 mV = 5V(0.009P-0.095)  P = 88.4   kPa  h = 384m  Diff: 1079.5m
162
3503 mV = 5V(0.009P-0.095)  P = 88.4   kPa  h = 384m  Diff: 1079.5m
163
Pressure at sea level: 101.3 kPa. voltage: 5V * (0.009P-0.095) = 4.0835V
163
Pressure at sea level: 101.3 kPa. voltage: 5V * (0.009P-0.095) = 4.0835V
164
This is OCR2 = 143.15 at 1.5V in --> simple pressure =
164
This is OCR2 = 143.15 at 1.5V in --> simple pressure =
165
*/
165
*/
166
 
166
 
167
#define AIRPRESSURE_WINDOW_LENGTH 32
167
#define AIRPRESSURE_WINDOW_LENGTH 32
168
extern uint16_t airspeedVelocity;
168
extern uint16_t airspeedVelocity;
169
 
169
 
170
/*
170
/*
171
 * Flag: Interrupt handler has done all A/D conversion and processing.
171
 * Flag: Interrupt handler has done all A/D conversion and processing.
172
 */
172
 */
173
extern volatile uint8_t analogDataReady;
173
extern volatile uint8_t analogDataReady;
174
 
174
 
175
 
175
 
176
void analog_init(void);
176
void analog_init(void);
177
 
177
 
178
/*
178
/*
179
 * This is really only for use for the ENC-03 code module, which needs to get the raw value
179
 * This is really only for use for the ENC-03 code module, which needs to get the raw value
180
 * for its calibration. The raw value should not be used for anything else.
180
 * for its calibration. The raw value should not be used for anything else.
181
 */
181
 */
182
//uint16_t rawGyroValue(uint8_t axis);
182
//uint16_t rawGyroValue(uint8_t axis);
183
 
183
 
184
/*
184
/*
185
 * Start the conversion cycle. It will stop automatically.
185
 * Start the conversion cycle. It will stop automatically.
186
 */
186
 */
187
void startAnalogConversionCycle(void);
187
void startAnalogConversionCycle(void);
188
 
188
 
189
/*
189
/*
190
 * Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.
190
 * Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.
191
 */
191
 */
192
void analog_update(void);
192
void analog_update(void);
193
 
193
 
194
/*
194
/*
195
 * Read gyro and acc.meter calibration from EEPROM.
195
 * Read gyro and acc.meter calibration from EEPROM.
196
 */
196
 */
197
void analog_setNeutral(void);
197
void analog_setNeutral(void);
198
 
198
 
199
/*
199
/*
200
 * Zero-offset gyros and write the calibration data to EEPROM.
200
 * Zero-offset gyros and write the calibration data to EEPROM.
201
 */
201
 */
202
void analog_calibrate(void);
202
void analog_calibrate(void);
203
 
203
 
204
#endif //_ANALOG_H
204
#endif //_ANALOG_H
205
 
205