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;

long target_x = 0, target_y = 0, target_z = 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_x = 0;
long gps_home_y = 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_x = actualPos.x;
gps_home_y = actualPos.y;
beeptime = 80;
gps_gethome  = 1;
}*/


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

if (gps_getpos != 0){                                                           //Postion mit Schalter loggen
target_x = actualPos.x;
target_y = actualPos.y;
target_z = actualPos.z;
beeptime = 50;
gps_getpos = 0;}

//Regler ##########################################################################################################################
//P-Regler
zwn = ((sqrt(target_x^2+target_z^2)-sqrt(actualPos.x^2+actualPos.z^2))*gps_p)/10; //8
zwe = ((target_y-actualPos.y)*gps_p)/10;

//D-Regler
zwn2= (gps_d*actualPos.vx)/-3; //-2
zwe2= (gps_d*actualPos.vy)/-3; 

GPS_Nick = (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_Roll-sin(alpha)*GPS_Nick);
*/

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