Rev 1867 | Rev 1870 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1867 | Rev 1868 | ||
---|---|---|---|
Line 1... | Line 1... | ||
1 | \// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 04.2007 Holger Buss |
2 | // + Copyright (c) 04.2007 Holger Buss |
3 | // + Nur f�r den privaten Gebrauch |
3 | // + Nur für den privaten Gebrauch |
4 | // + www.MikroKopter.com |
4 | // + www.MikroKopter.com |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
6 | // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
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. |
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 |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen, |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht, |
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 |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
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" |
17 | // + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
18 | // + eindeutig als Ursprung verlinkt und genannt werden |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
20 | // + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion |
20 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
21 | // + Benutzung auf eigene Gefahr |
21 | // + Benutzung auf eigene Gefahr |
22 | // + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den |
22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
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 |
25 | // + mit unserer Zustimmung zulässig |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
30 | // + this list of conditions and the following disclaimer. |
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 |
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. |
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 |
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
34 | // + for non-commercial use (directly or indirectly) |
34 | // + for non-commercial use (directly or indirectly) |
35 | // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
35 | // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
36 | // + with our written permission |
36 | // + with our written permission |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
38 | // + clearly linked as origin |
38 | // + clearly linked as origin |
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
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" |
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
Line 87... | Line 87... | ||
87 | 87 | ||
88 | // Some integral weight constant... |
88 | // Some integral weight constant... |
89 | uint16_t Ki = 10300 / 33; |
89 | uint16_t Ki = 10300 / 33; |
Line 90... | Line -... | ||
90 | uint8_t RequiredMotors = 0; |
- | |
91 | - | ||
92 | // No support for altitude control right now. |
- | |
93 | // int16_t SetPointHeight = 0; |
90 | uint8_t RequiredMotors = 0; |
94 | 91 | ||
95 | /************************************************************************/ |
92 | /************************************************************************/ |
96 | /* Filter for motor value smoothing (necessary???) */ |
93 | /* Filter for motor value smoothing (necessary???) */ |
97 | /************************************************************************/ |
94 | /************************************************************************/ |
Line 177... | Line 174... | ||
177 | controlMixer_update(); |
174 | controlMixer_update(); |
Line 178... | Line 175... | ||
178 | 175 | ||
179 | // Fire the main flight attitude calculation, including integration of angles. |
176 | // Fire the main flight attitude calculation, including integration of angles. |
Line 180... | Line -... | ||
180 | calculateFlightAttitude(); |
- | |
181 | 177 | calculateFlightAttitude(); |
|
182 | J5HIGH; |
178 | |
183 | throttleTerm = controlThrottle; |
179 | throttleTerm = controlThrottle; |
184 | // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
180 | // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
Line 259... | Line 255... | ||
259 | */ |
255 | */ |
260 | if(looping) { |
256 | if(looping) { |
261 | if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit; |
257 | if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit; |
262 | } |
258 | } |
Line 263... | Line -... | ||
263 | - | ||
264 | J5LOW; |
- | |
265 | 259 | ||
266 | /************************************************************************/ |
260 | /************************************************************************/ |
267 | /* Yawing */ |
261 | /* Yawing */ |
268 | /************************************************************************/ |
262 | /************************************************************************/ |
269 | if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
263 | if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
Line 307... | Line 301... | ||
307 | 301 | ||
308 | /* Calculate control feedback from angle (gyro integral) */ |
302 | /* Calculate control feedback from angle (gyro integral) */ |
309 | /* and angular velocity (gyro signal) */ |
303 | /* and angular velocity (gyro signal) */ |
310 | /************************************************************************/ |
304 | /************************************************************************/ |
- | 305 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
|
311 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
306 | |
312 | for (axis=PITCH; axis<=ROLL; axis++) { |
307 | for (axis=PITCH; axis<=ROLL; axis++) { |
313 | if(looping & ((1<<4)<<axis)) { |
308 | if(looping & ((1<<4)<<axis)) { |
314 | PPart[axis] = 0; |
309 | PPart[axis] = 0; |
315 | } else { // TODO: Where do the 44000 come from??? |
310 | } else { // TODO: Where do the 44000 come from??? |
Line 346... | Line 341... | ||
346 | } |
341 | } |
Line 347... | Line 342... | ||
347 | 342 | ||
348 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
343 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
Line 349... | Line -... | ||
349 | throttleTerm *= CONTROL_SCALING; |
- | |
350 | - | ||
351 | J5HIGH; |
344 | throttleTerm *= CONTROL_SCALING; |
352 | 345 | ||
353 | /* |
346 | /* |
354 | * Compose yaw term. |
347 | * Compose yaw term. |
355 | * The yaw term is limited: Absolute value is max. = the throttle term / 2. |
348 | * The yaw term is limited: Absolute value is max. = the throttle term / 2. |
Line 389... | Line 382... | ||
389 | } else if (yawTerm > (tmp_int - throttleTerm)) { |
382 | } else if (yawTerm > (tmp_int - throttleTerm)) { |
390 | yawTerm = (tmp_int - throttleTerm); |
383 | yawTerm = (tmp_int - throttleTerm); |
391 | DebugOut.Digital[0] |= DEBUG_CLIP; |
384 | DebugOut.Digital[0] |= DEBUG_CLIP; |
392 | } |
385 | } |
Line 393... | Line -... | ||
393 | - | ||
394 | J5LOW; |
- | |
395 | 386 | ||
396 | // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
387 | // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
397 | DebugOut.Digital[1] &= ~DEBUG_CLIP; |
388 | DebugOut.Digital[1] &= ~DEBUG_CLIP; |
398 | for (axis=PITCH; axis<=ROLL; axis++) { |
389 | for (axis=PITCH; axis<=ROLL; axis++) { |
399 | /* |
390 | /* |
Line 430... | Line 421... | ||
430 | } |
421 | } |
431 | CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
422 | CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
432 | } |
423 | } |
433 | // end part 3: 350 - 400 usec. |
424 | // end part 3: 350 - 400 usec. |
Line 434... | Line -... | ||
434 | - | ||
435 | J5HIGH; |
- | |
436 | 425 | ||
437 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
426 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
438 | // Universal Mixer |
427 | // Universal Mixer |
439 | // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
428 | // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
Line 473... | Line 462... | ||
473 | if (i < 4) |
462 | if (i < 4) |
474 | DebugOut.Analog[22+i] = motor[i].SetPoint; |
463 | DebugOut.Analog[22+i] = motor[i].SetPoint; |
475 | } |
464 | } |
476 | I2C_Start(TWI_STATE_MOTOR_TX); |
465 | I2C_Start(TWI_STATE_MOTOR_TX); |
Line 477... | Line -... | ||
477 | - | ||
478 | J5LOW; |
- | |
479 | 466 | ||
480 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
467 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
481 | // Debugging |
468 | // Debugging |
482 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
469 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
483 | if(!(--debugDataTimer)) { |
470 | if(!(--debugDataTimer)) { |