Rev 21 | Rev 36 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 21 | Rev 32 | ||
---|---|---|---|
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 08.2007 by Christopher Hartmann / Daniel Schmitz |
2 | // + Copyright (c) 08.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 erste Probeversion! |
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; |
10 | volatile int loop = 0; |
11 | long gps_northing = 0, gps_easting = 0; |
11 | long gps_northing = 0, gps_easting = 0; |
12 | 12 | ||
13 | volatile int alpha = 0; |
13 | volatile int alpha = 0; |
14 | 14 | ||
15 | long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0; |
15 | long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0; |
16 | volatile int gps_getpos = 5; |
16 | volatile int gps_getpos = 5; |
17 | volatile int gps_gethome = 0; |
17 | volatile int gps_gethome = 0; |
18 | long gps_home_n = 0; |
18 | long gps_home_n = 0; |
19 | long gps_home_e = 0; |
19 | long gps_home_e = 0; |
20 | 20 | ||
21 | // GPS Einstellungen +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
21 | // GPS Einstellungen +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
22 | volatile int komp_dreh = 0; // verdrehten Kompasseinbau kompensieren (+/-Grad) |
22 | volatile int komp_dreh = 0; // verdrehten Kompasseinbau kompensieren (+/-Grad) |
23 | volatile int gpsmax = 30; //maximal zulässiger "GPS-Steuerausschlag" |
23 | volatile int gpsmax = 30; //maximal zulässiger "GPS-Steuerausschlag" |
24 | 24 | ||
25 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
25 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
26 | 26 | ||
27 | signed int GPS_Nick = 0; |
27 | signed int GPS_Nick = 0; |
28 | signed int GPS_Roll = 0; |
28 | signed int GPS_Roll = 0; |
29 | 29 | ||
30 | void gps_main(void) |
30 | void gps_main(void) |
31 | { |
31 | { |
32 | if (MotorenEin > 1 && gps_gethome == 0 && actualPos.state != 0){ //speichert GPS-Home-Position |
32 | if (MotorenEin > 1 && gps_gethome == 0 && actualPos.state != 0){ //speichert GPS-Home-Position |
33 | gps_home_n = actualPos.northing; |
33 | gps_home_n = actualPos.northing; |
34 | gps_home_e = actualPos.easting; |
34 | gps_home_e = actualPos.easting; |
35 | beeptime = 80; |
35 | beeptime = 80; |
36 | gps_gethome = 1; |
36 | gps_gethome = 1; |
37 | } |
37 | } |
38 | 38 | ||
39 | 39 | ||
40 | if (Poti1>0 && actualPos.state != 0){ //Beginn GPS-Position-Hold |
40 | if (Poti1>0 && actualPos.state != 0){ //Beginn GPS-Position-Hold |
41 | 41 | ||
42 | if (gps_getpos != 0){ //Postion mit Schalter loggen |
42 | if (gps_getpos != 0){ //Postion mit Schalter loggen |
43 | gps_northing = actualPos.northing; |
43 | gps_northing = actualPos.northing; |
44 | gps_easting = actualPos.easting; |
44 | gps_easting = actualPos.easting; |
45 | beeptime = 50; |
45 | beeptime = 50; |
46 | gps_getpos = 0;} |
46 | gps_getpos = 0;} |
47 | 47 | ||
48 | //Regler ########################################################################################################################## |
48 | //Regler ########################################################################################################################## |
49 | //P-Regler |
49 | //P-Regler |
50 | zwn = (gps_northing-actualPos.northing)*gps_p; |
50 | zwn = (gps_northing-actualPos.northing)*gps_p; |
51 | zwe = (gps_easting-actualPos.easting)*gps_p; |
51 | zwe = (gps_easting-actualPos.easting)*gps_p; |
52 | 52 | ||
53 | //I-Regler |
53 | //I-Regler |
54 | zwn1=0; |
54 | zwn1=0; |
55 | zwe1=0; |
55 | zwe1=0; |
56 | 56 | ||
57 | //D-Regler |
57 | //D-Regler |
58 | zwn2= gps_d*actualPos.velNorth; |
58 | zwn2= gps_d*actualPos.velNorth; |
59 | zwe2= gps_d*actualPos.velEast; |
59 | zwe2= gps_d*actualPos.velEast; |
60 | 60 | ||
61 | GPS_Nick = -1*(zwn+zwn1-zwn2) / skal; |
61 | GPS_Nick = (-zwn+zwn1-zwn2) / skal; |
62 | GPS_Roll = (zwe+zwe1-zwe2) / skal; |
62 | GPS_Roll = (zwe+zwe1-zwe2) / skal; |
63 | 63 | ||
64 | //GPS-Mixer######################################################################################################################## |
64 | //GPS-Mixer######################################################################################################################## |
65 | if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} //min-max Wert überprüfen |
65 | if (GPS_Nick>gpsmax){GPS_Nick=gpsmax;} else if (GPS_Nick<(-1*gpsmax)){GPS_Nick=(-1*gpsmax);} //min-max Wert überprüfen |
66 | if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);} |
66 | if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);} |
67 | 67 | ||
68 | //Rotationsmatrix################################################################################################################## |
68 | //Rotationsmatrix################################################################################################################## |
69 | 69 | ||
70 | //Kompass ++++++++++++++++++++++++++++ |
70 | //Kompass ++++++++++++++++++++++++++++ |
71 | alpha=0; |
71 | alpha=0; |
72 | //alpha = komp_dreh+KompassValue; |
72 | //alpha = komp_dreh+KompassValue; |
73 | //if (KompassValue>300) {beeptime=50;} |
73 | //if (KompassValue>300) {beeptime=50;} |
74 | //if (alpha>359) {alpha=alpha-360;} |
74 | //if (alpha>359) {alpha=alpha-360;} |
75 | 75 | ||
76 | GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick); |
76 | GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick); |
77 | GPS_Roll=(cos(alpha)*GPS_Nick-sin(alpha)*GPS_Roll); |
77 | GPS_Roll=(cos(alpha)*GPS_Nick-sin(alpha)*GPS_Roll); |
78 | 78 | ||
79 | }else { |
79 | }else { |
80 | gps_getpos=5; |
80 | gps_getpos=5; |
81 | GPS_Nick=0; |
81 | GPS_Nick=0; |
82 | GPS_Roll=0; |
82 | GPS_Roll=0; |
83 | zwn1=0; |
83 | zwn1=0; |
84 | zwe1=0; |
84 | zwe1=0; |
85 | } |
85 | } |
86 | } |
86 | } |
87 | 87 | ||
88 | 88 | ||
89 | 89 | ||
90 | 90 | ||
91 | 91 |