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