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