Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 242 → Rev 243

/branches/GPS_BETA_chris2798_hallo2/GPS.c
1,89 → 1,114
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 08.2007 by Christopher Hartmann / Daniel Schmitz
// + Copyright (c) 10.2007 by Christopher Hartmann / Daniel Schmitz
// +
// + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine erste Probeversion!
// + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine BETAVersion!
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#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;
long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0, entf_x = 0, entf_y = 0;
 
// GPS Einstellungen +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
volatile int komp_dreh = 0; // verdrehten Kompasseinbau kompensieren (+/-Grad)
volatile int gpsmax = 35; //maximal zulässiger "GPS-Steuerausschlag"
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 (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 (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 (Poti1>0 && actualPos.state != 0){ //Beginn GPS-Position-Hold
if (actualPos.state != 0 && gps_new==1 && Poti1>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;
 
 
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 = ((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;
 
zwn = (target_x-actualPos.northing)*(gps_p);
zwe = (target_y-actualPos.easting)*(gps_p);
 
//D-Regler
zwn2= (gps_d*actualPos.vx)/-3; //-2
zwe2= (gps_d*actualPos.vy)/-3;
zwn2 = (actualPos.velNorth)*(-gps_d);
zwe2 = (actualPos.velEast)*(-gps_d);
 
GPS_Nick = (zwn+zwn2); // skal;
GPS_Roll = (zwe+zwe2); // skal;
//Rotationsmatrix + Kompass########################################################################################################
 
//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 (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_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick);
GPS_Roll=(cos(alpha)*GPS_Roll-sin(alpha)*GPS_Nick);
*/
GPS_Nick2 = (zwn+zwn2)/skal;
GPS_Roll2 = (zwe+zwe2)/skal;
 
}else {
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;
GPS_Roll=0;}}
}
}