Rev 2047 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2047 | Rev 2048 | ||
---|---|---|---|
Line 1... | Line -... | ||
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
2 | // + Copyright (c) 04.2007 Holger Buss |
- | |
3 | // + Nur f�r den privaten Gebrauch |
- | |
4 | // + www.MikroKopter.com |
- | |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
6 | // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
- | |
7 | // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist. |
- | |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
- | |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
- | |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen, |
- | |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
- | |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht, |
- | |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen |
- | |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
- | |
17 | // + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
- | |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
- | |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
20 | // + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion |
- | |
21 | // + Benutzung auf eigene Gefahr |
- | |
22 | // + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den |
- | |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
- | |
25 | // + mit unserer Zustimmung zul�ssig |
- | |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
- | |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
- | |
30 | // + this list of conditions and the following disclaimer. |
- | |
31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
- | |
32 | // + from this software without specific prior written permission. |
- | |
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
- | |
34 | // + for non-commercial use (directly or indirectly) |
- | |
35 | // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
- | |
36 | // + with our written permission |
- | |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
- | |
38 | // + clearly linked as origin |
- | |
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
- | |
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
- | |
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
- | |
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
- | |
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
- | |
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
- | |
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
- | |
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
- | |
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
- | |
48 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
- | |
49 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
- | |
50 | // + POSSIBILITY OF SUCH DAMAGE. |
- | |
51 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
52 | /************************************************************************/ |
- | |
53 | /* Flight Attitude */ |
- | |
54 | /************************************************************************/ |
- | |
55 | - | ||
56 | #include <stdlib.h> |
1 | #include <stdlib.h> |
57 | #include <avr/io.h> |
2 | #include <avr/io.h> |
Line 58... | Line 3... | ||
58 | 3 | ||
59 | #include "attitude.h" |
4 | #include "attitude.h" |
- | 5 | #include "dongfangMath.h" |
|
Line 60... | Line 6... | ||
60 | #include "dongfangMath.h" |
6 | #include "commands.h" |
61 | 7 | ||
Line 62... | Line 8... | ||
62 | // For scope debugging only! |
8 | // For scope debugging only! |
Line 100... | Line 46... | ||
100 | 46 | ||
101 | /* |
47 | /* |
102 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
48 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
103 | * horizontal plane, yaw relative to yaw at start. |
49 | * horizontal plane, yaw relative to yaw at start. |
104 | */ |
50 | */ |
Line 105... | Line 51... | ||
105 | int32_t angle[2], yawAngleDiff; |
51 | int32_t attitude[2]; |
Line 106... | Line 52... | ||
106 | 52 | ||
Line 107... | Line 53... | ||
107 | int readingHeight = 0; |
53 | //int readingHeight = 0; |
108 | 54 | ||
Line 109... | Line 55... | ||
109 | // Yaw angle and compass stuff. |
55 | // Yaw angle and compass stuff. |
110 | 56 | ||
- | 57 | // This is updated/written from MM3. Negative angle indicates invalid data. |
|
- | 58 | int16_t magneticHeading = -1; |
|
Line 111... | Line 59... | ||
111 | // This is updated/written from MM3. Negative angle indicates invalid data. |
59 | |
112 | int16_t magneticHeading = -1; |
60 | // This is NOT updated from MM3. Negative angle indicates invalid data. |
113 | 61 | // int16_t headingInDegrees = -1; |
|
Line 114... | Line -... | ||
114 | // This is NOT updated from MM3. Negative angle indicates invalid data. |
- | |
115 | int16_t compassCourse = -1; |
- | |
116 | 62 | ||
Line 117... | Line 63... | ||
117 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
63 | int32_t targetHeading; |
118 | // Not necessary. Never read anywhere. |
64 | |
Line 119... | Line -... | ||
119 | // int16_t compassOffCourse = 0; |
- | |
120 | - | ||
121 | uint8_t updateCompassCourse = 0; |
- | |
122 | uint8_t compassCalState = 0; |
- | |
123 | uint16_t ignoreCompassTimer = 500; |
65 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
Line 124... | Line 66... | ||
124 | 66 | // Not necessary. Never read anywhere. |
|
125 | int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
67 | // int16_t compassOffCourse = 0; |
Line 154... | Line 96... | ||
154 | * it is hardly worth the trouble. |
96 | * it is hardly worth the trouble. |
155 | ************************************************************************/ |
97 | ************************************************************************/ |
Line 156... | Line 98... | ||
156 | 98 | ||
157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
99 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
158 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
100 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
159 | return (int32_t)GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];// + correctionTerm; |
101 | return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm; |
160 | // return 342L * filteredAcc[axis]; |
102 | // return 342L * filteredAcc[axis]; |
Line 161... | Line 103... | ||
161 | } |
103 | } |
162 | 104 | ||
163 | void setStaticAttitudeAngles(void) { |
105 | void setStaticAttitudeAngles(void) { |
164 | #ifdef ATTITUDE_USE_ACC_SENSORS |
106 | #ifdef ATTITUDE_USE_ACC_SENSORS |
165 | angle[PITCH] = getAngleEstimateFromAcc(PITCH); |
107 | attitude[PITCH] = getAngleEstimateFromAcc(PITCH); |
166 | angle[ROLL] = getAngleEstimateFromAcc(ROLL); |
108 | attitude[ROLL] = getAngleEstimateFromAcc(ROLL); |
167 | #else |
109 | #else |
168 | angle[PITCH] = angle[ROLL] = 0; |
110 | attitude[PITCH] = attitude[ROLL] = 0; |
Line 169... | Line 111... | ||
169 | #endif |
111 | #endif |
170 | } |
112 | } |
Line 182... | Line 124... | ||
182 | // Calibrate hardware. |
124 | // Calibrate hardware. |
183 | analog_setNeutral(); |
125 | analog_setNeutral(); |
Line 184... | Line 126... | ||
184 | 126 | ||
185 | // reset gyro integrals to acc guessing |
127 | // reset gyro integrals to acc guessing |
186 | setStaticAttitudeAngles(); |
- | |
187 | yawAngleDiff = 0; |
- | |
188 | - | ||
189 | // update compass course to current heading |
128 | setStaticAttitudeAngles(); |
190 | compassCourse = magneticHeading; |
- | |
191 | - | ||
192 | // Inititialize YawGyroIntegral value with current compass heading |
- | |
193 | yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
- | |
194 | 129 | attitude_resetHeadingToMagnetic(); |
|
195 | // Servo_On(); //enable servo output |
130 | // Servo_On(); //enable servo output |
Line 196... | Line 131... | ||
196 | } |
131 | } |
197 | 132 | ||
Line 222... | Line 157... | ||
222 | * (from airframe-local axes to a ground-based system). For some reason |
157 | * (from airframe-local axes to a ground-based system). For some reason |
223 | * the MK uses a left-hand coordinate system. The tranformation has been |
158 | * the MK uses a left-hand coordinate system. The tranformation has been |
224 | * changed accordingly. |
159 | * changed accordingly. |
225 | */ |
160 | */ |
226 | void trigAxisCoupling(void) { |
161 | void trigAxisCoupling(void) { |
227 | int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
162 | int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
228 | int16_t pitchAngleInDegrees = angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
163 | int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
Line 229... | Line 164... | ||
229 | 164 | ||
230 | int16_t cospitch = cos_360(pitchAngleInDegrees); |
165 | int16_t cospitch = cos_360(pitchAngleInDegrees); |
231 | int16_t cosroll = cos_360(rollAngleInDegrees); |
166 | int16_t cosroll = cos_360(rollAngleInDegrees); |
Line 232... | Line 167... | ||
232 | int16_t sinroll = sin_360(rollAngleInDegrees); |
167 | int16_t sinroll = sin_360(rollAngleInDegrees); |
233 | 168 | ||
234 | ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
- | |
235 | * sinroll) >> LOG_MATH_UNIT_FACTOR); |
- | |
236 | - | ||
Line -... | Line 169... | ||
- | 169 | ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll |
|
- | 170 | - (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR); |
|
- | 171 | ||
- | 172 | ACRate[ROLL] = rate_ATT[ROLL] |
|
- | 173 | + (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
|
- | 174 | >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) |
|
237 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
175 | >> LOG_MATH_UNIT_FACTOR); |
- | 176 | ||
238 | + (int32_t)yawRate * cosroll) >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) >> LOG_MATH_UNIT_FACTOR); |
177 | ACYawRate = |
- | 178 | ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
|
239 | 179 | / cospitch; |
|
- | 180 | ||
240 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
181 | ACYawRate = |
Line 241... | Line 182... | ||
241 | 182 | ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
|
242 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
183 | / cospitch; |
243 | } |
184 | } |
Line 256... | Line 197... | ||
256 | } |
197 | } |
Line 257... | Line 198... | ||
257 | 198 | ||
258 | /* |
199 | /* |
259 | * Yaw |
200 | * Yaw |
260 | * Calculate yaw gyro integral (~ to rotation angle) |
201 | * Calculate yaw gyro integral (~ to rotation angle) |
261 | * Limit yawGyroHeading proportional to 0 deg to 360 deg |
202 | * Limit heading proportional to 0 deg to 360 deg |
262 | */ |
203 | */ |
263 | yawGyroHeading += ACYawRate; |
- | |
264 | yawAngleDiff += yawRate; |
- | |
265 | 204 | heading += ACYawRate; |
|
266 | if (yawGyroHeading >= YAWOVER360) { |
- | |
267 | yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
- | |
268 | } else if (yawGyroHeading < 0) { |
- | |
269 | yawGyroHeading += YAWOVER360; |
- | |
270 | } |
- | |
271 | 205 | intervalWrap(&heading, YAWOVER360); |
|
272 | /* |
206 | /* |
273 | * Pitch axis integration and range boundary wrap. |
207 | * Pitch axis integration and range boundary wrap. |
274 | */ |
208 | */ |
275 | for (axis = PITCH; axis <= ROLL; axis++) { |
209 | for (axis = PITCH; axis <= ROLL; axis++) { |
276 | angle[axis] += ACRate[axis]; |
210 | attitude[axis] += ACRate[axis]; |
277 | if (angle[axis] > PITCHROLLOVER180) { |
211 | if (attitude[axis] > PITCHROLLOVER180) { |
278 | angle[axis] -= PITCHROLLOVER360; |
212 | attitude[axis] -= PITCHROLLOVER360; |
279 | } else if (angle[axis] <= -PITCHROLLOVER180) { |
213 | } else if (attitude[axis] <= -PITCHROLLOVER180) { |
280 | angle[axis] += PITCHROLLOVER360; |
214 | attitude[axis] += PITCHROLLOVER360; |
281 | } |
215 | } |
282 | } |
216 | } |
Line 283... | Line 217... | ||
283 | } |
217 | } |
Line 297... | Line 231... | ||
297 | int32_t temp; |
231 | int32_t temp; |
Line 298... | Line 232... | ||
298 | 232 | ||
299 | uint8_t ca = controlActivity >> 8; |
233 | uint8_t ca = controlActivity >> 8; |
Line 300... | Line 234... | ||
300 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
234 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
301 | 235 | ||
302 | if (highControlActivity) { |
236 | if (highControlActivity) { |
303 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
237 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
304 | } else { |
238 | } else { |
Line 305... | Line 239... | ||
305 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
239 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
306 | } |
240 | } |
307 | 241 | ||
308 | if (accVector <= dynamicParams.maxAccVector) { |
242 | if (accVector <= dynamicParams.maxAccVector) { |
309 | debugOut.digital[0] &= ~ DEBUG_ACC0THORDER; |
243 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
Line 310... | Line 244... | ||
310 | 244 | ||
311 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
245 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
312 | int32_t accDerived; |
246 | int32_t accDerived; |
313 | 247 | ||
314 | /* |
248 | /* |
315 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
249 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
316 | permilleAcc /= 2; |
250 | permilleAcc /= 2; |
317 | debugFullWeight = 0; |
251 | debugFullWeight = 0; |
318 | } |
252 | } |
319 | 253 | ||
Line 320... | Line 254... | ||
320 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
254 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
321 | permilleAcc /= 2; |
255 | permilleAcc /= 2; |
322 | debugFullWeight = 0; |
256 | debugFullWeight = 0; |
323 | */ |
257 | */ |
324 | 258 | ||
325 | if (highControlActivity) { // reduce effect during stick control activity |
259 | if (highControlActivity) { // reduce effect during stick control activity |
Line 326... | Line 260... | ||
326 | permilleAcc /= 4; |
260 | permilleAcc /= 4; |
327 | if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
261 | if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity |
328 | permilleAcc /= 4; |
262 | permilleAcc /= 4; |
329 | } |
263 | } |
330 | } |
264 | } |
331 | 265 | ||
332 | /* |
266 | /* |
333 | * Add to each sum: The amount by which the angle is changed just below. |
267 | * Add to each sum: The amount by which the angle is changed just below. |
334 | */ |
268 | */ |
335 | for (axis = PITCH; axis <= ROLL; axis++) { |
269 | for (axis = PITCH; axis <= ROLL; axis++) { |
336 | accDerived = getAngleEstimateFromAcc(axis); |
270 | accDerived = getAngleEstimateFromAcc(axis); |
337 | debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
271 | debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
338 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
272 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
339 | temp = angle[axis]; |
273 | temp = attitude[axis]; |
340 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
274 | attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
341 | + (int32_t) permilleAcc * accDerived) / 1000L; |
275 | + (int32_t) permilleAcc * accDerived) / 1000L; |
Line 370... | Line 304... | ||
370 | 304 | ||
371 | if (!--timer) { |
305 | if (!--timer) { |
372 | timer = DRIFTCORRECTION_TIME; |
306 | timer = DRIFTCORRECTION_TIME; |
373 | for (axis = PITCH; axis <= ROLL; axis++) { |
307 | for (axis = PITCH; axis <= ROLL; axis++) { |
374 | // Take the sum of corrections applied, add it to delta |
308 | // Take the sum of corrections applied, add it to delta |
375 | if (correctionSum[axis] >=0) |
309 | if (correctionSum[axis] >= 0) |
376 | round = DRIFTCORRECTION_TIME / 2; |
310 | round = DRIFTCORRECTION_TIME / 2; |
377 | else |
311 | else |
378 | round = -DRIFTCORRECTION_TIME / 2; |
312 | round = -DRIFTCORRECTION_TIME / 2; |
379 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
313 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
380 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
314 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
381 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
315 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
382 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
316 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
383 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
317 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
384 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
318 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
385 | // debugOut.analog[28 + axis] = driftComp[axis]; |
- | |
386 | 319 | // debugOut.analog[28 + axis] = driftComp[axis]; |
|
387 | correctionSum[axis] = 0; |
320 | correctionSum[axis] = 0; |
388 | } |
321 | } |
389 | } |
322 | } |
Line 390... | Line 323... | ||
390 | } |
323 | } |
391 | 324 | ||
392 | void calculateAccVector(void) { |
325 | void calculateAccVector(void) { |
393 | int16_t temp; |
326 | int16_t temp; |
394 | temp = filteredAcc[0] >> 3; |
327 | temp = filteredAcc[0] >> 3; |
395 | accVector = temp * temp; |
328 | accVector = temp * temp; |
396 | temp = filteredAcc[1] >> 3; |
329 | temp = filteredAcc[1] >> 3; |
397 | accVector += temp * temp; |
330 | accVector += temp * temp; |
398 | temp = filteredAcc[2] >> 3; |
331 | temp = filteredAcc[2] >> 3; |
- | 332 | accVector += temp * temp; |
|
- | 333 | //debugOut.analog[18] = accVector; |
|
- | 334 | } |
|
- | 335 | ||
- | 336 | void attitude_resetHeadingToMagnetic(void) { |
|
- | 337 | if (commands_isCalibratingCompass()) |
|
- | 338 | return; |
|
- | 339 | ||
- | 340 | // Compass is off, skip. |
|
- | 341 | if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
|
- | 342 | return; |
|
- | 343 | ||
- | 344 | // Compass is invalid, skip. |
|
- | 345 | if (magneticHeading < 0) |
|
- | 346 | return; |
|
- | 347 | ||
- | 348 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
|
- | 349 | targetHeading = heading; |
|
- | 350 | ||
- | 351 | debugOut.digital[0] ^= DEBUG_COMPASS; |
|
- | 352 | } |
|
- | 353 | ||
- | 354 | void correctHeadingToMagnetic(void) { |
|
- | 355 | int32_t error; |
|
- | 356 | ||
- | 357 | debugOut.analog[27] = heading; |
|
- | 358 | ||
- | 359 | if (commands_isCalibratingCompass()) |
|
- | 360 | return; |
|
- | 361 | ||
- | 362 | // Compass is off, skip. |
|
- | 363 | // Naaah this is assumed. |
|
- | 364 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
|
- | 365 | // return; |
|
- | 366 | ||
- | 367 | // Compass is invalid, skip. |
|
- | 368 | if (magneticHeading < 0) |
|
- | 369 | return; |
|
- | 370 | ||
- | 371 | // Spinning fast, skip |
|
- | 372 | if (abs(yawRate) > 128) |
|
- | 373 | return; |
|
- | 374 | ||
- | 375 | // Otherwise invalidated, skip |
|
- | 376 | if (ignoreCompassTimer) { |
|
- | 377 | ignoreCompassTimer--; |
|
- | 378 | return; |
|
- | 379 | } |
|
- | 380 | ||
- | 381 | // TODO: Find computational cost of this. |
|
- | 382 | error = (magneticHeading*GYRO_DEG_FACTOR_YAW - heading) % GYRO_DEG_FACTOR_YAW; |
|
- | 383 | ||
- | 384 | // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
|
- | 385 | // better resolution of the gyros to the worse resolution of the compass all the time. |
|
- | 386 | // The correction should really only serve to compensate for gyro drift. |
|
- | 387 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
|
- | 388 | ||
- | 389 | int32_t correction = (error * (int32_t)dynamicParams.compassYawEffect) >> 8; |
|
- | 390 | ||
- | 391 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
|
- | 392 | // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
|
- | 393 | // no effect on control at all!!! It only has effect on the values of the two variables. However, these values |
|
- | 394 | // could have effect on control elsewhere, like in compassControl.c . |
|
- | 395 | heading += correction; |
|
- | 396 | intervalWrap(&heading, YAWOVER360); |
|
- | 397 | ||
- | 398 | targetHeading += correction; |
|
- | 399 | intervalWrap(&targetHeading, YAWOVER360); |
|
399 | accVector += temp * temp; |
400 | |
Line 400... | Line 401... | ||
400 | //debugOut.analog[18] = accVector; |
401 | debugOut.digital[1] ^= DEBUG_COMPASS; |
401 | } |
402 | } |
402 | 403 | ||
Line 414... | Line 415... | ||
414 | #endif |
415 | #endif |
Line 415... | Line 416... | ||
415 | 416 | ||
416 | // We are done reading variables from the analog module. |
417 | // We are done reading variables from the analog module. |
417 | // Interrupt-driven sensor reading may restart. |
418 | // Interrupt-driven sensor reading may restart. |
418 | startAnalogConversionCycle(); |
- | |
419 | } |
- | |
420 | - | ||
421 | void updateCompass(void) { |
- | |
Line 422... | Line 419... | ||
422 | int16_t w, v, r, correction, error; |
419 | startAnalogConversionCycle(); |
423 | - | ||
424 | if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
- | |
425 | if (controlMixer_testCompassCalState()) { |
- | |
426 | compassCalState++; |
- | |
427 | if (compassCalState < 5) |
- | |
428 | beepNumber(compassCalState); |
- | |
429 | else |
- | |
430 | beep(1000); |
- | |
431 | } |
- | |
432 | } else { |
- | |
433 | // get maximum attitude angle |
- | |
434 | w = abs(angle[PITCH] / 512); |
- | |
435 | v = abs(angle[ROLL] / 512); |
- | |
436 | if (v > w) |
420 | |
437 | w = v; |
- | |
438 | correction = w / 8 + 1; |
- | |
439 | // calculate the deviation of the yaw gyro heading and the compass heading |
- | |
440 | if (magneticHeading < 0) |
- | |
441 | error = 0; // disable yaw drift compensation if compass heading is undefined |
- | |
442 | else if (abs(yawRate) > 128) { // spinning fast |
- | |
443 | error = 0; |
- | |
444 | } else { |
- | |
445 | // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
- | |
446 | error = ((540 + magneticHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) |
- | |
447 | % 360) - 180; |
- | |
448 | } |
- | |
449 | if (!ignoreCompassTimer && w < 25) { |
- | |
450 | yawGyroDrift += error; |
- | |
451 | // Basically this gets set if we are in "fix" mode, and when starting. |
- | |
452 | if (updateCompassCourse) { |
- | |
453 | beep(200); |
- | |
454 | yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
- | |
455 | compassCourse = magneticHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
- | |
456 | updateCompassCourse = 0; |
- | |
457 | } |
- | |
458 | } |
- | |
459 | yawGyroHeading += (error * 8) / correction; |
- | |
460 | - | ||
461 | /* |
- | |
462 | w = (w * dynamicParams.CompassYawEffect) / 32; |
- | |
463 | w = dynamicParams.CompassYawEffect - w; |
- | |
464 | */ |
- | |
465 | w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect) |
- | |
466 | / 32; |
- | |
467 | - | ||
468 | // As readable formula: |
- | |
469 | // w = dynamicParams.CompassYawEffect * (1-w/32); |
- | |
470 | - | ||
471 | if (w >= 0) { // maxAttitudeAngle < 32 |
- | |
472 | if (!ignoreCompassTimer) { |
- | |
473 | /*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/ |
- | |
474 | v = 64 + controlActivity / 100; |
- | |
475 | // yawGyroHeading - compassCourse on a -180..179 degree interval. |
- | |
476 | r |
- | |
477 | = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
- | |
478 | % 360) - 180; |
- | |
479 | v = (r * w) / v; // align to compass course |
- | |
480 | // limit yaw rate |
- | |
481 | w = 3 * dynamicParams.compassYawEffect; |
- | |
482 | if (v > w) |
- | |
483 | v = w; |
- | |
484 | else if (v < -w) |
- | |
485 | v = -w; |
- | |
486 | yawAngleDiff += v; |
- | |
487 | } else { // wait a while |
- | |
488 | ignoreCompassTimer--; |
- | |
489 | } |
- | |
490 | } else { // ignore compass at extreme attitudes for a while |
- | |
491 | ignoreCompassTimer = 500; |
421 | if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
492 | } |
422 | correctHeadingToMagnetic(); |
Line 493... | Line 423... | ||
493 | } |
423 | } |
494 | } |
424 | } |