WebSVN - Projects - Rev 2339 - /RaspberryPi/ExPlat/unknown/filedetails.htm

<!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" /> 
<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&' />

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

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


</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>
<a href="listing.php?repname=Projects&">Projects</a>
</h1>
<h2 id="pathlinks"><a href="listing.php?repname=Projects&path=%2F&" class="root">(root)</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>/flightcontrol.c - 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" /><input type="submit" value="Los" /></form></div>

<a href="blame.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&rev=2105">Blame</a> |
<a href="revision.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&rev=2105">Letzte Änderung</a> |
<a href="log.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&rev=2105">Log anzeigen</a>
| <a href="rss.php?repname=Projects&path=%2FRaspberryPi%2FExPlat%2Fflightcontrol.c&">RSS feed</a>

<div class="listing">
#include "main.h" 
#include "flightcontrol.h" 
 
int reference_altitude;                                 //Regelgröße 
int reference_accz;                                             //Regelgröße D-Anteil 
int reference_flow_x; 
int reference_flow_y; 
 
int altitude_differenz;                                 //Regelabweichung 
int altitude_differenz_sum = 0;                 //I-Glied 
int xDirection_differenz_sum = 0; 
int yDirection_differenz_sum = 0; 
 
int altitude_old; 
int altitude_velocity; 
int xDirection_old; 
int xDirection_velocity; 
int yDirection_old; 
int yDirection_velocity; 
 
float gas_correction_dpart; 
float gas_correction_ipart = 0.0; 
float gas_correction_ppart; 
float gas_correction_final; 
float nick_correction_dpart; 
float nick_correction_ipart = 0.0; 
float nick_correction_ppart; 
float nick_correction_final; 
float roll_correction_dpart; 
float roll_correction_ipart = 0.0; 
float roll_correction_ppart; 
float roll_correction_final; 
 
float pFactorGas = 0.3;                                 //Verstärkungsfaktor P 
float iFactorGas = 0.02;                                //Verstärkungsfaktor I 
float dFactorGas = 0.3;                                 //Verstärkungsfaktor D 
float pFactorNick = 0.2;                                //Verstärkungsfaktor P 
float iFactorNick = 0.02;                               //Verstärkungsfaktor I 
float dFactorNick = 0.3;                                //Verstärkungsfaktor D 
float pFactorRoll = 0.2;                                //Verstärkungsfaktor P 
float iFactorRoll = 0.02;                               //Verstärkungsfaktor I 
float dFactorRoll = 0.3;                                //Verstärkungsfaktor D 
 
//>> Performing Flight Correction 
//------------------------------------------------------------------------------------------------------ 
void perform_flight_correction(){ 
        reference_altitude = (int)(optical_flow_values.ground_distance*1000);                   //Regelgröße in mm 
        reference_flow_x += (int)(optical_flow_values.flow_comp_m_x*1000); 
        reference_flow_y += (int)(optical_flow_values.flow_comp_m_y*1000); 
        altitude_differenz = STARTING_ALTITUDE - reference_altitude;                                    //Regelabweichung in mm 
 
        if(reference_altitude != altitude_old){ 
                altitude_velocity = (reference_altitude - altitude_old) / 0.1; 
        } 
        if(reference_flow_x != xDirection_old){ 
                xDirection_velocity = (reference_flow_x - xDirection_old) / 0.1; 
        } 
        if(reference_flow_y != yDirection_old){ 
                yDirection_velocity = (reference_flow_y - yDirection_old) / 0.1; 
        } 
 
        if(altitude_differenz_sum < 10000 && altitude_differenz_sum > -10000){ 
                altitude_differenz_sum += altitude_differenz * 0.1; 
                if(altitude_differenz_sum > 9999){altitude_differenz_sum = 9999;}else 
                        if(altitude_differenz_sum < -9999){altitude_differenz_sum = -9999;} 
        } 
        if(xDirection_differenz_sum < 1000 && xDirection_differenz_sum > -1000){ 
                xDirection_differenz_sum += reference_flow_x; 
                if(xDirection_differenz_sum > 9999){xDirection_differenz_sum = 9999;}else 
                        if(xDirection_differenz_sum < -9999){xDirection_differenz_sum = -9999;} 
        } 
        if(yDirection_differenz_sum < 1000 && yDirection_differenz_sum > -1000){ 
                yDirection_differenz_sum += reference_flow_y; 
                if(yDirection_differenz_sum > 9999){yDirection_differenz_sum = 9999;}else 
                        if(yDirection_differenz_sum < -9999){yDirection_differenz_sum = -9999;} 
        } 
 
        ////////////////////////////////////////////////////////////////////////// 
    // PID Regelung Gas 
        gas_correction_ipart = iFactorGas * altitude_differenz_sum; 
        gas_correction_dpart = dFactorGas * altitude_velocity *-1; 
        gas_correction_ppart = pFactorGas * altitude_differenz; 
 
        gas_correction_final = (gas_correction_ppart + gas_correction_dpart + gas_correction_ipart) / 10; 
        ////////////////////////////////////////////////////////////////////////// 
        ////////////////////////////////////////////////////////////////////////// 
    // PID Regelung Nick 
        nick_correction_ipart = iFactorNick * yDirection_differenz_sum; 
        nick_correction_dpart = dFactorNick * yDirection_velocity; 
        nick_correction_ppart = pFactorNick * reference_flow_y; 
 
        nick_correction_final = (nick_correction_ppart + nick_correction_dpart + nick_correction_ipart) / -10; 
        ////////////////////////////////////////////////////////////////////////// 
        ////////////////////////////////////////////////////////////////////////// 
    // PID Regelung Roll 
        roll_correction_ipart = iFactorRoll * xDirection_differenz_sum; 
        roll_correction_dpart = dFactorRoll * xDirection_velocity; 
        roll_correction_ppart = pFactorRoll * reference_flow_x; 
 
        roll_correction_final = (roll_correction_ppart + roll_correction_dpart + roll_correction_ipart) / -10; 
        ////////////////////////////////////////////////////////////////////////// 
        //printf("Final: %f   PPART: %f   DPART: %f\n", nick_correction_final, nick_correction_ppart, nick_correction_dpart); 
 
        ////////////////////////////////////////////////////////////////////////// 
    // Set Limits 
        if(gas_correction_final >= 20){ 
                fc_correction_data.Gas = 20; 
        }else if(gas_correction_final <= -20){ 
                fc_correction_data.Gas = -20; 
        }else{ 
                fc_correction_data.Gas = gas_correction_final; 
        } 
        if(nick_correction_final >= 20){ 
                fc_correction_data.Nick = 20; 
        }else if(nick_correction_final <= -20){ 
                fc_correction_data.Nick = -20; 
        }else{ 
                fc_correction_data.Nick = nick_correction_final; 
        } 
        if(roll_correction_final >= 20){ 
                fc_correction_data.Roll = 20; 
        }else if(roll_correction_final <= -20){ 
                fc_correction_data.Roll = -20; 
        }else{ 
                fc_correction_data.Roll = roll_correction_final; 
        } 
 
        //printf("%d\n", reference_altitude/10); 
        <a href="http://www.opengroup.org/onlinepubs/009695399/functions/printf.html">printf</a>("Final: %f   PPART: %f   DPART: %f   IPART: %f    Gas Correction: %d\n", gas_correction_final, gas_correction_ppart, gas_correction_dpart, gas_correction_ipart, fc_correction_data.Gas ); 
        //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 ); 
        fc_correction_data.Gas = 0; 
        //fc_correction_data.Roll = 0; 
        //fc_correction_data.Nick = 0; 
 
 
        altitude_old = reference_altitude; 
        xDirection_old = reference_flow_x; 
        yDirection_old = reference_flow_y; 
} 
 
//>> Transmitting Correction Data 
//------------------------------------------------------------------------------------------------------ 
u8 *flight_values_stream; 
serial_data_struct data_package_flightcontrol; 
void transmit_flight_data(){ 
        flight_values_stream = (unsigned char *) &fc_correction_data; 
        //printf("Correction: %d\n", fc_correction_data.Gas); 
        for (int i = 0; i < sizeof(struct str_fc_correction_data); ++i) 
        { 
                data_package_flightcontrol.data[i] = flight_values_stream[i]; 
        } 
        create_serial_frame(1, 'b', sizeof(struct str_fc_correction_data), &data_package_flightcontrol); 
        //printf("%s\n", data_package_flightcontrol.txrxdata); 
        transmit_data(TO_KOPTER, &data_package_flightcontrol); 
} 
 
//>> Resetting Flow and Altitude Values 
//------------------------------------------------------------------------------------------------------ 
void reset_reference_values(){ 
        reference_flow_x = 0.0; 
        reference_flow_y = 0.0; 
        reference_altitude = 0.0; 
} 
 
//>> Initializing flightcontrol 
//------------------------------------------------------------------------------------------------------ 
void flightcontrol_init(){ 
        fc_correction_data.Nick = 0; 
        fc_correction_data.Roll = 0; 
        fc_correction_data.Gier = 0; 
        fc_correction_data.Gas = 0; 
        fc_correction_data.Config = EC_VALID | EC_GAS_ADD | EC_IGNORE_RC;               //Ignore Remote Control 
} 
  </div>
</div>
<div id="footer">
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>
</div>
</body>
</html>

Subversion Repositories Projects

(root)/RaspberryPi/ExPlat/unknown/filedetails.htm @ 2548 - Rev 2339