Rev 1617 | Rev 1645 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1617 | Rev 1634 | ||
---|---|---|---|
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 | 4 | ||
5 | #include "invenSense.h" |
5 | //#include "invenSense.h" |
6 | // #include "ENC-03_FC1.3.h" |
6 | #include "ENC-03_FC1.3.h" |
7 | 7 | ||
8 | 8 | ||
9 | /* |
9 | /* |
10 | * How much low pass filtering to apply for hiResPitchGyro and hiResRollGyro. |
10 | * How much low pass filtering to apply for hiResPitchGyro and hiResRollGyro. |
11 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
11 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
12 | * Temporarily replaced by userparam-configurable variable. |
12 | * Temporarily replaced by userparam-configurable variable. |
13 | */ |
13 | */ |
14 | //#define GYROS_FIRSTORDERFILTER 2 |
14 | //#define GYROS_FIRSTORDERFILTER 2 |
15 | 15 | ||
16 | /* |
16 | /* |
17 | * How much low pass filtering to apply for filteredHiResPitchGyro and filteredHiResRollGyro. |
17 | * How much low pass filtering to apply for filteredHiResPitchGyro and filteredHiResRollGyro. |
18 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
18 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
19 | * Temporarily replaced by userparam-configurable variable. |
19 | * Temporarily replaced by userparam-configurable variable. |
20 | */ |
20 | */ |
21 | //#define GYROS_SECONDORDERFILTER 2 |
21 | //#define GYROS_SECONDORDERFILTER 2 |
22 | 22 | ||
23 | // Temporarily replaced by userparam-configurable variable. |
23 | // Temporarily replaced by userparam-configurable variable. |
24 | //#define ACC_FILTER 4 |
24 | //#define ACC_FILTER 4 |
25 | 25 | ||
26 | /* |
26 | /* |
27 | About setting constants right for different gyros: |
27 | About setting constants right for different gyros: |
28 | Main parameters are positive directions and voltage/angular speed gain. |
28 | Main parameters are positive directions and voltage/angular speed gain. |
29 | The "Positive direction" is the rotation direction around an axis where |
29 | The "Positive direction" is the rotation direction around an axis where |
30 | the corresponding gyro gives a voltage > the no-rotation voltage. |
30 | the corresponding gyro gives a voltage > the no-rotation voltage. |
31 | A gyro is considered, in this code, to be "forward" if its positive |
31 | A gyro is considered, in this code, to be "forward" if its positive |
32 | direction is the same as in FC1.0/1.1/1.2/1.3, and reverse otherwise. |
32 | direction is the same as in FC1.0/1.1/1.2/1.3, and reverse otherwise. |
33 | Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and |
33 | Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and |
34 | GYRO_REVERSE_PITCH #define's if the respective gyros are reverse. |
34 | GYRO_REVERSE_PITCH #define's if the respective gyros are reverse. |
35 | |
35 | |
36 | Setting gyro gain correctly: All sensor measurements in analog.c take |
36 | Setting gyro gain correctly: All sensor measurements in analog.c take |
37 | place in a cycle, each cycle comprising all sensors. Some sensors are |
37 | place in a cycle, each cycle comprising all sensors. Some sensors are |
38 | sampled more than ones, and the results added. The pitch and roll gyros |
38 | sampled more than ones, and the results added. The pitch and roll gyros |
39 | are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74 |
39 | are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74 |
40 | code. |
40 | code. |
41 | In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0) |
41 | In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0) |
42 | or 4 (other versions), offset to zero, low pass filtered and then assigned |
42 | or 4 (other versions), offset to zero, low pass filtered and then assigned |
43 | to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or |
43 | to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or |
44 | roll. |
44 | roll. |
45 | So: |
45 | So: |
46 | |
46 | |
47 | HiResXXXX = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4), |
47 | HiResXXXX = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4), |
48 | where V is 2 for FC1.0 and 4 for all others. |
48 | where V is 2 for FC1.0 and 4 for all others. |
49 | |
49 | |
50 | Assuming constant ADCValue, in the H&I code: |
50 | Assuming constant ADCValue, in the H&I code: |
51 | |
51 | |
52 | HiResXXXX = I * ADCValue. |
52 | HiResXXXX = I * ADCValue. |
53 | 53 | ||
54 | where I is 8 for FC1.0 and 16 for all others. |
54 | where I is 8 for FC1.0 and 16 for all others. |
55 | 55 | ||
56 | The relation between rotation rate and ADCValue: |
56 | The relation between rotation rate and ADCValue: |
57 | ADCValue [units] = |
57 | ADCValue [units] = |
58 | rotational speed [deg/s] * |
58 | rotational speed [deg/s] * |
59 | gyro sensitivity [V / deg/s] * |
59 | gyro sensitivity [V / deg/s] * |
60 | amplifier gain [units] * |
60 | amplifier gain [units] * |
61 | 1024 [units] / |
61 | 1024 [units] / |
62 | 3V full range [V] |
62 | 3V full range [V] |
63 | 63 | ||
64 | or: H is the number of steps the ADC value changes with, |
64 | or: H is the number of steps the ADC value changes with, |
65 | for a 1 deg/s change in rotational velocity: |
65 | for a 1 deg/s change in rotational velocity: |
66 | H = ADCValue [units] / rotation rate [deg/s] = |
66 | H = ADCValue [units] / rotation rate [deg/s] = |
67 | gyro sensitivity [V / deg/s] * |
67 | gyro sensitivity [V / deg/s] * |
68 | amplifier gain [units] * |
68 | amplifier gain [units] * |
69 | 1024 [units] / |
69 | 1024 [units] / |
70 | 3V full range [V] |
70 | 3V full range [V] |
71 | 71 | ||
72 | Examples: |
72 | Examples: |
73 | FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7: |
73 | FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7: |
74 | H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s). |
74 | H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s). |
75 | FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers: |
75 | FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers: |
76 | H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s). |
76 | H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s). |
77 | My InvenSense copter has 2mV/deg/s gyros and no amplifiers: |
77 | My InvenSense copter has 2mV/deg/s gyros and no amplifiers: |
78 | H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s) |
78 | H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s) |
79 | (only about half as sensitive as V1.3. But it will take about twice the |
79 | (only about half as sensitive as V1.3. But it will take about twice the |
80 | rotation rate!) |
80 | rotation rate!) |
81 | 81 | ||
82 | All together: HiResXXXX = I * H * rotation rate [units / (deg/s)]. |
82 | All together: HiResXXXX = I * H * rotation rate [units / (deg/s)]. |
83 | */ |
83 | */ |
84 | 84 | ||
85 | /* |
85 | /* |
86 | * A factor that the raw gyro values are multiplied by, |
86 | * A factor that the raw gyro values are multiplied by, |
87 | * before being zero-offset, filtered and passed to the attitude module. |
87 | * before being zero-offset, filtered and passed to the attitude module. |
88 | * A value of 1 would cause a little bit of loss of precision in the |
88 | * A value of 1 would cause a little bit of loss of precision in the |
89 | * filtering (on the other hand the values are so noisy in flight that |
89 | * filtering (on the other hand the values are so noisy in flight that |
90 | * it will not really matter - but when testing on the desk it might be |
90 | * it will not really matter - but when testing on the desk it might be |
91 | * noticeable). 4 is fine for the default filtering. |
91 | * noticeable). 4 is fine for the default filtering. |
92 | */ |
92 | */ |
93 | #define GYRO_FACTOR_PITCHROLL 4 |
93 | #define GYRO_FACTOR_PITCHROLL 4 |
94 | 94 | ||
95 | /* |
95 | /* |
96 | * How many samples are summed in one ADC loop, for pitch&roll and yaw, |
96 | * How many samples are summed in one ADC loop, for pitch&roll and yaw, |
97 | * respectively. This is = the number of occurences of each channel in the |
97 | * respectively. This is = the number of occurences of each channel in the |
98 | * channelsForStates array in analog.c. |
98 | * channelsForStates array in analog.c. |
99 | */ |
99 | */ |
100 | #define GYRO_SUMMATION_FACTOR_PITCHROLL 4 |
100 | #define GYRO_SUMMATION_FACTOR_PITCHROLL 4 |
101 | #define GYRO_SUMMATION_FACTOR_YAW 2 |
101 | #define GYRO_SUMMATION_FACTOR_YAW 2 |
102 | 102 | ||
103 | /* |
103 | /* |
104 | Integration: |
104 | Integration: |
105 | The HiResXXXX values are divided by 8 (in H&I firmware) before 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 |
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, |
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 |
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 |
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 |
110 | division by the constant takes place in the flight attitude code, the |
111 | constant is there. |
111 | constant is there. |
112 | 112 | ||
113 | The control loop executes every 2ms, and for each iteration |
113 | The control loop executes every 2ms, and for each iteration |
114 | HiResXXXX is added to gyroIntegralXXXX. |
114 | HiResXXXX is added to gyroIntegralXXXX. |
115 | Assuming a constant rotation rate v and an initial gyroIntegralXXXX (for this |
115 | Assuming a constant rotation rate v and an initial gyroIntegralXXXX (for this |
116 | explanation), we get: |
116 | explanation), we get: |
117 | |
117 | |
118 | gyroIntegralXXXX = |
118 | gyroIntegralXXXX = |
119 | N * HiResXXXX / HIRES_GYRO_INTEGRATION_FACTOR = |
119 | N * HiResXXXX / HIRES_GYRO_INTEGRATION_FACTOR = |
120 | N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
120 | N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
121 | |
121 | |
122 | where N is the number of summations; N = t/2ms. |
122 | where N is the number of summations; N = t/2ms. |
123 | 123 | ||
124 | For one degree of rotation: t*v = 1: |
124 | For one degree of rotation: t*v = 1: |
125 | 125 | ||
126 | gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR. |
126 | gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR. |
127 | 127 | ||
128 | This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor. |
128 | This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor. |
129 | 129 | ||
130 | Examples: |
130 | Examples: |
131 | FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304 |
131 | FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304 |
132 | FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260 |
132 | FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260 |
133 | My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365 |
133 | My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365 |
134 | */ |
134 | */ |
135 | 135 | ||
136 | /* |
136 | /* |
137 | * The value of hiResXXXX for one deg/s = The hardware factor H * the number of samples * multiplier factor. |
137 | * The value of hiResXXXX for one deg/s = The hardware factor H * the number of samples * multiplier factor. |
138 | * Will be about 10 or so for InvenSense, and about 33 for ADXRS610. |
138 | * Will be about 10 or so for InvenSense, and about 33 for ADXRS610. |
139 | */ |
139 | */ |
140 | #define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL) |
140 | #define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL) |
141 | #define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW) |
141 | #define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW) |
142 | 142 | ||
143 | /* |
143 | /* |
144 | * This value is subtracted from the gyro noise measurement in each iteration, |
144 | * This value is subtracted from the gyro noise measurement in each iteration, |
145 | * making it return towards zero. |
145 | * making it return towards zero. |
146 | */ |
146 | */ |
147 | #define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
147 | #define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
148 | 148 | ||
149 | /* |
149 | /* |
150 | * The values that this module outputs |
150 | * The values that this module outputs |
151 | */ |
151 | */ |
152 | extern volatile int16_t hiResPitchGyro, hiResRollGyro; |
152 | extern volatile int16_t hiResPitchGyro, hiResRollGyro; |
153 | extern volatile int16_t filteredHiResPitchGyro, filteredHiResRollGyro; |
153 | extern volatile int16_t filteredHiResPitchGyro, filteredHiResRollGyro; |
154 | extern volatile int16_t pitchGyroD, rollGyroD; |
154 | extern volatile int16_t pitchGyroD, rollGyroD; |
155 | extern volatile uint16_t ADCycleCount; |
155 | extern volatile uint16_t ADCycleCount; |
156 | extern volatile int16_t UBat; |
156 | extern volatile int16_t UBat; |
157 | extern volatile int16_t yawGyro; |
157 | extern volatile int16_t yawGyro; |
158 | 158 | ||
159 | /* |
159 | /* |
160 | * This is not really for external use - but the ENC-03 gyro modules needs it. |
160 | * This is not really for external use - but the ENC-03 gyro modules needs it. |
161 | */ |
161 | */ |
162 | extern volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum; |
162 | extern volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum; |
163 | 163 | ||
164 | /* |
164 | /* |
165 | * The acceleration values that this module outputs |
165 | * The acceleration values that this module outputs |
166 | */ |
166 | */ |
167 | extern volatile int16_t pitchAxisAcc, rollAxisAcc, ZAxisAcc; |
167 | extern volatile int16_t pitchAxisAcc, rollAxisAcc, ZAxisAcc; |
168 | extern volatile int16_t filteredPitchAxisAcc, filteredRollAxisAcc; |
168 | extern volatile int16_t filteredPitchAxisAcc, filteredRollAxisAcc; |
169 | 169 | ||
170 | // Only for debugging! Not to be exported! Remove when finished. |
170 | // Only for debugging! Not to be exported! Remove when finished. |
171 | // extern volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset; |
171 | // extern volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset; |
172 | 172 | ||
173 | // Air pressure measurement not supported right now. |
173 | // Air pressure measurement not supported right now. |
174 | // extern volatile int32_t AirPressure; |
174 | // extern volatile int32_t AirPressure; |
175 | // extern volatile int16_t HeightD; |
175 | // extern volatile int16_t HeightD; |
176 | // extern volatile uint16_t ReadingAirPressure; |
176 | // extern volatile uint16_t ReadingAirPressure; |
177 | // extern volatile int16_t StartAirPressure; |
177 | // extern volatile int16_t StartAirPressure; |
178 | // extern uint8_t PressureSensorOffset; |
178 | // extern uint8_t PressureSensorOffset; |
179 | // extern int8_t ExpandBaro; |
179 | // extern int8_t ExpandBaro; |
180 | 180 | ||
181 | /* |
181 | /* |
182 | * Flag: Interrupt handler has done all A/D conversion and processing. |
182 | * Flag: Interrupt handler has done all A/D conversion and processing. |
183 | */ |
183 | */ |
184 | extern volatile uint8_t analogDataReady; |
184 | extern volatile uint8_t analogDataReady; |
185 | 185 | ||
186 | // Diagnostics: Gyro noise level because of motor vibrations. The variables |
186 | // Diagnostics: Gyro noise level because of motor vibrations. The variables |
187 | // only really reflect the noise level when the copter stands still but with |
187 | // only really reflect the noise level when the copter stands still but with |
188 | // its motors running. |
188 | // its motors running. |
189 | extern volatile uint16_t pitchGyroNoisePeak, rollGyroNoisePeak; |
189 | extern volatile uint16_t pitchGyroNoisePeak, rollGyroNoisePeak; |
190 | extern volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak; |
190 | extern volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak; |
191 | 191 | ||
192 | // void SearchAirPressureOffset(void); |
192 | // void SearchAirPressureOffset(void); |
193 | 193 | ||
194 | void analog_init(void); |
194 | void analog_init(void); |
195 | 195 | ||
196 | // clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
196 | // clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
197 | #define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE))) |
197 | #define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE))) |
198 | 198 | ||
199 | // set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
199 | // set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
200 | #define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE)) |
200 | #define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE)) |
201 | 201 | ||
202 | /* |
202 | /* |
203 | * "Warm" calibration: Zero-offset gyros. |
203 | * "Warm" calibration: Zero-offset gyros. |
204 | */ |
204 | */ |
205 | void analog_calibrate(void); |
205 | void analog_calibrate(void); |
206 | 206 | ||
207 | /* |
207 | /* |
208 | * "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM. |
208 | * "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM. |
209 | */ |
209 | */ |
210 | void analog_calibrateAcc(void); |
210 | void analog_calibrateAcc(void); |
211 | #endif //_ANALOG_H |
211 | #endif //_ANALOG_H |
212 | 212 |