Rev 2230 | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
2230 | - | 1 | <!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd"> |
2 | <html xmlns="http://www.w3.org/1999/xhtml" xml:lang="de" lang="de"> |
||
3 | <head> |
||
4 | <meta http-equiv="content-type" content="text/html;charset=UTF-8" /> |
||
5 | <meta http-equiv="generator" content="WebSVN 2.3.3" /> <!-- leave this for stats --> |
||
6 | <link rel="shortcut icon" type="image/x-icon" href="./templates/mkstyle/images/favicon.ico" /> |
||
7 | <link type="text/css" href="./templates/mkstyle/styles.css" rel="stylesheet" media="screen" /> |
||
8 | <link rel='alternate' type='application/rss+xml' title='WebSVN RSS' href='rss.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&' /> |
||
9 | <!--[if gte IE 5.5000]> |
||
10 | <script type="text/javascript" src="./templates/mkstyle/png.js"></script> |
||
11 | <style type="text/css" media="screen"> |
||
12 | tbody tr td { padding:1px 0 } |
||
13 | #wrap h2 { padding:10px 5px 0 5px; margin-bottom:-8px } |
||
14 | </style> |
||
15 | <![endif]--> |
||
16 | <title> |
||
17 | WebSVN |
||
18 | - Projects |
||
19 | - Revision 2105 |
||
20 | - /RaspberryPi/ExPlat/flightcontrol.c |
||
21 | </title> |
||
22 | <script type="text/javascript"> |
||
23 | //<![CDATA[ |
||
24 | function getPath() |
||
25 | { |
||
26 | return '.'; |
||
27 | } |
||
28 | |||
29 | function checkCB(chBox) |
||
30 | { |
||
31 | count = 0 |
||
32 | first = null |
||
33 | f = chBox.form |
||
34 | for (i = 0 ; i < f.elements.length ; i++) |
||
35 | if (f.elements[i].type == 'checkbox' && f.elements[i].checked) |
||
36 | { |
||
37 | if (first == null && f.elements[i] != chBox) |
||
38 | first = f.elements[i] |
||
39 | count += 1 |
||
40 | } |
||
41 | |||
42 | if (count > 2) |
||
43 | { |
||
44 | first.checked = false |
||
45 | count -= 1 |
||
46 | } |
||
47 | } |
||
48 | //]]> |
||
49 | </script> |
||
50 | </head> |
||
51 | <body id="file"> |
||
52 | <div id="container"> |
||
53 | <div id="usebbnetnavigation"> |
||
54 | <ul> |
||
55 | |||
56 | <li><a href="http://www.mikrokopter.de" target="_blank">MikroKopter.de</a></li> |
||
57 | <li><a href="http://www.mikrokopter.de/ucwiki/" target="_blank">Wiki</a></li> |
||
58 | <li><a href="http://forum.mikrokopter.de" >Forum</a></li> |
||
59 | <li><a href="http://gallery3.mikrokopter.de" target="_blank">Bilder</a></li> |
||
60 | <li><a href="http://svn.mikrokopter.de" target="_blank">SVN</a></li> |
||
61 | <li><a href="http://wiki.mikrokopter.de/VideoListe" target="_blank">Videos</a></li> |
||
62 | <li><a href="http://wiki.mikrokopter.de/MikroKopter-FAQ" target="_blank">FAQ</a></li> |
||
63 | <li><a href="http://map.mikrokopter.de" target="_blank">Map-Tool</a></li> |
||
64 | <li><a href="http://www.multikoptermap.de/" target="_blank">User-Map</a></li> |
||
65 | <li><a href="https://www.mikrocontroller.com" target="_blank">SHOP</a></li> |
||
66 | <span class="rightalign"> |
||
67 | <li><a href="http://wiki.mikrokopter.de/Datenschutz" target="_blank">Datenschutz</a></li> |
||
68 | <li><a href="http://wiki.mikrokopter.de/Impressum" target="_blank">Impressum</a></li> |
||
69 | </span> |
||
70 | <!--<li></li> --> |
||
71 | </ul> |
||
72 | </div> |
||
73 | <div id="select"> |
||
74 | <form method="get" action="" id="project"><select name="repname" onchange="javascript:this.form.submit();"><option value="Projects" selected="selected">Projects</option><option value="BL-Ctrl">BL-Ctrl</option><option value="FlightCtrl">FlightCtrl</option><option value="MK3Mag">MK3Mag</option><option value="NaviCtrl">NaviCtrl</option></select><noscript><input type="submit" value="Los" /></noscript></form> |
||
75 | |||
76 | <form method="get" action="" id="language"><input type="hidden" name="repname" value="Projects"/><input type="hidden" name="path" value="/RaspberryPi/ExPlat/flightcontrol.c"/><select name="language" onchange="javascript:this.form.submit();"><option value="ca">Català-Valencià - Catalan</option><option value="zh-CN">中文 - Chinese (Simplified)</option><option value="zh-TW">中文 - Chinese (Traditional)</option><option value="cs">Česky - Czech</option><option value="da">Dansk - Danish</option><option value="nl">Nederlands - Dutch</option><option value="en">English - English</option><option value="fi">Suomi - Finnish</option><option value="fr">Français - French</option><option value="de" selected="selected">Deutsch - German</option><option value="he-IL">עברית - Hebrew</option><option value="hin">हिंदी - Hindi</option><option value="hu">Magyar - Hungarian</option><option value="id">Bahasa Indonesia - Indonesian</option><option value="it">Italiano - Italian</option><option value="ja">日本語 - Japanese</option><option value="ko">한국어 - Korean</option><option value="mk">Македонски - Macedonian</option><option value="mr">मराठी - Marathi</option><option value="no">Norsk - Norwegian</option><option value="pl">Polski - Polish</option><option value="pt">Português - Portuguese</option><option value="pt-BR">Português - Portuguese (Brazil)</option><option value="ru">Русский - Russian</option><option value="sk">Slovenčina - Slovak</option><option value="sl">Slovenščina - Slovenian</option><option value="es">Español - Spanish</option><option value="sv">Svenska - Swedish</option><option value="tr">Türkçe - Turkish</option><option value="uk">Українська - Ukrainian</option><option value="uz">Oëzbekcha - Uzbek</option></select><noscript><input type="submit" value="Los" /></noscript></form> |
||
77 | </div> |
||
78 | <h1><a href=".?" title="Subversion-Projekte">Subversion-Projekte</a> |
||
79 | <span><a href="listing.php?repname=Projects&">Projects</a></span> |
||
80 | </h1> |
||
81 | <h2 id="pathlinks"><a href="listing.php?repname=Projects&path=%2F&" class="root"><span>(root)</span></a>/<a href="listing.php?repname=Projects&path=%2FRaspberryPi%2F&#a7620b340efeec9e2ce42c417f0d0b3fb">RaspberryPi</a>/<a href="listing.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2F&#a9d008415d602dfb4ce4a9442c3038a72">ExPlat</a>/<span class="file">flightcontrol.c</span> - Revision 2105</h2> |
||
82 | <div id="revjump"><form method="get" action="" id="revision"><input type="hidden" name="repname" value="Projects" /><input type="hidden" name="path" value="/RaspberryPi/ExPlat/flightcontrol.c" />Revision <input type="text" size="5" name="rev" placeholder="HEAD" /><span class="submit"><input type="submit" value="Los" /></span></form></div> |
||
83 | <p> |
||
84 | <span class="blame"><a href="blame.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&rev=2105">Blame</a></span> | |
||
85 | <span class="changes"><a href="revision.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&rev=2105">Letzte Änderung</a></span> | |
||
86 | <span class="log"><a href="log.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&rev=2105">Log anzeigen</a></span> |
||
87 | | <span class="feed"><a href="rss.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&">RSS feed</a></span> |
||
88 | </p> |
||
89 | <div class="listing"> |
||
90 | <span style="color: #339933;">#include "main.h"</span><br /> |
||
91 | <span style="color: #339933;">#include "flightcontrol.h"</span><br /> |
||
92 | <br /> |
||
93 | <span style="color: #993333;">int</span> reference_altitude<span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Regelgröße</span><br /> |
||
94 | <span style="color: #993333;">int</span> reference_accz<span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Regelgröße D-Anteil</span><br /> |
||
95 | <span style="color: #993333;">int</span> reference_flow_x<span style="color: #339933;">;</span><br /> |
||
96 | <span style="color: #993333;">int</span> reference_flow_y<span style="color: #339933;">;</span><br /> |
||
97 | <br /> |
||
98 | <span style="color: #993333;">int</span> altitude_differenz<span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Regelabweichung</span><br /> |
||
99 | <span style="color: #993333;">int</span> altitude_differenz_sum <span style="color: #339933;">=</span> <span style="color: #0000dd;">0</span><span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//I-Glied</span><br /> |
||
100 | <span style="color: #993333;">int</span> xDirection_differenz_sum <span style="color: #339933;">=</span> <span style="color: #0000dd;">0</span><span style="color: #339933;">;</span><br /> |
||
101 | <span style="color: #993333;">int</span> yDirection_differenz_sum <span style="color: #339933;">=</span> <span style="color: #0000dd;">0</span><span style="color: #339933;">;</span><br /> |
||
102 | <br /> |
||
103 | <span style="color: #993333;">int</span> altitude_old<span style="color: #339933;">;</span><br /> |
||
104 | <span style="color: #993333;">int</span> altitude_velocity<span style="color: #339933;">;</span><br /> |
||
105 | <span style="color: #993333;">int</span> xDirection_old<span style="color: #339933;">;</span><br /> |
||
106 | <span style="color: #993333;">int</span> xDirection_velocity<span style="color: #339933;">;</span><br /> |
||
107 | <span style="color: #993333;">int</span> yDirection_old<span style="color: #339933;">;</span><br /> |
||
108 | <span style="color: #993333;">int</span> yDirection_velocity<span style="color: #339933;">;</span><br /> |
||
109 | <br /> |
||
110 | <span style="color: #993333;">float</span> gas_correction_dpart<span style="color: #339933;">;</span><br /> |
||
111 | <span style="color: #993333;">float</span> gas_correction_ipart <span style="color: #339933;">=</span> <span style="color:#800080;">0.0</span><span style="color: #339933;">;</span><br /> |
||
112 | <span style="color: #993333;">float</span> gas_correction_ppart<span style="color: #339933;">;</span><br /> |
||
113 | <span style="color: #993333;">float</span> gas_correction_final<span style="color: #339933;">;</span><br /> |
||
114 | <span style="color: #993333;">float</span> nick_correction_dpart<span style="color: #339933;">;</span><br /> |
||
115 | <span style="color: #993333;">float</span> nick_correction_ipart <span style="color: #339933;">=</span> <span style="color:#800080;">0.0</span><span style="color: #339933;">;</span><br /> |
||
116 | <span style="color: #993333;">float</span> nick_correction_ppart<span style="color: #339933;">;</span><br /> |
||
117 | <span style="color: #993333;">float</span> nick_correction_final<span style="color: #339933;">;</span><br /> |
||
118 | <span style="color: #993333;">float</span> roll_correction_dpart<span style="color: #339933;">;</span><br /> |
||
119 | <span style="color: #993333;">float</span> roll_correction_ipart <span style="color: #339933;">=</span> <span style="color:#800080;">0.0</span><span style="color: #339933;">;</span><br /> |
||
120 | <span style="color: #993333;">float</span> roll_correction_ppart<span style="color: #339933;">;</span><br /> |
||
121 | <span style="color: #993333;">float</span> roll_correction_final<span style="color: #339933;">;</span><br /> |
||
122 | <br /> |
||
123 | <span style="color: #993333;">float</span> pFactorGas <span style="color: #339933;">=</span> <span style="color:#800080;">0.3</span><span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Verstärkungsfaktor P</span><br /> |
||
124 | <span style="color: #993333;">float</span> iFactorGas <span style="color: #339933;">=</span> <span style="color:#800080;">0.02</span><span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Verstärkungsfaktor I</span><br /> |
||
125 | <span style="color: #993333;">float</span> dFactorGas <span style="color: #339933;">=</span> <span style="color:#800080;">0.3</span><span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Verstärkungsfaktor D</span><br /> |
||
126 | <span style="color: #993333;">float</span> pFactorNick <span style="color: #339933;">=</span> <span style="color:#800080;">0.2</span><span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Verstärkungsfaktor P</span><br /> |
||
127 | <span style="color: #993333;">float</span> iFactorNick <span style="color: #339933;">=</span> <span style="color:#800080;">0.02</span><span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Verstärkungsfaktor I</span><br /> |
||
128 | <span style="color: #993333;">float</span> dFactorNick <span style="color: #339933;">=</span> <span style="color:#800080;">0.3</span><span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Verstärkungsfaktor D</span><br /> |
||
129 | <span style="color: #993333;">float</span> pFactorRoll <span style="color: #339933;">=</span> <span style="color:#800080;">0.2</span><span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Verstärkungsfaktor P</span><br /> |
||
130 | <span style="color: #993333;">float</span> iFactorRoll <span style="color: #339933;">=</span> <span style="color:#800080;">0.02</span><span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Verstärkungsfaktor I</span><br /> |
||
131 | <span style="color: #993333;">float</span> dFactorRoll <span style="color: #339933;">=</span> <span style="color:#800080;">0.3</span><span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Verstärkungsfaktor D</span><br /> |
||
132 | <br /> |
||
133 | <span style="color: #666666; font-style: italic;">//>> Performing Flight Correction</span><br /> |
||
134 | <span style="color: #666666; font-style: italic;">//------------------------------------------------------------------------------------------------------</span><br /> |
||
135 | <span style="color: #993333;">void</span> perform_flight_correction<span style="color: #009900;">(</span><span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
136 | reference_altitude <span style="color: #339933;">=</span> <span style="color: #009900;">(</span><span style="color: #993333;">int</span><span style="color: #009900;">)</span><span style="color: #009900;">(</span>optical_flow_values.<span style="color: #202020;">ground_distance</span><span style="color: #339933;">*</span>1000<span style="color: #009900;">)</span><span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Regelgröße in mm</span><br /> |
||
137 | reference_flow_x <span style="color: #339933;">+=</span> <span style="color: #009900;">(</span><span style="color: #993333;">int</span><span style="color: #009900;">)</span><span style="color: #009900;">(</span>optical_flow_values.<span style="color: #202020;">flow_comp_m_x</span><span style="color: #339933;">*</span>1000<span style="color: #009900;">)</span><span style="color: #339933;">;</span><br /> |
||
138 | reference_flow_y <span style="color: #339933;">+=</span> <span style="color: #009900;">(</span><span style="color: #993333;">int</span><span style="color: #009900;">)</span><span style="color: #009900;">(</span>optical_flow_values.<span style="color: #202020;">flow_comp_m_y</span><span style="color: #339933;">*</span>1000<span style="color: #009900;">)</span><span style="color: #339933;">;</span><br /> |
||
139 | altitude_differenz <span style="color: #339933;">=</span> STARTING_ALTITUDE <span style="color: #339933;">-</span> reference_altitude<span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Regelabweichung in mm</span><br /> |
||
140 | <br /> |
||
141 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>reference_altitude <span style="color: #339933;">!=</span> altitude_old<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
142 | altitude_velocity <span style="color: #339933;">=</span> <span style="color: #009900;">(</span>reference_altitude <span style="color: #339933;">-</span> altitude_old<span style="color: #009900;">)</span> <span style="color: #339933;">/</span> <span style="color:#800080;">0.1</span><span style="color: #339933;">;</span><br /> |
||
143 | <span style="color: #009900;">}</span><br /> |
||
144 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>reference_flow_x <span style="color: #339933;">!=</span> xDirection_old<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
145 | xDirection_velocity <span style="color: #339933;">=</span> <span style="color: #009900;">(</span>reference_flow_x <span style="color: #339933;">-</span> xDirection_old<span style="color: #009900;">)</span> <span style="color: #339933;">/</span> <span style="color:#800080;">0.1</span><span style="color: #339933;">;</span><br /> |
||
146 | <span style="color: #009900;">}</span><br /> |
||
147 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>reference_flow_y <span style="color: #339933;">!=</span> yDirection_old<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
148 | yDirection_velocity <span style="color: #339933;">=</span> <span style="color: #009900;">(</span>reference_flow_y <span style="color: #339933;">-</span> yDirection_old<span style="color: #009900;">)</span> <span style="color: #339933;">/</span> <span style="color:#800080;">0.1</span><span style="color: #339933;">;</span><br /> |
||
149 | <span style="color: #009900;">}</span><br /> |
||
150 | <br /> |
||
151 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>altitude_differenz_sum <span style="color: #339933;"><</span> 10000 <span style="color: #339933;">&&</span> altitude_differenz_sum <span style="color: #339933;">></span> <span style="color: #339933;">-</span>10000<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
152 | altitude_differenz_sum <span style="color: #339933;">+=</span> altitude_differenz <span style="color: #339933;">*</span> <span style="color:#800080;">0.1</span><span style="color: #339933;">;</span><br /> |
||
153 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>altitude_differenz_sum <span style="color: #339933;">></span> 9999<span style="color: #009900;">)</span><span style="color: #009900;">{</span>altitude_differenz_sum <span style="color: #339933;">=</span> <span style="color: #0000dd;">9999</span><span style="color: #339933;">;</span><span style="color: #009900;">}</span><span style="color: #b1b100;">else</span> <br /> |
||
154 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>altitude_differenz_sum <span style="color: #339933;"><</span> <span style="color: #339933;">-</span>9999<span style="color: #009900;">)</span><span style="color: #009900;">{</span>altitude_differenz_sum <span style="color: #339933;">=</span> <span style="color: #339933;">-</span><span style="color: #0000dd;">9999</span><span style="color: #339933;">;</span><span style="color: #009900;">}</span><br /> |
||
155 | <span style="color: #009900;">}</span><br /> |
||
156 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>xDirection_differenz_sum <span style="color: #339933;"><</span> 1000 <span style="color: #339933;">&&</span> xDirection_differenz_sum <span style="color: #339933;">></span> <span style="color: #339933;">-</span>1000<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
157 | xDirection_differenz_sum <span style="color: #339933;">+=</span> reference_flow_x<span style="color: #339933;">;</span><br /> |
||
158 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>xDirection_differenz_sum <span style="color: #339933;">></span> 9999<span style="color: #009900;">)</span><span style="color: #009900;">{</span>xDirection_differenz_sum <span style="color: #339933;">=</span> <span style="color: #0000dd;">9999</span><span style="color: #339933;">;</span><span style="color: #009900;">}</span><span style="color: #b1b100;">else</span> <br /> |
||
159 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>xDirection_differenz_sum <span style="color: #339933;"><</span> <span style="color: #339933;">-</span>9999<span style="color: #009900;">)</span><span style="color: #009900;">{</span>xDirection_differenz_sum <span style="color: #339933;">=</span> <span style="color: #339933;">-</span><span style="color: #0000dd;">9999</span><span style="color: #339933;">;</span><span style="color: #009900;">}</span><br /> |
||
160 | <span style="color: #009900;">}</span><br /> |
||
161 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>yDirection_differenz_sum <span style="color: #339933;"><</span> 1000 <span style="color: #339933;">&&</span> yDirection_differenz_sum <span style="color: #339933;">></span> <span style="color: #339933;">-</span>1000<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
162 | yDirection_differenz_sum <span style="color: #339933;">+=</span> reference_flow_y<span style="color: #339933;">;</span><br /> |
||
163 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>yDirection_differenz_sum <span style="color: #339933;">></span> 9999<span style="color: #009900;">)</span><span style="color: #009900;">{</span>yDirection_differenz_sum <span style="color: #339933;">=</span> <span style="color: #0000dd;">9999</span><span style="color: #339933;">;</span><span style="color: #009900;">}</span><span style="color: #b1b100;">else</span> <br /> |
||
164 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>yDirection_differenz_sum <span style="color: #339933;"><</span> <span style="color: #339933;">-</span>9999<span style="color: #009900;">)</span><span style="color: #009900;">{</span>yDirection_differenz_sum <span style="color: #339933;">=</span> <span style="color: #339933;">-</span><span style="color: #0000dd;">9999</span><span style="color: #339933;">;</span><span style="color: #009900;">}</span><br /> |
||
165 | <span style="color: #009900;">}</span><br /> |
||
166 | <br /> |
||
167 | <span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
||
168 | <span style="color: #666666; font-style: italic;">// PID Regelung Gas</span><br /> |
||
169 | gas_correction_ipart <span style="color: #339933;">=</span> iFactorGas <span style="color: #339933;">*</span> altitude_differenz_sum<span style="color: #339933;">;</span><br /> |
||
170 | gas_correction_dpart <span style="color: #339933;">=</span> dFactorGas <span style="color: #339933;">*</span> altitude_velocity <span style="color: #339933;">*-</span><span style="color: #0000dd;">1</span><span style="color: #339933;">;</span><br /> |
||
171 | gas_correction_ppart <span style="color: #339933;">=</span> pFactorGas <span style="color: #339933;">*</span> altitude_differenz<span style="color: #339933;">;</span><br /> |
||
172 | <br /> |
||
173 | gas_correction_final <span style="color: #339933;">=</span> <span style="color: #009900;">(</span>gas_correction_ppart <span style="color: #339933;">+</span> gas_correction_dpart <span style="color: #339933;">+</span> gas_correction_ipart<span style="color: #009900;">)</span> <span style="color: #339933;">/</span> <span style="color: #0000dd;">10</span><span style="color: #339933;">;</span><br /> |
||
174 | <span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
||
175 | <span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
||
176 | <span style="color: #666666; font-style: italic;">// PID Regelung Nick</span><br /> |
||
177 | nick_correction_ipart <span style="color: #339933;">=</span> iFactorNick <span style="color: #339933;">*</span> yDirection_differenz_sum<span style="color: #339933;">;</span><br /> |
||
178 | nick_correction_dpart <span style="color: #339933;">=</span> dFactorNick <span style="color: #339933;">*</span> yDirection_velocity<span style="color: #339933;">;</span><br /> |
||
179 | nick_correction_ppart <span style="color: #339933;">=</span> pFactorNick <span style="color: #339933;">*</span> reference_flow_y<span style="color: #339933;">;</span><br /> |
||
180 | <br /> |
||
181 | nick_correction_final <span style="color: #339933;">=</span> <span style="color: #009900;">(</span>nick_correction_ppart <span style="color: #339933;">+</span> nick_correction_dpart <span style="color: #339933;">+</span> nick_correction_ipart<span style="color: #009900;">)</span> <span style="color: #339933;">/</span> <span style="color: #339933;">-</span><span style="color: #0000dd;">10</span><span style="color: #339933;">;</span><br /> |
||
182 | <span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
||
183 | <span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
||
184 | <span style="color: #666666; font-style: italic;">// PID Regelung Roll</span><br /> |
||
185 | roll_correction_ipart <span style="color: #339933;">=</span> iFactorRoll <span style="color: #339933;">*</span> xDirection_differenz_sum<span style="color: #339933;">;</span><br /> |
||
186 | roll_correction_dpart <span style="color: #339933;">=</span> dFactorRoll <span style="color: #339933;">*</span> xDirection_velocity<span style="color: #339933;">;</span><br /> |
||
187 | roll_correction_ppart <span style="color: #339933;">=</span> pFactorRoll <span style="color: #339933;">*</span> reference_flow_x<span style="color: #339933;">;</span><br /> |
||
188 | <br /> |
||
189 | roll_correction_final <span style="color: #339933;">=</span> <span style="color: #009900;">(</span>roll_correction_ppart <span style="color: #339933;">+</span> roll_correction_dpart <span style="color: #339933;">+</span> roll_correction_ipart<span style="color: #009900;">)</span> <span style="color: #339933;">/</span> <span style="color: #339933;">-</span><span style="color: #0000dd;">10</span><span style="color: #339933;">;</span><br /> |
||
190 | <span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
||
191 | <span style="color: #666666; font-style: italic;">//printf("Final: %f PPART: %f DPART: %f\n", nick_correction_final, nick_correction_ppart, nick_correction_dpart);</span><br /> |
||
192 | <br /> |
||
193 | <span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
||
194 | <span style="color: #666666; font-style: italic;">// Set Limits</span><br /> |
||
195 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>gas_correction_final <span style="color: #339933;">>=</span> 20<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
196 | fc_correction_data.<span style="color: #202020;">Gas</span> <span style="color: #339933;">=</span> <span style="color: #0000dd;">20</span><span style="color: #339933;">;</span><br /> |
||
197 | <span style="color: #009900;">}</span><span style="color: #b1b100;">else</span> <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>gas_correction_final <span style="color: #339933;"><=</span> <span style="color: #339933;">-</span>20<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
198 | fc_correction_data.<span style="color: #202020;">Gas</span> <span style="color: #339933;">=</span> <span style="color: #339933;">-</span><span style="color: #0000dd;">20</span><span style="color: #339933;">;</span><br /> |
||
199 | <span style="color: #009900;">}</span><span style="color: #b1b100;">else</span><span style="color: #009900;">{</span><br /> |
||
200 | fc_correction_data.<span style="color: #202020;">Gas</span> <span style="color: #339933;">=</span> gas_correction_final<span style="color: #339933;">;</span><br /> |
||
201 | <span style="color: #009900;">}</span><br /> |
||
202 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>nick_correction_final <span style="color: #339933;">>=</span> 20<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
203 | fc_correction_data.<span style="color: #202020;">Nick</span> <span style="color: #339933;">=</span> <span style="color: #0000dd;">20</span><span style="color: #339933;">;</span><br /> |
||
204 | <span style="color: #009900;">}</span><span style="color: #b1b100;">else</span> <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>nick_correction_final <span style="color: #339933;"><=</span> <span style="color: #339933;">-</span>20<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
205 | fc_correction_data.<span style="color: #202020;">Nick</span> <span style="color: #339933;">=</span> <span style="color: #339933;">-</span><span style="color: #0000dd;">20</span><span style="color: #339933;">;</span><br /> |
||
206 | <span style="color: #009900;">}</span><span style="color: #b1b100;">else</span><span style="color: #009900;">{</span><br /> |
||
207 | fc_correction_data.<span style="color: #202020;">Nick</span> <span style="color: #339933;">=</span> nick_correction_final<span style="color: #339933;">;</span><br /> |
||
208 | <span style="color: #009900;">}</span><br /> |
||
209 | <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>roll_correction_final <span style="color: #339933;">>=</span> 20<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
210 | fc_correction_data.<span style="color: #202020;">Roll</span> <span style="color: #339933;">=</span> <span style="color: #0000dd;">20</span><span style="color: #339933;">;</span><br /> |
||
211 | <span style="color: #009900;">}</span><span style="color: #b1b100;">else</span> <span style="color: #b1b100;">if</span><span style="color: #009900;">(</span>roll_correction_final <span style="color: #339933;"><=</span> <span style="color: #339933;">-</span>20<span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
212 | fc_correction_data.<span style="color: #202020;">Roll</span> <span style="color: #339933;">=</span> <span style="color: #339933;">-</span><span style="color: #0000dd;">20</span><span style="color: #339933;">;</span><br /> |
||
213 | <span style="color: #009900;">}</span><span style="color: #b1b100;">else</span><span style="color: #009900;">{</span><br /> |
||
214 | fc_correction_data.<span style="color: #202020;">Roll</span> <span style="color: #339933;">=</span> roll_correction_final<span style="color: #339933;">;</span><br /> |
||
215 | <span style="color: #009900;">}</span><br /> |
||
216 | <br /> |
||
217 | <span style="color: #666666; font-style: italic;">//printf("%d\n", reference_altitude/10);</span><br /> |
||
218 | <a href="http://www.opengroup.org/onlinepubs/009695399/functions/printf.html"><span style="color: #000066;">printf</span></a><span style="color: #009900;">(</span><span style="color: #ff0000;">"Final: %f PPART: %f DPART: %f IPART: %f Gas Correction: %d<span style="color: #000099; font-weight: bold;">\n</span>"</span><span style="color: #339933;">,</span> gas_correction_final<span style="color: #339933;">,</span> gas_correction_ppart<span style="color: #339933;">,</span> gas_correction_dpart<span style="color: #339933;">,</span> gas_correction_ipart<span style="color: #339933;">,</span> fc_correction_data.<span style="color: #202020;">Gas</span> <span style="color: #009900;">)</span><span style="color: #339933;">;</span><br /> |
||
219 | <span style="color: #666666; font-style: italic;">//printf("Final: %f PPART: %f DPART: %f IPART: %f Gas Correction: %d\n", nick_correction_final, nick_correction_ppart, nick_correction_dpart, nick_correction_ipart, fc_correction_data.Nick );</span><br /> |
||
220 | fc_correction_data.<span style="color: #202020;">Gas</span> <span style="color: #339933;">=</span> <span style="color: #0000dd;">0</span><span style="color: #339933;">;</span><br /> |
||
221 | <span style="color: #666666; font-style: italic;">//fc_correction_data.Roll = 0;</span><br /> |
||
222 | <span style="color: #666666; font-style: italic;">//fc_correction_data.Nick = 0;</span><br /> |
||
223 | <br /> |
||
224 | <br /> |
||
225 | altitude_old <span style="color: #339933;">=</span> reference_altitude<span style="color: #339933;">;</span><br /> |
||
226 | xDirection_old <span style="color: #339933;">=</span> reference_flow_x<span style="color: #339933;">;</span><br /> |
||
227 | yDirection_old <span style="color: #339933;">=</span> reference_flow_y<span style="color: #339933;">;</span><br /> |
||
228 | <span style="color: #009900;">}</span><br /> |
||
229 | <br /> |
||
230 | <span style="color: #666666; font-style: italic;">//>> Transmitting Correction Data</span><br /> |
||
231 | <span style="color: #666666; font-style: italic;">//------------------------------------------------------------------------------------------------------</span><br /> |
||
232 | u8 <span style="color: #339933;">*</span>flight_values_stream<span style="color: #339933;">;</span><br /> |
||
233 | serial_data_struct data_package_flightcontrol<span style="color: #339933;">;</span><br /> |
||
234 | <span style="color: #993333;">void</span> transmit_flight_data<span style="color: #009900;">(</span><span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
235 | flight_values_stream <span style="color: #339933;">=</span> <span style="color: #009900;">(</span><span style="color: #993333;">unsigned</span> <span style="color: #993333;">char</span> <span style="color: #339933;">*</span><span style="color: #009900;">)</span> <span style="color: #339933;">&</span>fc_correction_data<span style="color: #339933;">;</span><br /> |
||
236 | <span style="color: #666666; font-style: italic;">//printf("Correction: %d\n", fc_correction_data.Gas);</span><br /> |
||
237 | <span style="color: #b1b100;">for</span> <span style="color: #009900;">(</span><span style="color: #993333;">int</span> i <span style="color: #339933;">=</span> <span style="color: #0000dd;">0</span><span style="color: #339933;">;</span> i <span style="color: #339933;"><</span> <span style="color: #993333;">sizeof</span><span style="color: #009900;">(</span><span style="color: #993333;">struct</span> str_fc_correction_data<span style="color: #009900;">)</span><span style="color: #339933;">;</span> <span style="color: #339933;">++</span>i<span style="color: #009900;">)</span><br /> |
||
238 | <span style="color: #009900;">{</span><br /> |
||
239 | data_package_flightcontrol.<span style="color: #202020;">data</span><span style="color: #009900;">[</span>i<span style="color: #009900;">]</span> <span style="color: #339933;">=</span> flight_values_stream<span style="color: #009900;">[</span>i<span style="color: #009900;">]</span><span style="color: #339933;">;</span><br /> |
||
240 | <span style="color: #009900;">}</span><br /> |
||
241 | create_serial_frame<span style="color: #009900;">(</span><span style="color: #0000dd;">1</span><span style="color: #339933;">,</span> <span style="color: #ff0000;">'b'</span><span style="color: #339933;">,</span> <span style="color: #993333;">sizeof</span><span style="color: #009900;">(</span><span style="color: #993333;">struct</span> str_fc_correction_data<span style="color: #009900;">)</span><span style="color: #339933;">,</span> <span style="color: #339933;">&</span>data_package_flightcontrol<span style="color: #009900;">)</span><span style="color: #339933;">;</span><br /> |
||
242 | <span style="color: #666666; font-style: italic;">//printf("%s\n", data_package_flightcontrol.txrxdata);</span><br /> |
||
243 | transmit_data<span style="color: #009900;">(</span>TO_KOPTER<span style="color: #339933;">,</span> <span style="color: #339933;">&</span>data_package_flightcontrol<span style="color: #009900;">)</span><span style="color: #339933;">;</span><br /> |
||
244 | <span style="color: #009900;">}</span><br /> |
||
245 | <br /> |
||
246 | <span style="color: #666666; font-style: italic;">//>> Resetting Flow and Altitude Values</span><br /> |
||
247 | <span style="color: #666666; font-style: italic;">//------------------------------------------------------------------------------------------------------</span><br /> |
||
248 | <span style="color: #993333;">void</span> reset_reference_values<span style="color: #009900;">(</span><span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
249 | reference_flow_x <span style="color: #339933;">=</span> <span style="color:#800080;">0.0</span><span style="color: #339933;">;</span><br /> |
||
250 | reference_flow_y <span style="color: #339933;">=</span> <span style="color:#800080;">0.0</span><span style="color: #339933;">;</span><br /> |
||
251 | reference_altitude <span style="color: #339933;">=</span> <span style="color:#800080;">0.0</span><span style="color: #339933;">;</span><br /> |
||
252 | <span style="color: #009900;">}</span><br /> |
||
253 | <br /> |
||
254 | <span style="color: #666666; font-style: italic;">//>> Initializing flightcontrol</span><br /> |
||
255 | <span style="color: #666666; font-style: italic;">//------------------------------------------------------------------------------------------------------</span><br /> |
||
256 | <span style="color: #993333;">void</span> flightcontrol_init<span style="color: #009900;">(</span><span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
||
257 | fc_correction_data.<span style="color: #202020;">Nick</span> <span style="color: #339933;">=</span> <span style="color: #0000dd;">0</span><span style="color: #339933;">;</span> <br /> |
||
258 | fc_correction_data.<span style="color: #202020;">Roll</span> <span style="color: #339933;">=</span> <span style="color: #0000dd;">0</span><span style="color: #339933;">;</span><br /> |
||
259 | fc_correction_data.<span style="color: #202020;">Gier</span> <span style="color: #339933;">=</span> <span style="color: #0000dd;">0</span><span style="color: #339933;">;</span><br /> |
||
260 | fc_correction_data.<span style="color: #202020;">Gas</span> <span style="color: #339933;">=</span> <span style="color: #0000dd;">0</span><span style="color: #339933;">;</span><br /> |
||
261 | fc_correction_data.<span style="color: #202020;">Config</span> <span style="color: #339933;">=</span> EC_VALID <span style="color: #339933;">|</span> EC_GAS_ADD <span style="color: #339933;">|</span> EC_IGNORE_RC<span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Ignore Remote Control</span><br /> |
||
262 | <span style="color: #009900;">}</span><br /> |
||
263 | </div> |
||
264 | </div> |
||
265 | <div id="footer"> |
||
266 | <p style="padding:0; margin:0"><small>Powered by <a href="http://www.websvn.info/">WebSVN</a> 2.3.3 und <a href="http://subversion.tigris.org">Subversion</a> 1.6.17 ✓ <a href="http://validator.w3.org/check?uri=http://svn.mikrokopter.de/websvn/filedetails.php?repname=Projects%26path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c%26template=%26language=de">XHTML</a> & <a href="http://jigsaw.w3.org/css-validator/validator?uri=http://svn.mikrokopter.de/websvn/filedetails.php?repname=Projects%26path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c%26template=%26language=de">CSS</a></small></p> |
||
267 | </div> |
||
268 | </body> |
||
269 | </html> |