<!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">&#
20013;&#
25991; - Chinese
(Simplified
)<
/option><
option value="zh-TW">&#
20013;&#
25991; - Chinese
(Traditional
)<
/option><
option value="cs">&#
268;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">&#
1506;&#
1489;&#
1512;&#
1497;&#
1514; - Hebrew<
/option><
option value="hin">&#
2361;&#
2367;&#
2306;&#
2342;&#
2368; - Hindi<
/option><
option value="hu">Magyar - Hungarian<
/option><
option value="id">Bahasa Indonesia - Indonesian<
/option><
option value="it">Italiano - Italian<
/option><
option value="ja">&#
26085;&#
26412;&#
35486; - Japanese<
/option><
option value="ko">&#
54620;&#
44397;&#
50612; - Korean<
/option><
option value="mk">&#
1052;&#
1072;&#
1082;&#
1077;&#
1076;&#
1086;&#
1085;&#
1089;&#
1082;&#
1080; - Macedonian<
/option><
option value="mr">&#
2350;&#
2352;&#
2366;&#
2336;&#
2368; - 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">&#
1056;&#
1091;&#
1089;&#
1089;&#
1082;&#
1080;&#
1081; - Russian<
/option><
option value="sk">Sloven&#
269;ina - Slovak<
/option><
option value="sl">Sloven&#
353;&#
269;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">&#
1059;&#
1082;&#
1088;&#
1072;&#
1111;&#
1085;&#
1089;&#
1100;&#
1082;&#
1072; - 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> &#
124;
<
span class="changes"><
a href="revision.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&rev=2105">Letzte Änderung<
/a><
/span> &#
124;
<
span class="log"><
a href="log.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&rev=2105">Log anzeigen<
/a><
/span>
&#
124; <
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;">&#
40;<
/span><
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/span><
br />
reference_altitude <
span style="color: #339933;">
=<
/span> <
span style="color: #009900;">&#
40;<
/span><
span style="color: #993333;">int<
/span><
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
40;<
/span>optical_flow_values.<
span style="color: #202020;">ground_distance<
/span><
span style="color: #339933;">*<
/span>
1000<
span style="color: #009900;">&#
41;<
/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;">&#
40;<
/span><
span style="color: #993333;">int<
/span><
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
40;<
/span>optical_flow_values.<
span style="color: #202020;">flow_comp_m_x<
/span><
span style="color: #339933;">*<
/span>
1000<
span style="color: #009900;">&#
41;<
/span><
span style="color: #339933;">;<
/span><
br />
reference_flow_y <
span style="color: #339933;">+
=<
/span> <
span style="color: #009900;">&#
40;<
/span><
span style="color: #993333;">int<
/span><
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
40;<
/span>optical_flow_values.<
span style="color: #202020;">flow_comp_m_y<
/span><
span style="color: #339933;">*<
/span>
1000<
span style="color: #009900;">&#
41;<
/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;">&#
40;<
/span>reference_altitude <
span style="color: #339933;">!
=<
/span> altitude_old<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/span><
br />
altitude_velocity <
span style="color: #339933;">
=<
/span> <
span style="color: #009900;">&#
40;<
/span>reference_altitude <
span style="color: #339933;">-<
/span> altitude_old<
span style="color: #009900;">&#
41;<
/span> <
span style="color: #339933;">
/<
/span> <
span style="color:#800080;">
0.1<
/span><
span style="color: #339933;">;<
/span><
br />
<
span style="color: #009900;">&#
125;<
/span><
br />
<
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/span>reference_flow_x <
span style="color: #339933;">!
=<
/span> xDirection_old<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/span><
br />
xDirection_velocity <
span style="color: #339933;">
=<
/span> <
span style="color: #009900;">&#
40;<
/span>reference_flow_x <
span style="color: #339933;">-<
/span> xDirection_old<
span style="color: #009900;">&#
41;<
/span> <
span style="color: #339933;">
/<
/span> <
span style="color:#800080;">
0.1<
/span><
span style="color: #339933;">;<
/span><
br />
<
span style="color: #009900;">&#
125;<
/span><
br />
<
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/span>reference_flow_y <
span style="color: #339933;">!
=<
/span> yDirection_old<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/span><
br />
yDirection_velocity <
span style="color: #339933;">
=<
/span> <
span style="color: #009900;">&#
40;<
/span>reference_flow_y <
span style="color: #339933;">-<
/span> yDirection_old<
span style="color: #009900;">&#
41;<
/span> <
span style="color: #339933;">
/<
/span> <
span style="color:#800080;">
0.1<
/span><
span style="color: #339933;">;<
/span><
br />
<
span style="color: #009900;">&#
125;<
/span><
br />
<
br />
<
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/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;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
40;<
/span>altitude_differenz_sum <
span style="color: #339933;">><
/span>
9999<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/span>altitude_differenz_sum <
span style="color: #339933;">
=<
/span> <
span style="color: #0000dd;">
9999<
/span><
span style="color: #339933;">;<
/span><
span style="color: #009900;">&#
125;<
/span><
span style="color: #b1b100;">else<
/span> <
br />
<
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/span>altitude_differenz_sum <
span style="color: #339933;"><<
/span> <
span style="color: #339933;">-<
/span>
9999<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/span><
br />
<
span style="color: #009900;">&#
125;<
/span><
br />
<
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/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;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
40;<
/span>xDirection_differenz_sum <
span style="color: #339933;">><
/span>
9999<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/span>xDirection_differenz_sum <
span style="color: #339933;">
=<
/span> <
span style="color: #0000dd;">
9999<
/span><
span style="color: #339933;">;<
/span><
span style="color: #009900;">&#
125;<
/span><
span style="color: #b1b100;">else<
/span> <
br />
<
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/span>xDirection_differenz_sum <
span style="color: #339933;"><<
/span> <
span style="color: #339933;">-<
/span>
9999<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/span><
br />
<
span style="color: #009900;">&#
125;<
/span><
br />
<
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/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;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
40;<
/span>yDirection_differenz_sum <
span style="color: #339933;">><
/span>
9999<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/span>yDirection_differenz_sum <
span style="color: #339933;">
=<
/span> <
span style="color: #0000dd;">
9999<
/span><
span style="color: #339933;">;<
/span><
span style="color: #009900;">&#
125;<
/span><
span style="color: #b1b100;">else<
/span> <
br />
<
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/span>yDirection_differenz_sum <
span style="color: #339933;"><<
/span> <
span style="color: #339933;">-<
/span>
9999<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/span><
br />
<
span style="color: #009900;">&#
125;<
/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;">&#
40;<
/span>gas_correction_ppart <
span style="color: #339933;">+<
/span> gas_correction_dpart <
span style="color: #339933;">+<
/span> gas_correction_ipart<
span style="color: #009900;">&#
41;<
/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;">&#
40;<
/span>nick_correction_ppart <
span style="color: #339933;">+<
/span> nick_correction_dpart <
span style="color: #339933;">+<
/span> nick_correction_ipart<
span style="color: #009900;">&#
41;<
/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;">&#
40;<
/span>roll_correction_ppart <
span style="color: #339933;">+<
/span> roll_correction_dpart <
span style="color: #339933;">+<
/span> roll_correction_ipart<
span style="color: #009900;">&#
41;<
/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;">&#
40;<
/span>gas_correction_final <
span style="color: #339933;">>
=<
/span>
20<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/span><
span style="color: #b1b100;">else<
/span> <
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/span>gas_correction_final <
span style="color: #339933;"><
=<
/span> <
span style="color: #339933;">-<
/span>
20<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/span><
span style="color: #b1b100;">else<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/span><
br />
<
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/span>nick_correction_final <
span style="color: #339933;">>
=<
/span>
20<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/span><
span style="color: #b1b100;">else<
/span> <
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/span>nick_correction_final <
span style="color: #339933;"><
=<
/span> <
span style="color: #339933;">-<
/span>
20<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/span><
span style="color: #b1b100;">else<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/span><
br />
<
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/span>roll_correction_final <
span style="color: #339933;">>
=<
/span>
20<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/span><
span style="color: #b1b100;">else<
/span> <
span style="color: #b1b100;">if<
/span><
span style="color: #009900;">&#
40;<
/span>roll_correction_final <
span style="color: #339933;"><
=<
/span> <
span style="color: #339933;">-<
/span>
20<
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/span><
span style="color: #b1b100;">else<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/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;">&#
40;<
/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;">&#
41;<
/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;">&#
125;<
/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;">&#
40;<
/span><
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/span><
br />
flight_values_stream <
span style="color: #339933;">
=<
/span> <
span style="color: #009900;">&#
40;<
/span><
span style="color: #993333;">unsigned<
/span> <
span style="color: #993333;">char<
/span> <
span style="color: #339933;">*<
/span><
span style="color: #009900;">&#
41;<
/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;">&#
40;<
/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;">&#
40;<
/span><
span style="color: #993333;">struct<
/span> str_fc_correction_data<
span style="color: #009900;">&#
41;<
/span><
span style="color: #339933;">;<
/span> <
span style="color: #339933;">++<
/span>i<
span style="color: #009900;">&#
41;<
/span><
br />
<
span style="color: #009900;">&#
123;<
/span><
br />
data_package_flightcontrol.<
span style="color: #202020;">data<
/span><
span style="color: #009900;">&#
91;<
/span>i<
span style="color: #009900;">&#
93;<
/span> <
span style="color: #339933;">
=<
/span> flight_values_stream<
span style="color: #009900;">&#
91;<
/span>i<
span style="color: #009900;">&#
93;<
/span><
span style="color: #339933;">;<
/span><
br />
<
span style="color: #009900;">&#
125;<
/span><
br />
create_serial_frame<
span style="color: #009900;">&#
40;<
/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;">&#
40;<
/span><
span style="color: #993333;">struct<
/span> str_fc_correction_data<
span style="color: #009900;">&#
41;<
/span><
span style="color: #339933;">,<
/span> <
span style="color: #339933;">&<
/span>data_package_flightcontrol<
span style="color: #009900;">&#
41;<
/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;">&#
40;<
/span>TO_KOPTER<
span style="color: #339933;">,<
/span> <
span style="color: #339933;">&<
/span>data_package_flightcontrol<
span style="color: #009900;">&#
41;<
/span><
span style="color: #339933;">;<
/span><
br />
<
span style="color: #009900;">&#
125;<
/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;">&#
40;<
/span><
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/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;">&#
40;<
/span><
span style="color: #009900;">&#
41;<
/span><
span style="color: #009900;">&#
123;<
/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;">&#
125;<
/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>