0,0 → 1,269 |
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd"> |
<html xmlns="http://www.w3.org/1999/xhtml" xml:lang="de" lang="de"> |
<head> |
<meta http-equiv="content-type" content="text/html;charset=UTF-8" /> |
<meta http-equiv="generator" content="WebSVN 2.3.3" /> <!-- leave this for stats --> |
<link rel="shortcut icon" type="image/x-icon" href="./templates/mkstyle/images/favicon.ico" /> |
<link type="text/css" href="./templates/mkstyle/styles.css" rel="stylesheet" media="screen" /> |
<link rel='alternate' type='application/rss+xml' title='WebSVN RSS' href='rss.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&' /> |
<!--[if gte IE 5.5000]> |
<script type="text/javascript" src="./templates/mkstyle/png.js"></script> |
<style type="text/css" media="screen"> |
tbody tr td { padding:1px 0 } |
#wrap h2 { padding:10px 5px 0 5px; margin-bottom:-8px } |
</style> |
<![endif]--> |
<title> |
WebSVN |
- Projects |
- Revision 2105 |
- /RaspberryPi/ExPlat/flightcontrol.c |
</title> |
<script type="text/javascript"> |
//<![CDATA[ |
function getPath() |
{ |
return '.'; |
} |
|
function checkCB(chBox) |
{ |
count = 0 |
first = null |
f = chBox.form |
for (i = 0 ; i < f.elements.length ; i++) |
if (f.elements[i].type == 'checkbox' && f.elements[i].checked) |
{ |
if (first == null && f.elements[i] != chBox) |
first = f.elements[i] |
count += 1 |
} |
|
if (count > 2) |
{ |
first.checked = false |
count -= 1 |
} |
} |
//]]> |
</script> |
</head> |
<body id="file"> |
<div id="container"> |
<div id="usebbnetnavigation"> |
<ul> |
|
<li><a href="http://www.mikrokopter.de" target="_blank">MikroKopter.de</a></li> |
<li><a href="http://www.mikrokopter.de/ucwiki/" target="_blank">Wiki</a></li> |
<li><a href="http://forum.mikrokopter.de" >Forum</a></li> |
<li><a href="http://gallery3.mikrokopter.de" target="_blank">Bilder</a></li> |
<li><a href="http://svn.mikrokopter.de" target="_blank">SVN</a></li> |
<li><a href="http://wiki.mikrokopter.de/VideoListe" target="_blank">Videos</a></li> |
<li><a href="http://wiki.mikrokopter.de/MikroKopter-FAQ" target="_blank">FAQ</a></li> |
<li><a href="http://map.mikrokopter.de" target="_blank">Map-Tool</a></li> |
<li><a href="http://www.multikoptermap.de/" target="_blank">User-Map</a></li> |
<li><a href="https://www.mikrocontroller.com" target="_blank">SHOP</a></li> |
<span class="rightalign"> |
<li><a href="http://wiki.mikrokopter.de/Datenschutz" target="_blank">Datenschutz</a></li> |
<li><a href="http://wiki.mikrokopter.de/Impressum" target="_blank">Impressum</a></li> |
</span> |
<!--<li></li> --> |
</ul> |
</div> |
<div id="select"> |
<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> |
|
<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> |
</div> |
<h1><a href=".?" title="Subversion-Projekte">Subversion-Projekte</a> |
<span><a href="listing.php?repname=Projects&">Projects</a></span> |
</h1> |
<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> |
<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> |
<p> |
<span class="blame"><a href="blame.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&rev=2105">Blame</a></span> | |
<span class="changes"><a href="revision.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&rev=2105">Letzte Änderung</a></span> | |
<span class="log"><a href="log.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&rev=2105">Log anzeigen</a></span> |
| <span class="feed"><a href="rss.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&">RSS feed</a></span> |
</p> |
<div class="listing"> |
<span style="color: #339933;">#include "main.h"</span><br /> |
<span style="color: #339933;">#include "flightcontrol.h"</span><br /> |
<br /> |
<span style="color: #993333;">int</span> reference_altitude<span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Regelgröße</span><br /> |
<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 /> |
<span style="color: #993333;">int</span> reference_flow_x<span style="color: #339933;">;</span><br /> |
<span style="color: #993333;">int</span> reference_flow_y<span style="color: #339933;">;</span><br /> |
<br /> |
<span style="color: #993333;">int</span> altitude_differenz<span style="color: #339933;">;</span> <span style="color: #666666; font-style: italic;">//Regelabweichung</span><br /> |
<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 /> |
<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 /> |
<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 /> |
<br /> |
<span style="color: #993333;">int</span> altitude_old<span style="color: #339933;">;</span><br /> |
<span style="color: #993333;">int</span> altitude_velocity<span style="color: #339933;">;</span><br /> |
<span style="color: #993333;">int</span> xDirection_old<span style="color: #339933;">;</span><br /> |
<span style="color: #993333;">int</span> xDirection_velocity<span style="color: #339933;">;</span><br /> |
<span style="color: #993333;">int</span> yDirection_old<span style="color: #339933;">;</span><br /> |
<span style="color: #993333;">int</span> yDirection_velocity<span style="color: #339933;">;</span><br /> |
<br /> |
<span style="color: #993333;">float</span> gas_correction_dpart<span style="color: #339933;">;</span><br /> |
<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 /> |
<span style="color: #993333;">float</span> gas_correction_ppart<span style="color: #339933;">;</span><br /> |
<span style="color: #993333;">float</span> gas_correction_final<span style="color: #339933;">;</span><br /> |
<span style="color: #993333;">float</span> nick_correction_dpart<span style="color: #339933;">;</span><br /> |
<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 /> |
<span style="color: #993333;">float</span> nick_correction_ppart<span style="color: #339933;">;</span><br /> |
<span style="color: #993333;">float</span> nick_correction_final<span style="color: #339933;">;</span><br /> |
<span style="color: #993333;">float</span> roll_correction_dpart<span style="color: #339933;">;</span><br /> |
<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 /> |
<span style="color: #993333;">float</span> roll_correction_ppart<span style="color: #339933;">;</span><br /> |
<span style="color: #993333;">float</span> roll_correction_final<span style="color: #339933;">;</span><br /> |
<br /> |
<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 /> |
<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 /> |
<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 /> |
<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 /> |
<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 /> |
<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 /> |
<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 /> |
<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 /> |
<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 /> |
<br /> |
<span style="color: #666666; font-style: italic;">//>> Performing Flight Correction</span><br /> |
<span style="color: #666666; font-style: italic;">//------------------------------------------------------------------------------------------------------</span><br /> |
<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 /> |
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 /> |
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 /> |
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 /> |
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 /> |
<br /> |
<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 /> |
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 /> |
<span style="color: #009900;">}</span><br /> |
<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 /> |
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 /> |
<span style="color: #009900;">}</span><br /> |
<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 /> |
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 /> |
<span style="color: #009900;">}</span><br /> |
<br /> |
<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 /> |
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 /> |
<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 /> |
<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 /> |
<span style="color: #009900;">}</span><br /> |
<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 /> |
xDirection_differenz_sum <span style="color: #339933;">+=</span> reference_flow_x<span style="color: #339933;">;</span><br /> |
<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 /> |
<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 /> |
<span style="color: #009900;">}</span><br /> |
<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 /> |
yDirection_differenz_sum <span style="color: #339933;">+=</span> reference_flow_y<span style="color: #339933;">;</span><br /> |
<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 /> |
<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 /> |
<span style="color: #009900;">}</span><br /> |
<br /> |
<span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
<span style="color: #666666; font-style: italic;">// PID Regelung Gas</span><br /> |
gas_correction_ipart <span style="color: #339933;">=</span> iFactorGas <span style="color: #339933;">*</span> altitude_differenz_sum<span style="color: #339933;">;</span><br /> |
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 /> |
gas_correction_ppart <span style="color: #339933;">=</span> pFactorGas <span style="color: #339933;">*</span> altitude_differenz<span style="color: #339933;">;</span><br /> |
<br /> |
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 /> |
<span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
<span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
<span style="color: #666666; font-style: italic;">// PID Regelung Nick</span><br /> |
nick_correction_ipart <span style="color: #339933;">=</span> iFactorNick <span style="color: #339933;">*</span> yDirection_differenz_sum<span style="color: #339933;">;</span><br /> |
nick_correction_dpart <span style="color: #339933;">=</span> dFactorNick <span style="color: #339933;">*</span> yDirection_velocity<span style="color: #339933;">;</span><br /> |
nick_correction_ppart <span style="color: #339933;">=</span> pFactorNick <span style="color: #339933;">*</span> reference_flow_y<span style="color: #339933;">;</span><br /> |
<br /> |
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 /> |
<span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
<span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
<span style="color: #666666; font-style: italic;">// PID Regelung Roll</span><br /> |
roll_correction_ipart <span style="color: #339933;">=</span> iFactorRoll <span style="color: #339933;">*</span> xDirection_differenz_sum<span style="color: #339933;">;</span><br /> |
roll_correction_dpart <span style="color: #339933;">=</span> dFactorRoll <span style="color: #339933;">*</span> xDirection_velocity<span style="color: #339933;">;</span><br /> |
roll_correction_ppart <span style="color: #339933;">=</span> pFactorRoll <span style="color: #339933;">*</span> reference_flow_x<span style="color: #339933;">;</span><br /> |
<br /> |
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 /> |
<span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
<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 /> |
<br /> |
<span style="color: #666666; font-style: italic;">//////////////////////////////////////////////////////////////////////////</span><br /> |
<span style="color: #666666; font-style: italic;">// Set Limits</span><br /> |
<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 /> |
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 /> |
<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 /> |
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 /> |
<span style="color: #009900;">}</span><span style="color: #b1b100;">else</span><span style="color: #009900;">{</span><br /> |
fc_correction_data.<span style="color: #202020;">Gas</span> <span style="color: #339933;">=</span> gas_correction_final<span style="color: #339933;">;</span><br /> |
<span style="color: #009900;">}</span><br /> |
<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 /> |
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 /> |
<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 /> |
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 /> |
<span style="color: #009900;">}</span><span style="color: #b1b100;">else</span><span style="color: #009900;">{</span><br /> |
fc_correction_data.<span style="color: #202020;">Nick</span> <span style="color: #339933;">=</span> nick_correction_final<span style="color: #339933;">;</span><br /> |
<span style="color: #009900;">}</span><br /> |
<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 /> |
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 /> |
<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 /> |
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 /> |
<span style="color: #009900;">}</span><span style="color: #b1b100;">else</span><span style="color: #009900;">{</span><br /> |
fc_correction_data.<span style="color: #202020;">Roll</span> <span style="color: #339933;">=</span> roll_correction_final<span style="color: #339933;">;</span><br /> |
<span style="color: #009900;">}</span><br /> |
<br /> |
<span style="color: #666666; font-style: italic;">//printf("%d\n", reference_altitude/10);</span><br /> |
<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 /> |
<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 /> |
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 /> |
<span style="color: #666666; font-style: italic;">//fc_correction_data.Roll = 0;</span><br /> |
<span style="color: #666666; font-style: italic;">//fc_correction_data.Nick = 0;</span><br /> |
<br /> |
<br /> |
altitude_old <span style="color: #339933;">=</span> reference_altitude<span style="color: #339933;">;</span><br /> |
xDirection_old <span style="color: #339933;">=</span> reference_flow_x<span style="color: #339933;">;</span><br /> |
yDirection_old <span style="color: #339933;">=</span> reference_flow_y<span style="color: #339933;">;</span><br /> |
<span style="color: #009900;">}</span><br /> |
<br /> |
<span style="color: #666666; font-style: italic;">//>> Transmitting Correction Data</span><br /> |
<span style="color: #666666; font-style: italic;">//------------------------------------------------------------------------------------------------------</span><br /> |
u8 <span style="color: #339933;">*</span>flight_values_stream<span style="color: #339933;">;</span><br /> |
serial_data_struct data_package_flightcontrol<span style="color: #339933;">;</span><br /> |
<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 /> |
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 /> |
<span style="color: #666666; font-style: italic;">//printf("Correction: %d\n", fc_correction_data.Gas);</span><br /> |
<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 /> |
<span style="color: #009900;">{</span><br /> |
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 /> |
<span style="color: #009900;">}</span><br /> |
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 /> |
<span style="color: #666666; font-style: italic;">//printf("%s\n", data_package_flightcontrol.txrxdata);</span><br /> |
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 /> |
<span style="color: #009900;">}</span><br /> |
<br /> |
<span style="color: #666666; font-style: italic;">//>> Resetting Flow and Altitude Values</span><br /> |
<span style="color: #666666; font-style: italic;">//------------------------------------------------------------------------------------------------------</span><br /> |
<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 /> |
reference_flow_x <span style="color: #339933;">=</span> <span style="color:#800080;">0.0</span><span style="color: #339933;">;</span><br /> |
reference_flow_y <span style="color: #339933;">=</span> <span style="color:#800080;">0.0</span><span style="color: #339933;">;</span><br /> |
reference_altitude <span style="color: #339933;">=</span> <span style="color:#800080;">0.0</span><span style="color: #339933;">;</span><br /> |
<span style="color: #009900;">}</span><br /> |
<br /> |
<span style="color: #666666; font-style: italic;">//>> Initializing flightcontrol</span><br /> |
<span style="color: #666666; font-style: italic;">//------------------------------------------------------------------------------------------------------</span><br /> |
<span style="color: #993333;">void</span> flightcontrol_init<span style="color: #009900;">(</span><span style="color: #009900;">)</span><span style="color: #009900;">{</span><br /> |
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 /> |
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 /> |
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 /> |
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 /> |
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 /> |
<span style="color: #009900;">}</span><br /> |
</div> |
</div> |
<div id="footer"> |
<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> |
</div> |
</body> |
</html> |