Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 10.2007 by Christopher Hartmann / Daniel Schmitz
// +
// + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine BETAVersion!
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include "math.h"

// GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
volatile int  alpha = 0;

long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0, entf_x = 0, entf_y = 0;

int x=0;
int hoehedurchlauf=0;

float gps_sin=0;
float gps_cos=0;

signed int GPS_Nick = 0;
signed int GPS_Roll = 0;

long alpha2 = 900;
long GPS_Nick2 = 0;
long GPS_Roll2 = 0;
 
void gps_main(void)
{
if (StickNick>-15 && StickNick<15 && StickRoll>-15 && StickRoll<15){
stick_time++;
 if (stick_time==2){
 sticks_centered=1;}
 else if (stick_time>35){
 stick_time=0;
 }}
 else
{stick_time=0; sticks_centered=0;}


if (actualPos.state != 0 && gps_new==1 && Poti1>0){                             //Beginn GPS-Position-Hold



if (gps_getpos != 0 && (Poti2 == 0||gps_gethome==5) && sticks_centered==1){                                                             //PH aktivieren
target_x = actualPos.northing;
target_y = actualPos.easting;
if (gps_gethome!=5){beeptime = 50;}
gps_getpos = 0;}


if (Poti2>0 && gps_gethome==1){                   // Home anfliegen

target_x = gps_home_x;
target_y = gps_home_y;

if ((actualPos.northing < target_x+200 && actualPos.northing > target_x-200) && (actualPos.easting < target_y+200 && actualPos.easting > target_y-200) && homing_state !=1)
{beeptime = 100; homing_state=1; gps_getpos = 0;}else {beeptime = 50;}

//Höhenreglung für Home-Anflug
if (SollHoehe > home_height && hoehedurchlauf==2){SollHoehe=SollHoehe-1;
hoehedurchlauf=0;}
if (SollHoehe < home_height && hoehedurchlauf==2){SollHoehe=SollHoehe+1;
hoehedurchlauf=0;}
else if ((SollHoehe > home_height || SollHoehe < home_height) && hoehedurchlauf<2) {hoehedurchlauf++;}}


if (sticks_centered==1){
//Regler ##########################################################################################################################
//P-Regler

zwn = (target_x-actualPos.northing)*(gps_p);
zwe = (target_y-actualPos.easting)*(gps_p);

//D-Regler
zwn2 = (actualPos.velNorth)*(-gps_d);
zwe2 = (actualPos.velEast)*(-gps_d);

//Rotationsmatrix + Kompass########################################################################################################

if (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV){
alpha = komp_dreh+KompassStartwert;                    
if (alpha>359) {alpha=alpha-360;}

if (alpha != alpha2){
gps_sin =floor(sin(alpha*M_PI/180)*100)/100;
gps_cos =floor(cos(alpha*M_PI/180)*100)/100;
alpha2 = alpha;
}

GPS_Nick2 = (zwn+zwn2)/skal;
GPS_Roll2 = (zwe+zwe2)/skal;

GPS_Nick= (gps_sin*GPS_Roll2)+(gps_cos*GPS_Nick2);
GPS_Roll= (gps_cos*GPS_Roll2)-(gps_sin*GPS_Nick2);

}else{
GPS_Nick = (zwn+zwn2)/skal;
GPS_Roll = (zwe+zwe2)/skal;
}

GPS_Nick=GPS_Nick*-1;

//GPS-Min-Max Prüfung###############################################################################################################
if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);}
if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);}

gps_new=0;
}else{
gps_getpos=5;
GPS_Nick=0;
GPS_Roll=0;}}
}