Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
21 | user | 1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 08.2007 by Christopher Hartmann / Daniel Schmitz |
||
3 | // + |
||
4 | // + Bitte die read_me Datei beachten! Es handelt sich hierbei um eine erste Probeversion! |
||
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
6 | #include "main.h" |
||
7 | #include "math.h" |
||
8 | |||
9 | // GPS feste Variablen++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
10 | volatile int loop = 0; |
||
11 | long gps_northing = 0, gps_easting = 0; |
||
12 | |||
13 | volatile int alpha = 0; |
||
14 | |||
15 | long zwn = 0, zwe = 0, zwn1 = 0, zwe1 = 0, zwn2 = 0, zwe2 = 0; |
||
16 | volatile int gps_getpos = 5; |
||
17 | volatile int gps_gethome = 0; |
||
18 | long gps_home_n = 0; |
||
19 | long gps_home_e = 0; |
||
20 | |||
21 | // GPS Einstellungen +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
22 | volatile int komp_dreh = 0; // verdrehten Kompasseinbau kompensieren (+/-Grad) |
||
23 | volatile int gpsmax = 30; //maximal zulässiger "GPS-Steuerausschlag" |
||
24 | |||
25 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
26 | |||
27 | signed int GPS_Nick = 0; |
||
28 | signed int GPS_Roll = 0; |
||
29 | |||
30 | void gps_main(void) |
||
31 | { |
||
32 | if (MotorenEin > 1 && gps_gethome == 0 && actualPos.state != 0){ //speichert GPS-Home-Position |
||
33 | gps_home_n = actualPos.northing; |
||
34 | gps_home_e = actualPos.easting; |
||
35 | beeptime = 80; |
||
36 | gps_gethome = 1; |
||
37 | } |
||
38 | |||
39 | |||
40 | if (Poti1>0 && actualPos.state != 0){ //Beginn GPS-Position-Hold |
||
41 | |||
42 | if (gps_getpos != 0){ //Postion mit Schalter loggen |
||
43 | gps_northing = actualPos.northing; |
||
44 | gps_easting = actualPos.easting; |
||
45 | beeptime = 50; |
||
46 | gps_getpos = 0;} |
||
47 | |||
48 | //Regler ########################################################################################################################## |
||
49 | //P-Regler |
||
50 | zwn = (gps_northing-actualPos.northing)*gps_p; |
||
51 | zwe = (gps_easting-actualPos.easting)*gps_p; |
||
52 | |||
53 | //I-Regler |
||
54 | zwn1=0; |
||
55 | zwe1=0; |
||
56 | |||
57 | //D-Regler |
||
58 | zwn2= gps_d*actualPos.velNorth; |
||
59 | zwe2= gps_d*actualPos.velEast; |
||
60 | |||
32 | hallo2 | 61 | GPS_Nick = (-zwn+zwn1-zwn2) / skal; |
21 | user | 62 | GPS_Roll = (zwe+zwe1-zwe2) / skal; |
63 | |||
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 |
||
66 | if (GPS_Roll>gpsmax){GPS_Roll=gpsmax;} else if (GPS_Roll<(-1*gpsmax)){GPS_Roll=(-1*gpsmax);} |
||
67 | |||
68 | //Rotationsmatrix################################################################################################################## |
||
69 | |||
70 | //Kompass ++++++++++++++++++++++++++++ |
||
71 | alpha=0; |
||
72 | //alpha = komp_dreh+KompassValue; |
||
73 | //if (KompassValue>300) {beeptime=50;} |
||
74 | //if (alpha>359) {alpha=alpha-360;} |
||
75 | |||
76 | GPS_Nick=(sin(alpha)*GPS_Roll+cos(alpha)*GPS_Nick); |
||
77 | GPS_Roll=(cos(alpha)*GPS_Nick-sin(alpha)*GPS_Roll); |
||
78 | |||
79 | }else { |
||
80 | gps_getpos=5; |
||
81 | GPS_Nick=0; |
||
82 | GPS_Roll=0; |
||
83 | zwn1=0; |
||
84 | zwe1=0; |
||
85 | } |
||
86 | } |
||
87 | |||
88 | |||
89 | |||
90 |