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 |