Subversion Repositories FlightCtrl

Rev

Rev 130 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 130 Rev 243
1 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2 
// + Copyright (c) 08.2007 by Christopher Hartmann / Daniel Schmitz
 2 
// + Copyright (c) 10.2007 by Christopher Hartmann / Daniel Schmitz
3 
// +
 3 
// +
4 
// + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine erste Probeversion!
 4 
// + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine BETAVersion!
5 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 5 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6 
#include "main.h"
 6 
#include "main.h"
7 
#include "math.h"
 7 
#include "math.h"
8 
 
 8 
 
9 
// GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 9 
// GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10 
//volatile int loop = 0;
 - 
 
11 
//long gps_northing = 0, gps_easting = 0, gps_altitude = 0;
 - 
 
12 
 
 - 
 
13 
long target_x = 0, target_y = 0, target_z = 0;
 - 
 
14 
 
 - 
 
15 
volatile int  alpha = 0;
 10 
volatile int  alpha = 0;
16 
 
 11 
 
17 
long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0;
 - 
 
18 
volatile int  gps_getpos = 5;
 - 
 
19 
long gps_home_x = 0;
 - 
 
20 
long gps_home_y = 0;
 - 
 
21 
 
 - 
 
22 
// GPS Einstellungen +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
23 
volatile int komp_dreh = 0;             // verdrehten Kompasseinbau kompensieren (+/-Grad)
 - 
 
- 
 
 12 
long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0, entf_x = 0, entf_y = 0;
24 
volatile int gpsmax = 35;                       //maximal zulässiger "GPS-Steuerausschlag"
 13 
 
- 
 
 14 
int x=0;
- 
 
 15 
int hoehedurchlauf=0;
- 
 
 16 
 
25 
 
 17 
float gps_sin=0;
26 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 18 
float gps_cos=0;
27 
 
 19 
 
28 
signed int GPS_Nick = 0;
 20 
signed int GPS_Nick = 0;
29 
signed int GPS_Roll = 0;
 21 
signed int GPS_Roll = 0;
- 
 
 22 
 
- 
 
 23 
long alpha2 = 900;
- 
 
 24 
long GPS_Nick2 = 0;
- 
 
 25 
long GPS_Roll2 = 0;
30 
 
 26 
 
31 
void gps_main(void)
 27 
void gps_main(void)
32 
{
 28 
{
33 
/*
 - 
 
34 
if (MotorenEin = 1 && gps_gethome == 0 && actualPos.state != 0){                //speichert GPS-Home-Position
 29 
if (StickNick>-15 && StickNick<15 && StickRoll>-15 && StickRoll<15){
- 
 
 30 
stick_time++;
35 
gps_home_x = actualPos.x;
 31 
 if (stick_time==2){
36 
gps_home_y = actualPos.y;
 32 
 sticks_centered=1;}
37 
beeptime = 80;
 33 
 else if (stick_time>35){
38 
gps_gethome  = 1;
 34 
 stick_time=0;
39 
}*/
 35 
 }}
- 
 
 36 
 else
- 
 
 37 
{stick_time=0; sticks_centered=0;}
40 
 
 38 
 
41 
 
 39 
 
42 
if (Poti1>0 && actualPos.state != 0){                           //Beginn GPS-Position-Hold
 40 
if (actualPos.state != 0 && gps_new==1 && Poti1>0){                             //Beginn GPS-Position-Hold
- 
 
 41 
 
- 
 
 42 
 
43 
 
 43 
 
44 
if (gps_getpos != 0){                                                           //Postion mit Schalter loggen
 44 
if (gps_getpos != 0 && (Poti2 == 0||gps_gethome==5) && sticks_centered==1){                                                             //PH aktivieren
45 
target_x = actualPos.x;
 45 
target_x = actualPos.northing;
46 
target_y = actualPos.y;
 46 
target_y = actualPos.easting;
47 
target_z = actualPos.z;
 - 
 
48 
beeptime = 50;
 47 
if (gps_gethome!=5){beeptime = 50;}
49 
gps_getpos = 0;}
 48 
gps_getpos = 0;}
- 
 
 49 
 
- 
 
 50 
 
- 
 
 51 
if (Poti2>0 && gps_gethome==1){                   // Home anfliegen
- 
 
 52 
 
- 
 
 53 
target_x = gps_home_x;
- 
 
 54 
target_y = gps_home_y;
- 
 
 55 
 
- 
 
 56 
if ((actualPos.northing < target_x+200 && actualPos.northing > target_x-200) && (actualPos.easting < target_y+200 && actualPos.easting > target_y-200) && homing_state !=1)
- 
 
 57 
{beeptime = 100; homing_state=1; gps_getpos = 0;}else {beeptime = 50;}
- 
 
 58 
 
- 
 
 59 
//Höhenreglung für Home-Anflug
- 
 
 60 
if (SollHoehe > home_height && hoehedurchlauf==2){SollHoehe=SollHoehe-1;
- 
 
 61 
hoehedurchlauf=0;}
- 
 
 62 
if (SollHoehe < home_height && hoehedurchlauf==2){SollHoehe=SollHoehe+1;
- 
 
 63 
hoehedurchlauf=0;}
- 
 
 64 
else if ((SollHoehe > home_height || SollHoehe < home_height) && hoehedurchlauf<2) {hoehedurchlauf++;}}
- 
 
 65 
 
- 
 
 66 
 
50 
 
 67 
if (sticks_centered==1){
51 
//Regler ##########################################################################################################################
 68 
//Regler ##########################################################################################################################
52 
//P-Regler
 - 
 
53 
zwn = ((sqrt(target_x^2+target_z^2)-sqrt(actualPos.x^2+actualPos.z^2))*gps_p)/10; //8
 - 
 
54 
zwe = ((target_y-actualPos.y)*gps_p)/10;
 - 
 
55 
 
 69 
//P-Regler
56 
//D-Regler
 70 
 
- 
 
 71 
zwn = (target_x-actualPos.northing)*(gps_p);
57 
zwn2= (gps_d*actualPos.vx)/-3; //-2
 72 
zwe = (target_y-actualPos.easting)*(gps_p);
58 
zwe2= (gps_d*actualPos.vy)/-3; 
 73 
 
59 
 
 74 
//D-Regler
60 
GPS_Nick = (zwn+zwn2); // skal;
 - 
 
61 
GPS_Roll = (zwe+zwe2); // skal;
- 
 
62 
 
 - 
 
63 
//GPS-Mixer########################################################################################################################
 - 
 
64 
if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} //min-max Wert überprüfen
 75 
zwn2 = (actualPos.velNorth)*(-gps_d);
65 
if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);}
 - 
 
66 
 
 76 
zwe2 = (actualPos.velEast)*(-gps_d);
67 
/*
 - 
 
68 
//Rotationsmatrix##################################################################################################################
 77 
 
- 
 
 78 
//Rotationsmatrix + Kompass########################################################################################################
- 
 
 79 
 
- 
 
 80 
if (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV){
- 
 
 81 
alpha = komp_dreh+KompassStartwert;                    
- 
 
 82 
if (alpha>359) {alpha=alpha-360;}
69 
//Kompass ++++++++++++++++++++++++++++
 83 
 
70 
alpha=0;
 84 
if (alpha != alpha2){
71 
alpha = komp_dreh+KompassValue;                
 - 
 
- 
 
 85 
gps_sin =floor(sin(alpha*M_PI/180)*100)/100;
- 
 
 86 
gps_cos =floor(cos(alpha*M_PI/180)*100)/100;
- 
 
 87 
alpha2 = alpha;
72 
if (KompassValue>300) {beeptime=50;}
 88 
}
73 
if (alpha>359) {alpha=alpha-360;}
 - 
 
74 
 
 89 
 
75 
 
 90 
GPS_Nick2 = (zwn+zwn2)/skal;
76 
GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick);
 - 
 
77 
GPS_Roll=(cos(alpha)*GPS_Roll-sin(alpha)*GPS_Nick);
 91 
GPS_Roll2 = (zwe+zwe2)/skal;
- 
 
 92 
 
- 
 
 93 
GPS_Nick= (gps_sin*GPS_Roll2)+(gps_cos*GPS_Nick2);
- 
 
 94 
GPS_Roll= (gps_cos*GPS_Roll2)-(gps_sin*GPS_Nick2);
- 
 
 95 
 
- 
 
 96 
}else{
- 
 
 97 
GPS_Nick = (zwn+zwn2)/skal;
- 
 
 98 
GPS_Roll = (zwe+zwe2)/skal;
- 
 
 99 
}
- 
 
 100 
 
- 
 
 101 
GPS_Nick=GPS_Nick*-1;
- 
 
 102 
 
78 
*/
 103 
//GPS-Min-Max Prüfung###############################################################################################################
79 
 
 104 
if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);}
80 
}else {
 105 
if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);}
81 
gps_getpos=5;
 106 
 
82 
GPS_Nick=0;
 107 
gps_new=0;
83 
GPS_Roll=0;
 108 
}else{
84 
}
 109 
gps_getpos=5;
85 
}
 110 
GPS_Nick=0;
86 
 
 111 
GPS_Roll=0;}}
87 
 
 112 
}
88 
 
 113 
 
89 
 
 114 
 
90 
 
 115