Rev 1634 | Rev 1646 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1634 | Rev 1645 | ||
---|---|---|---|
Line 1... | Line 1... | ||
1 | #ifndef _ANALOG_H |
1 | #ifndef _ANALOG_H |
2 | #define _ANALOG_H |
2 | #define _ANALOG_H |
3 | #include <inttypes.h> |
3 | #include <inttypes.h> |
Line 4... | Line 4... | ||
4 | 4 | ||
5 | //#include "invenSense.h" |
5 | // #include "invenSense.h" |
Line 6... | Line -... | ||
6 | #include "ENC-03_FC1.3.h" |
- | |
7 | 6 | #include "ENC-03_FC1.3.h" |
|
8 | 7 | ||
9 | /* |
8 | /* |
10 | * How much low pass filtering to apply for hiResPitchGyro and hiResRollGyro. |
9 | * How much low pass filtering to apply for gyro_PID. |
11 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
10 | * 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. |
11 | * Temporarily replaced by userparam-configurable variable. |
Line 13... | Line 12... | ||
13 | */ |
12 | */ |
14 | //#define GYROS_FIRSTORDERFILTER 2 |
13 | #define GYROS_PIDFILTER 1 |
15 | 14 | ||
16 | /* |
15 | /* |
17 | * How much low pass filtering to apply for filteredHiResPitchGyro and filteredHiResRollGyro. |
16 | * How much low pass filtering to apply for gyro_ATT. |
18 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
17 | * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc... |
Line 19... | Line 18... | ||
19 | * Temporarily replaced by userparam-configurable variable. |
18 | * Temporarily replaced by userparam-configurable variable. |
20 | */ |
19 | */ |
Line 21... | Line 20... | ||
21 | //#define GYROS_SECONDORDERFILTER 2 |
20 | #define GYROS_INTEGRALFILTER 1 |
22 | 21 | ||
23 | // Temporarily replaced by userparam-configurable variable. |
22 | // Temporarily replaced by userparam-configurable variable. |
24 | //#define ACC_FILTER 4 |
23 | //#define ACC_FILTER 4 |
25 | 24 | ||
26 | /* |
25 | /* |
27 | About setting constants right for different gyros: |
26 | About setting constants for different gyros: |
28 | Main parameters are positive directions and voltage/angular speed gain. |
27 | Main parameters are positive directions and voltage/angular speed gain. |
29 | The "Positive direction" is the rotation direction around an axis where |
28 | The "Positive direction" is the rotation direction around an axis where |
Line 30... | Line 29... | ||
30 | the corresponding gyro gives a voltage > the no-rotation voltage. |
29 | the corresponding gyro outputs a voltage > the no-rotation voltage. |
31 | A gyro is considered, in this code, to be "forward" if its positive |
30 | A gyro is considered, in this code, to be "forward" if its positive |
Line 42... | Line 41... | ||
42 | or 4 (other versions), offset to zero, low pass filtered and then assigned |
41 | 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 |
42 | to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or |
44 | roll. |
43 | roll. |
45 | So: |
44 | So: |
Line 46... | Line 45... | ||
46 | |
45 | |
47 | HiResXXXX = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4), |
46 | gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4), |
Line 48... | Line 47... | ||
48 | where V is 2 for FC1.0 and 4 for all others. |
47 | where V is 2 for FC1.0 and 4 for all others. |
Line 49... | Line 48... | ||
49 | |
48 | |
Line 50... | Line 49... | ||
50 | Assuming constant ADCValue, in the H&I code: |
49 | Assuming constant ADCValue, in the H&I code: |
Line 51... | Line 50... | ||
51 | |
50 | |
52 | HiResXXXX = I * ADCValue. |
51 | gyro = I * ADCValue. |
Line 77... | Line 76... | ||
77 | My InvenSense copter has 2mV/deg/s gyros and no amplifiers: |
76 | 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) |
77 | 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 |
78 | (only about half as sensitive as V1.3. But it will take about twice the |
80 | rotation rate!) |
79 | rotation rate!) |
Line 81... | Line 80... | ||
81 | 80 | ||
82 | All together: HiResXXXX = I * H * rotation rate [units / (deg/s)]. |
81 | All together: gyro = I * H * rotation rate [units / (deg/s)]. |
Line 83... | Line 82... | ||
83 | */ |
82 | */ |
84 | 83 | ||
85 | /* |
84 | /* |
86 | * A factor that the raw gyro values are multiplied by, |
85 | * A factor that the raw gyro values are multiplied by, |
87 | * before being zero-offset, filtered and passed to the attitude module. |
86 | * before being filtered and passed to the attitude module. |
88 | * A value of 1 would cause a little bit of loss of precision in the |
87 | * 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 |
88 | * filtering (on the other hand the values are so noisy in flight that |
- | 89 | * 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 |
90 | * noticeable). 4 is fine for the default filtering. |
91 | * noticeable). 4 is fine for the default filtering. |
91 | * Experiment: Set it to 1. |
Line 92... | Line 92... | ||
92 | */ |
92 | */ |
93 | #define GYRO_FACTOR_PITCHROLL 4 |
93 | #define GYRO_FACTOR_PITCHROLL 4 |
Line 109... | Line 109... | ||
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. |
Line 112... | Line 112... | ||
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 | gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL]. |
115 | Assuming a constant rotation rate v and an initial gyroIntegralXXXX (for this |
115 | Assuming a constant rotation rate v and a zero initial gyroIntegral |
Line 116... | Line 116... | ||
116 | explanation), we get: |
116 | (for this explanation), we get: |
117 | |
117 | |
118 | gyroIntegralXXXX = |
118 | gyroIntegral = |
Line 119... | Line 119... | ||
119 | N * HiResXXXX / HIRES_GYRO_INTEGRATION_FACTOR = |
119 | N * gyro / HIRES_GYRO_INTEGRATION_FACTOR = |
Line 120... | Line 120... | ||
120 | N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
120 | N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
Line 132... | Line 132... | ||
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 | */ |
Line 135... | Line 135... | ||
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 gyro[PITCH/ROLL] 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) |
Line 141... | Line 141... | ||
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 | ||
- | 143 | /* |
|
- | 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) |
|
- | 149 | #define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL) |
|
- | 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) |
|
142 | 154 | ||
143 | /* |
155 | /* |
144 | * This value is subtracted from the gyro noise measurement in each iteration, |
156 | * This value is subtracted from the gyro noise measurement in each iteration, |
145 | * making it return towards zero. |
157 | * making it return towards zero. |
Line -... | Line 158... | ||
- | 158 | */ |
|
- | 159 | #define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
|
146 | */ |
160 | |
147 | #define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
161 | #define PITCH 0 |
- | 162 | #define ROLL 1 |
|
- | 163 | /* |
|
- | 164 | * The values that this module outputs |
|
- | 165 | * These first 2 exported arrays are zero-offset. The "PID" ones are used |
|
- | 166 | * in the attitude control as rotation rates. The "ATT" ones are for |
|
- | 167 | * integration to angles. For the same axis, the PID and ATT variables |
|
148 | 168 | * generally have about the same values. There are just some differences |
|
149 | /* |
169 | * in filtering, and when a gyro becomes near saturated. |
150 | * The values that this module outputs |
170 | * Maybe this distinction is not really necessary. |
151 | */ |
171 | */ |
- | 172 | extern volatile int16_t gyro_PID[2]; |
|
152 | extern volatile int16_t hiResPitchGyro, hiResRollGyro; |
173 | extern volatile int16_t gyro_ATT[2]; |
153 | extern volatile int16_t filteredHiResPitchGyro, filteredHiResRollGyro; |
174 | extern volatile int16_t gyroD[2]; |
154 | extern volatile int16_t pitchGyroD, rollGyroD; |
175 | |
Line 155... | Line 176... | ||
155 | extern volatile uint16_t ADCycleCount; |
176 | extern volatile uint16_t ADCycleCount; |
156 | extern volatile int16_t UBat; |
177 | extern volatile int16_t UBat; |
157 | extern volatile int16_t yawGyro; |
178 | extern volatile int16_t yawGyro; |
158 | 179 | ||
Line 159... | Line 180... | ||
159 | /* |
180 | /* |
160 | * This is not really for external use - but the ENC-03 gyro modules needs it. |
181 | * This is not really for external use - but the ENC-03 gyro modules needs it. |
161 | */ |
182 | */ |
162 | extern volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum; |
- | |
163 | - | ||
164 | /* |
- | |
165 | * The acceleration values that this module outputs |
- | |
166 | */ |
- | |
167 | extern volatile int16_t pitchAxisAcc, rollAxisAcc, ZAxisAcc; |
- | |
168 | extern volatile int16_t filteredPitchAxisAcc, filteredRollAxisAcc; |
- | |
169 | - | ||
170 | // Only for debugging! Not to be exported! Remove when finished. |
183 | extern volatile int16_t rawGyroSum[2], rawYawGyroSum; |
171 | // extern volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset; |
- | |
172 | 184 | ||
173 | // Air pressure measurement not supported right now. |
- | |
174 | // extern volatile int32_t AirPressure; |
- | |
Line 175... | Line 185... | ||
175 | // extern volatile int16_t HeightD; |
185 | /* |
176 | // extern volatile uint16_t ReadingAirPressure; |
186 | * The acceleration values that this module outputs. They are zero based. |
177 | // extern volatile int16_t StartAirPressure; |
187 | */ |
178 | // extern uint8_t PressureSensorOffset; |
188 | extern volatile int16_t acc[2], ZAcc; |
Line 179... | Line 189... | ||
179 | // extern int8_t ExpandBaro; |
189 | extern volatile int16_t filteredAcc[2]; |
180 | 190 | ||
181 | /* |
191 | /* |
182 | * Flag: Interrupt handler has done all A/D conversion and processing. |
192 | * Flag: Interrupt handler has done all A/D conversion and processing. |
183 | */ |
193 | */ |
Line 184... | Line 194... | ||
184 | extern volatile uint8_t analogDataReady; |
194 | extern volatile uint8_t analogDataReady; |
Line 185... | Line 195... | ||
185 | 195 |