Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Copyright (c) 08.2007 by Christopher Hartmann / Daniel Schmitz

// +

// + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine erste Probeversion!

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include "main.h"

#include "math.h"

// GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

volatile int loop = 0;

long gps_northing = 0, gps_easting = 0, gps_altitude = 0;

volatile int  alpha = 0;

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

volatile int  gps_getpos = 5;

long gps_home_n = 0;

long gps_home_e = 0;

// GPS Einstellungen +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

volatile int komp_dreh = 0;             // verdrehten Kompasseinbau kompensieren (+/-Grad)

volatile int gpsmax = 35;                       //maximal zulässiger "GPS-Steuerausschlag"

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

signed int GPS_Nick = 0;

signed int GPS_Roll = 0;

void gps_main(void)

{

/*

if (MotorenEin = 1 && gps_gethome == 0 && actualPos.state != 0){                //speichert GPS-Home-Position

gps_home_n = actualPos.northing;

gps_home_e = actualPos.easting;

beeptime = 80;

gps_gethome  = 1;

}*/

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

if (gps_getpos != 0){                                                           //Postion mit Schalter loggen

gps_northing = actualPos.northing;

gps_easting = actualPos.easting;

gps_altitude = actualPos.altitude;

beeptime = 50;

gps_getpos = 0;}

//Regler ##########################################################################################################################

//P-Regler

zwn = ((sqrt(gps_northing^2+gps_altitude^2)-sqrt(actualPos.northing^2+actualPos.altitude^2))*gps_p)/8;

zwe = ((gps_easting-actualPos.easting)*gps_p)/8;

//D-Regler

zwn2= (gps_d*actualPos.velNorth)/-2;

zwe2= (gps_d*actualPos.velEast)/-2;    

GPS_Nick = -1*(zwn+zwn2); // skal;

GPS_Roll = (zwe+zwe2); // skal; 

//GPS-Mixer########################################################################################################################

if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} //min-max Wert überprüfen

if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);}

/*

//Rotationsmatrix##################################################################################################################

//Kompass ++++++++++++++++++++++++++++

alpha=0;

alpha = komp_dreh+KompassValue;                

if (KompassValue>300) {beeptime=50;}

if (alpha>359) {alpha=alpha-360;}

GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick);

GPS_Roll=(cos(alpha)*GPS_Nick-sin(alpha)*GPS_Roll);

*/

}else {

gps_getpos=5;

GPS_Nick=0;

GPS_Roll=0;

zwn1=0;

zwe1=0;

}

}