Details | 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> |