Rev 506 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 506 | Rev 507 | ||
---|---|---|---|
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 | // + Copyright (c) 10.2007 by Jochen Kromayer |
2 | // + Copyright (c) 10.2007 by Jochen Kromayer |
3 | // + es handelt sich um eine Beta-Software, die zu nichtkommerziellen Zwecken frei veröffentlich und weitergegeben werden darf. Für evtl. |
3 | // + es handelt sich um eine Beta-Software, die zu nichtkommerziellen Zwecken frei veröffentlich und weitergegeben werden darf. Für evtl. |
4 | // + Personen- und/der Sachschäden ist jegliche Haftung ausgeschlossen |
4 | // + Personen- und/der Sachschäden ist jegliche Haftung ausgeschlossen |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
6 | #include "main.h" |
6 | #include "main.h" |
7 | 7 | ||
8 | //Sinusfunktion von 0° bis 359°. Ergebnis wurde mit 1000 multipliziert, damit als Definition platzsparend int |
8 | //Sinusfunktion von 0° bis 359°. Ergebnis wurde mit 1000 multipliziert, damit als Definition platzsparend int |
9 | //verwendet werden kann. d.h., es muss also in der späteren Berechnung noch mit 0,001 multipliziert werden //(090907Kr) |
9 | //verwendet werden kann. d.h., es muss also in der späteren Berechnung noch mit 0,001 multipliziert werden //(090907Kr) |
10 | const int c_sin[] = {0,17,34,52,69,87,104,121,139,156,173,190, |
10 | const int c_sin[] = {0,17,34,52,69,87,104,121,139,156,173,190, |
11 | 207,224,241,258,275,292,309,325,342,358,374,390,406,422,438,453,469,484,500,515,529,544,559,573,587,601,615,629, |
11 | 207,224,241,258,275,292,309,325,342,358,374,390,406,422,438,453,469,484,500,515,529,544,559,573,587,601,615,629, |
12 | 642,656,669,682,694,707,719,731,743,754,766,777,788,798,809,819,829,838,848,857,866,874,882,891,898,906,913,920, |
12 | 642,656,669,682,694,707,719,731,743,754,766,777,788,798,809,819,829,838,848,857,866,874,882,891,898,906,913,920, |
13 | 927,933,939,945,951,956,961,965,970,974,978,981,984,987,990,992,994,996,997,998,999,999,1000,999,999,998,997,996, |
13 | 927,933,939,945,951,956,961,965,970,974,978,981,984,987,990,992,994,996,997,998,999,999,1000,999,999,998,997,996, |
14 | 994,992,990,987,984,981,978,974,970,965,961,956,951,945,939,933,927,920,913,906,898,891,882,874,866,857,848,838, |
14 | 994,992,990,987,984,981,978,974,970,965,961,956,951,945,939,933,927,920,913,906,898,891,882,874,866,857,848,838, |
15 | 829,819,809,798,788,777,766,754,743,731,719,707,694,682,669,656,642,629,615,601,587,573,559,544,529,515,500,484, |
15 | 829,819,809,798,788,777,766,754,743,731,719,707,694,682,669,656,642,629,615,601,587,573,559,544,529,515,500,484, |
16 | 469,453,438,422,406,390,374,358,342,325,309,292,275,258,241,224,207,190,173,156,139,121,104,87,69,52,34,17,0,-17, |
16 | 469,453,438,422,406,390,374,358,342,325,309,292,275,258,241,224,207,190,173,156,139,121,104,87,69,52,34,17,0,-17, |
17 | -34,-52,-69,-87,-104,-121,-139,-156,-173,-190,-207,-224,-241,-258,-275,-292,-309,-325,-342,-358,-374,-390,-406, |
17 | -34,-52,-69,-87,-104,-121,-139,-156,-173,-190,-207,-224,-241,-258,-275,-292,-309,-325,-342,-358,-374,-390,-406, |
18 | -422,-438,-453,-469,-484,-500,-515,-529,-544,-559,-573,-587,-601,-615,-629,-642,-656,-669,-682,-694,-707,-719, |
18 | -422,-438,-453,-469,-484,-500,-515,-529,-544,-559,-573,-587,-601,-615,-629,-642,-656,-669,-682,-694,-707,-719, |
19 | -731,-743,-754,-766,-777,-788,-798,-809,-819,-829,-838,-848,-857,-866,-874,-882,-891,-898,-906,-913,-920,-927, |
19 | -731,-743,-754,-766,-777,-788,-798,-809,-819,-829,-838,-848,-857,-866,-874,-882,-891,-898,-906,-913,-920,-927, |
20 | -933,-939,-945,-951,-956,-961,-965,-970,-974,-978,-981,-984,-987,-990,-992,-994,-996,-997,-998,-999,-999,-1000, |
20 | -933,-939,-945,-951,-956,-961,-965,-970,-974,-978,-981,-984,-987,-990,-992,-994,-996,-997,-998,-999,-999,-1000, |
21 | -999,-999,-998,-997,-996,-994,-992,-990,-987,-984,-981,-978,-974,-970,-965,-961,-956,-951,-945,-939,-933,-927, |
21 | -999,-999,-998,-997,-996,-994,-992,-990,-987,-984,-981,-978,-974,-970,-965,-961,-956,-951,-945,-939,-933,-927, |
22 | -920,-913,-906,-898,-891,-882,-874,-866,-857,-848,-838,-829,-819,-809,-798,-788,-777,-766,-754,-743,-731,-719, |
22 | -920,-913,-906,-898,-891,-882,-874,-866,-857,-848,-838,-829,-819,-809,-798,-788,-777,-766,-754,-743,-731,-719, |
23 | -707,-694,-682,-669,-656,-642,-629,-615,-601,-587,-573,-559,-544,-529,-515,-500,-484,-469,-453,-438,-422,-406, |
23 | -707,-694,-682,-669,-656,-642,-629,-615,-601,-587,-573,-559,-544,-529,-515,-500,-484,-469,-453,-438,-422,-406, |
24 | -390,-374,-358,-342,-325,-309,-292,-275,-258,-241,-224,-207,-190,-173,-156,-139,-121,-104,-87,-69,-52,-34,-17}; |
24 | -390,-374,-358,-342,-325,-309,-292,-275,-258,-241,-224,-207,-190,-173,-156,-139,-121,-104,-87,-69,-52,-34,-17}; |
25 | 25 | ||
26 | //Cosinusfunktion von 0° bis 359°. Ergebnis wurde mit 1000 multipliziert, damit als Definition platzsparend int |
26 | //Cosinusfunktion von 0° bis 359°. Ergebnis wurde mit 1000 multipliziert, damit als Definition platzsparend int |
27 | //verwendet werden kann. D.h., es muss also in der späteren Berechnung noch mit 0,001 multipliziert werden //(090907Kr) |
27 | //verwendet werden kann. D.h., es muss also in der späteren Berechnung noch mit 0,001 multipliziert werden //(090907Kr) |
28 | const int c_cos[] = {1000,999,999,998,997,996,994,992,990,987,984,981, |
28 | const int c_cos[] = {1000,999,999,998,997,996,994,992,990,987,984,981, |
29 | 978,974,970,965,961,956,951,945,939,933,927,920,913,906,898,891,882,874,866,857,848,838,829,819,809,798,788,777, |
29 | 978,974,970,965,961,956,951,945,939,933,927,920,913,906,898,891,882,874,866,857,848,838,829,819,809,798,788,777, |
30 | 766,754,743,731,719,707,694,682,669,656,642,629,615,601,587,573,559,544,529,515,500,484,469,453,438,422,406,390, |
30 | 766,754,743,731,719,707,694,682,669,656,642,629,615,601,587,573,559,544,529,515,500,484,469,453,438,422,406,390, |
31 | 374,358,342,325,309,292,275,258,241,224,207,190,173,156,139,121,104,87,69,52,34,17,0,-17,-34,-52,-69,-87,-104, |
31 | 374,358,342,325,309,292,275,258,241,224,207,190,173,156,139,121,104,87,69,52,34,17,0,-17,-34,-52,-69,-87,-104, |
32 | -121,-139,-156,-173,-190,-207,-224,-241,-258,-275,-292,-309,-325,-342,-358,-374,-390,-406,-422,-438,-453,-469, |
32 | -121,-139,-156,-173,-190,-207,-224,-241,-258,-275,-292,-309,-325,-342,-358,-374,-390,-406,-422,-438,-453,-469, |
33 | -484,-500,-515,-529,-544,-559,-573,-587,-601,-615,-629,-642,-656,-669,-682,-694,-707,-719,-731,-743,-754,-766, |
33 | -484,-500,-515,-529,-544,-559,-573,-587,-601,-615,-629,-642,-656,-669,-682,-694,-707,-719,-731,-743,-754,-766, |
34 | -777,-788,-798,-809,-819,-829,-838,-848,-857,-866,-874,-882,-891,-898,-906,-913,-920,-927,-933,-939,-945,-951, |
34 | -777,-788,-798,-809,-819,-829,-838,-848,-857,-866,-874,-882,-891,-898,-906,-913,-920,-927,-933,-939,-945,-951, |
35 | -956,-961,-965,-970,-974,-978,-981,-984,-987,-990,-992,-994,-996,-997,-998,-999,-999,-1000,-999,-999,-998,-997, |
35 | -956,-961,-965,-970,-974,-978,-981,-984,-987,-990,-992,-994,-996,-997,-998,-999,-999,-1000,-999,-999,-998,-997, |
36 | -996,-994,-992,-990,-987,-984,-981,-978,-974,-970,-965,-961,-956,-951,-945,-939,-933,-927,-920,-913,-906,-898, |
36 | -996,-994,-992,-990,-987,-984,-981,-978,-974,-970,-965,-961,-956,-951,-945,-939,-933,-927,-920,-913,-906,-898, |
37 | -891,-882,-874,-866,-857,-848,-838,-829,-819,-809,-798,-788,-777,-766,-754,-743,-731,-719,-707,-694,-682,-669, |
37 | -891,-882,-874,-866,-857,-848,-838,-829,-819,-809,-798,-788,-777,-766,-754,-743,-731,-719,-707,-694,-682,-669, |
38 | -656,-642,-629,-615,-601,-587,-573,-559,-544,-529,-515,-500,-484,-469,-453,-438,-422,-406,-390,-374,-358,-342, |
38 | -656,-642,-629,-615,-601,-587,-573,-559,-544,-529,-515,-500,-484,-469,-453,-438,-422,-406,-390,-374,-358,-342, |
39 | -325,-309,-292,-275,-258,-241,-224,-207,-190,-173,-156,-139,-121,-104,-87,-69,-52,-34,-17,0,17,34,52,69,87, |
39 | -325,-309,-292,-275,-258,-241,-224,-207,-190,-173,-156,-139,-121,-104,-87,-69,-52,-34,-17,0,17,34,52,69,87, |
40 | 104,121,139,156,173,190,207,224,241,258,275,292,309,325,342,358,374,390,406,422,438,453,469,484,500,515,529,544, |
40 | 104,121,139,156,173,190,207,224,241,258,275,292,309,325,342,358,374,390,406,422,438,453,469,484,500,515,529,544, |
41 | 559,573,587,601,615,629,642,656,669,682,694,707,719,731,743,754,766,777,788,798,809,819,829,838,848,857,866,874, |
41 | 559,573,587,601,615,629,642,656,669,682,694,707,719,731,743,754,766,777,788,798,809,819,829,838,848,857,866,874, |
42 | 882,891,898,906,913,920,927,933,939,945,951,956,961,965,970,974,978,981,984,987,990,992,994,996,997,998,999,999}; |
42 | 882,891,898,906,913,920,927,933,939,945,951,956,961,965,970,974,978,981,984,987,990,992,994,996,997,998,999,999}; |
43 | 43 | ||
44 | 44 | ||
45 | 45 | ||
46 | long Soll_Position_North = 0; |
46 | long Soll_Position_North = 0; |
47 | long Soll_Position_East = 0; |
47 | long Soll_Position_East = 0; |
48 | 48 | ||
49 | long GPS_Positionsabweichung_North = 0; |
49 | long GPS_Positionsabweichung_North = 0; |
50 | long GPS_Positionsabweichung_East = 0; |
50 | long GPS_Positionsabweichung_East = 0; |
51 | long GPS_Geschwindigkeit_North = 0; |
51 | long GPS_Geschwindigkeit_North = 0; |
52 | long GPS_Geschwindigkeit_East = 0; |
52 | long GPS_Geschwindigkeit_East = 0; |
53 | signed int GPS_StickSense = 15; |
53 | signed int GPS_StickSense = 15; |
54 | long P_Einfluss_North = 0; |
54 | long P_Einfluss_North = 0; |
55 | long D_Einfluss_North = 0; |
55 | long D_Einfluss_North = 0; |
56 | long P_Einfluss_East = 0; |
56 | long P_Einfluss_East = 0; |
57 | long D_Einfluss_East = 0; |
57 | long D_Einfluss_East = 0; |
58 | 58 | ||
59 | signed int GPS_North = 0; |
59 | signed int GPS_North = 0; |
60 | signed int GPS_East = 0; |
60 | signed int GPS_East = 0; |
61 | signed int GPS_Nick = 0; |
61 | signed int GPS_Nick = 0; |
62 | signed int GPS_Roll = 0; |
62 | signed int GPS_Roll = 0; |
63 | 63 | ||
64 | volatile char gethome = 0; //Flag ob GPS_Home_Position gelernt ist //(280807Kr) |
64 | volatile char gethome = 0; //Flag ob GPS_Home_Position gelernt ist //(280807Kr) |
65 | long GPS_Home_North = 0; |
65 | long GPS_Home_North = 0; |
66 | long GPS_Home_East = 0; |
66 | long GPS_Home_East = 0; |
67 | 67 | ||
68 | unsigned char blinkcount_LED2 = 0; //Hilfszähler für die blinkende LED (010907Kr) |
68 | unsigned char blinkcount_LED2 = 0; //Hilfszähler für die blinkende LED (010907Kr) |
- | 69 | ||
69 | #define GPS_Limit 35 |
70 | #define GPS_Limit 35 //(031207Kr) |
- | 71 | #define Limit_D_Anteil 30 //(031207Kr) |
|
70 | 72 | ||
71 | //**************************** MIRCOS DEFINITIONSANFANG FÜR DIE FILTERUNG |
73 | //**************************** MIRCOS DEFINITIONSANFANG FÜR DIE FILTERUNG |
72 | // Filterung der GPS Messwerte vom Ublox-Empfänger. Es wird der gleitende Durchschnitt aus n Messwerten gebildet |
74 | // Filterung der GPS Messwerte vom Ublox-Empfänger. Es wird der gleitende Durchschnitt aus n Messwerten gebildet |
73 | 75 | ||
74 | #define MITTELWERTZAHL 8 // Anzahl der Messwerte fuer gleitenden Mittelwert |
76 | #define MITTELWERTZAHL 8 // Anzahl der Messwerte fuer gleitenden Mittelwert |
75 | volatile char NeuerMittelwert = 0; //Flag ob ein neuer Mittelwert vorliegt, der mit GPS Frequenz berechnet wird, da nur dann neue Messwerte vom GPS vorliegen |
77 | volatile char NeuerMittelwert = 0; //Flag ob ein neuer Mittelwert vorliegt, der mit GPS Frequenz berechnet wird, da nur dann neue Messwerte vom GPS vorliegen |
76 | volatile unsigned char FilterPos = 0; |
78 | volatile unsigned char FilterPos = 0; |
77 | volatile long NORTH_MITTEL = 0; |
79 | volatile long NORTH_MITTEL = 0; |
78 | volatile long EAST_MITTEL = 0; |
80 | volatile long EAST_MITTEL = 0; |
79 | volatile long long NORTH_LONG_LONG = 0; |
81 | volatile long long NORTH_LONG_LONG = 0; |
80 | volatile long long EAST_LONG_LONG = 0; |
82 | volatile long long EAST_LONG_LONG = 0; |
81 | volatile int filterfilled = 0; |
83 | volatile int filterfilled = 0; |
82 | 84 | ||
83 | typedef struct { |
85 | typedef struct { |
84 | 86 | ||
85 | long NORTH[MITTELWERTZAHL]; |
87 | long NORTH[MITTELWERTZAHL]; |
86 | 88 | ||
87 | } FILTER_NORTH; // Filterdefinition für die Positions-Nordwerte aus dem GPS-Empfänger |
89 | } FILTER_NORTH; // Filterdefinition für die Positions-Nordwerte aus dem GPS-Empfänger |
88 | 90 | ||
89 | typedef struct { |
91 | typedef struct { |
90 | 92 | ||
91 | long EAST[MITTELWERTZAHL]; |
93 | long EAST[MITTELWERTZAHL]; |
92 | 94 | ||
93 | } FILTER_EAST; // Filterdefinition für die Positions-Ostwerte aus dem GPS-Empfänger |
95 | } FILTER_EAST; // Filterdefinition für die Positions-Ostwerte aus dem GPS-Empfänger |
94 | 96 | ||
95 | FILTER_NORTH filter_north; |
97 | FILTER_NORTH filter_north; |
96 | FILTER_EAST filter_east; |
98 | FILTER_EAST filter_east; |
97 | 99 | ||
98 | //**************************** MIRCOS DEFINITIONSENDE FÜR DIE FILTERUNG |
100 | //**************************** MIRCOS DEFINITIONSENDE FÜR DIE FILTERUNG |
99 | 101 | ||
100 | 102 | ||
101 | void gps_main(void) |
103 | void gps_main(void) |
102 | { |
104 | { |
103 | 105 | ||
104 | //**************************** MIRCOS FILTERUNG ANFANG |
106 | //**************************** MIRCOS FILTERUNG ANFANG |
105 | //bilde neuen Positionsmittelwert, wenn 3D-Fix und neue GPS-Daten vorliegen |
107 | //bilde neuen Positionsmittelwert, wenn 3D-Fix und neue GPS-Daten vorliegen |
106 | // if(actualPos.GPSFix == 3 && newData_navPosUtm == 1) |
108 | // if(actualPos.GPSFix == 3 && newData_navPosUtm == 1) |
107 | if(actualPos.GPSFix == 3) // hier umkommentieren, wenn dauernd (also nicht nur mit der GPS-Updaterate) die GPS-Berechnung durchgeführt werden soll |
109 | if(actualPos.GPSFix == 3) // hier umkommentieren, wenn dauernd (also nicht nur mit der GPS-Updaterate) die GPS-Berechnung durchgeführt werden soll |
108 | 110 | ||
109 | { |
111 | { |
110 | 112 | ||
111 | filter_north.NORTH[FilterPos] = actualPos.northing; |
113 | filter_north.NORTH[FilterPos] = actualPos.northing; |
112 | filter_east.EAST[FilterPos] = actualPos.easting; |
114 | filter_east.EAST[FilterPos] = actualPos.easting; |
113 | FilterPos++; |
115 | FilterPos++; |
114 | if (FilterPos==MITTELWERTZAHL) FilterPos=0; |
116 | if (FilterPos==MITTELWERTZAHL) FilterPos=0; |
115 | 117 | ||
116 | if (filterfilled<MITTELWERTZAHL) filterfilled++; |
118 | if (filterfilled<MITTELWERTZAHL) filterfilled++; |
117 | 119 | ||
118 | NORTH_LONG_LONG = 0; |
120 | NORTH_LONG_LONG = 0; |
119 | EAST_LONG_LONG = 0; |
121 | EAST_LONG_LONG = 0; |
120 | for (int i=0; i<filterfilled; i++) |
122 | for (int i=0; i<filterfilled; i++) |
121 | { |
123 | { |
122 | NORTH_LONG_LONG += filter_north.NORTH[i]; |
124 | NORTH_LONG_LONG += filter_north.NORTH[i]; |
123 | EAST_LONG_LONG += filter_east.EAST[i]; |
125 | EAST_LONG_LONG += filter_east.EAST[i]; |
124 | } |
126 | } |
125 | NORTH_LONG_LONG /= filterfilled; |
127 | NORTH_LONG_LONG /= filterfilled; |
126 | EAST_LONG_LONG /= filterfilled; |
128 | EAST_LONG_LONG /= filterfilled; |
127 | 129 | ||
128 | NORTH_MITTEL = NORTH_LONG_LONG; |
130 | NORTH_MITTEL = NORTH_LONG_LONG; |
129 | EAST_MITTEL = EAST_LONG_LONG; |
131 | EAST_MITTEL = EAST_LONG_LONG; |
130 | 132 | ||
131 | NeuerMittelwert = 1; |
133 | NeuerMittelwert = 1; |
132 | newData_navPosUtm = 0; |
134 | newData_navPosUtm = 0; |
133 | } |
135 | } |
134 | //******************************* MIRCOS FILTERUNG ENDE |
136 | //******************************* MIRCOS FILTERUNG ENDE |
135 | 137 | ||
136 | 138 | ||
137 | 139 | ||
138 | 140 | ||
139 | //wenn kein 3D-Fix vorhanden, über das Poti jedoch irgendeine GPS Funktion aktiv ist, werden die GPS-Steuerbefehle |
141 | //wenn kein 3D-Fix vorhanden, über das Poti jedoch irgendeine GPS Funktion aktiv ist, werden die GPS-Steuerbefehle |
140 | //auf Null gesetzt, um unkontrolliertes Wegfliegen durch das GPS zu verhindern. Dadurch wird abgesichert |
142 | //auf Null gesetzt, um unkontrolliertes Wegfliegen durch das GPS zu verhindern. Dadurch wird abgesichert |
141 | //(auch im laufenden Flug), dass bei einem Verlust der GPS-Verbindung GPS deaktivert wird |
143 | //(auch im laufenden Flug), dass bei einem Verlust der GPS-Verbindung GPS deaktivert wird |
142 | if (actualPos.GPSFix < 3 && Poti3 > 70) |
144 | if (actualPos.GPSFix < 3 && Poti3 > 70) |
143 | { |
145 | { |
144 | GPS_Nick = 0; |
146 | GPS_Nick = 0; |
145 | GPS_Roll = 0; |
147 | GPS_Roll = 0; |
146 | LED2_OFF; |
148 | LED2_OFF; |
147 | beeptime = 50; |
149 | beeptime = 50; |
148 | } |
150 | } |
149 | 151 | ||
150 | 152 | ||
151 | 153 | ||
152 | // speichert die GPS_Home_Position beim 3D-Fix, MotorEINschalten und wenn noch nicht zu weit weggeflogen wurde |
154 | // speichert die GPS_Home_Position beim 3D-Fix, MotorEINschalten und wenn noch nicht zu weit weggeflogen wurde |
153 | // (modell_fliegt_gps < 2000) einmalig ab. Die Flugzeitabfrage sichert ab, dass bei einem Losfliegen ohne 3D-Fix die Home_Positinen |
155 | // (modell_fliegt_gps < 2000) einmalig ab. Die Flugzeitabfrage sichert ab, dass bei einem Losfliegen ohne 3D-Fix die Home_Positinen |
154 | // irgendwann später dort gelernt werden, wo dann zu ersten mal ein 3D-Fix vorhanden ist |
156 | // irgendwann später dort gelernt werden, wo dann zu ersten mal ein 3D-Fix vorhanden ist |
155 | if (actualPos.GPSFix == 3 && MotorenEin == 1 && gethome == 0 && modell_fliegt_gps < 2000) |
157 | if (actualPos.GPSFix == 3 && MotorenEin == 1 && gethome == 0 && modell_fliegt_gps < 2000) |
156 | { |
158 | { |
157 | GPS_Home_North = NORTH_MITTEL; |
159 | GPS_Home_North = NORTH_MITTEL; |
158 | GPS_Home_East = EAST_MITTEL; |
160 | GPS_Home_East = EAST_MITTEL; |
159 | // Muss hier auch eingelernt werden, damit falls das Poti3 beim Motorenstarten in |
161 | // Muss hier auch eingelernt werden, damit falls das Poti3 beim Motorenstarten in |
160 | // Mittelstellung sein sollte, trotzdem schon eine Sollposition vorliegt. Andernfalls würde |
162 | // Mittelstellung sein sollte, trotzdem schon eine Sollposition vorliegt. Andernfalls würde |
161 | // GPS_Nick und _Roll maximal werden bis zum ersten Mal Stick_Nick und _Roll gesteuert würde |
163 | // GPS_Nick und _Roll maximal werden bis zum ersten Mal Stick_Nick und _Roll gesteuert würde |
162 | Soll_Position_North = NORTH_MITTEL; |
164 | Soll_Position_North = NORTH_MITTEL; |
163 | Soll_Position_East = EAST_MITTEL; |
165 | Soll_Position_East = EAST_MITTEL; |
164 | beeptime = 50; |
166 | beeptime = 50; |
165 | gethome = 1; |
167 | gethome = 1; |
166 | } |
168 | } |
167 | 169 | ||
168 | 170 | ||
169 | 171 | ||
170 | 172 | ||
171 | //löscht die GPS_Home_Position beim MotorAUSschalten wieder und resetet das Flag gethome //(280807Kr) |
173 | //löscht die GPS_Home_Position beim MotorAUSschalten wieder und resetet das Flag gethome //(280807Kr) |
172 | if (MotorenEin == 0) |
174 | if (MotorenEin == 0) |
173 | { |
175 | { |
174 | GPS_Home_North = 0; |
176 | GPS_Home_North = 0; |
175 | GPS_Home_East = 0; |
177 | GPS_Home_East = 0; |
176 | gethome = 0; |
178 | gethome = 0; |
177 | modell_fliegt_gps = 0; |
179 | modell_fliegt_gps = 0; |
178 | } |
180 | } |
179 | 181 | ||
180 | 182 | ||
181 | 183 | ||
182 | 184 | ||
183 | // wenn Schalter ausgeschaltet, dann wird die LED2 ausgeschaltet und das GPS Steuerkommando gelöscht |
185 | // wenn Schalter ausgeschaltet, dann wird die LED2 ausgeschaltet und das GPS Steuerkommando gelöscht |
184 | if(Poti3 < 70) |
186 | if(Poti3 < 70) |
185 | { |
187 | { |
186 | // Sollpositionen müssen auch hier beschrieben werden, damit ausgeschlossen ist, dass falls gelandet wird und die GPS-Funktion |
188 | // Sollpositionen müssen auch hier beschrieben werden, damit ausgeschlossen ist, dass falls gelandet wird und die GPS-Funktion |
187 | // über Poti3 deaktivert und anschließend wieder aktiviert wird keine Soll-Positionen vorliegen. |
189 | // über Poti3 deaktivert und anschließend wieder aktiviert wird keine Soll-Positionen vorliegen. |
188 | Soll_Position_North = NORTH_MITTEL; |
190 | Soll_Position_North = NORTH_MITTEL; |
189 | Soll_Position_East = EAST_MITTEL; |
191 | Soll_Position_East = EAST_MITTEL; |
190 | 192 | ||
191 | GPS_Nick = 0; |
193 | GPS_Nick = 0; |
192 | GPS_Roll = 0; |
194 | GPS_Roll = 0; |
193 | LED2_OFF; |
195 | LED2_OFF; |
194 | //hierdurch soll verhindert werden, dass wenn nach schonmal aktiver GPS Funktion (und damit 3D-fix) der GPS-Empfänger |
196 | //hierdurch soll verhindert werden, dass wenn nach schonmal aktiver GPS Funktion (und damit 3D-fix) der GPS-Empfänger |
195 | //abgezogen werden sollte weiterhin ein 3D-fix angezeigt wird. Grund ist, dass in diesem Fall in der UART.c nach einem |
197 | //abgezogen werden sollte weiterhin ein 3D-fix angezeigt wird. Grund ist, dass in diesem Fall in der UART.c nach einem |
196 | //Abziehen der Status nicht mehr auf Null zurückgesetzt wird. Dies geschied nun beim Ausschalten der GPS-Funktion. |
198 | //Abziehen der Status nicht mehr auf Null zurückgesetzt wird. Dies geschied nun beim Ausschalten der GPS-Funktion. |
197 | //Sie ist dann ohne erneuten 3D-Fix nicht mehr aktivierbar. (111107Kr) |
199 | //Sie ist dann ohne erneuten 3D-Fix nicht mehr aktivierbar. (111107Kr) |
198 | actualPos.GPSFix = 0; |
200 | actualPos.GPSFix = 0; |
199 | } |
201 | } |
200 | 202 | ||
201 | 203 | ||
202 | // wenn 3D-Fix, Schalter im mittleren Bereich und vom Pilot gesteuert wird, wird ständig neue GPS-Sollposition eingelernt |
204 | // wenn 3D-Fix, Schalter im mittleren Bereich und vom Pilot gesteuert wird, wird ständig neue GPS-Sollposition eingelernt |
203 | if(actualPos.GPSFix == 3 && Poti3 > 70 && Poti3 < 150 && (StickNick > GPS_StickSense || StickNick < -GPS_StickSense || StickRoll > GPS_StickSense || StickRoll < -GPS_StickSense)) |
205 | if(actualPos.GPSFix == 3 && Poti3 > 70 && Poti3 < 150 && (StickNick > GPS_StickSense || StickNick < -GPS_StickSense || StickRoll > GPS_StickSense || StickRoll < -GPS_StickSense)) |
204 | { |
206 | { |
205 | GPS_Nick = 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert |
207 | GPS_Nick = 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert |
206 | GPS_Roll = 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert |
208 | GPS_Roll = 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert |
207 | 209 | ||
208 | Soll_Position_North = NORTH_MITTEL; // Soll-Position wird geschrieben |
210 | Soll_Position_North = NORTH_MITTEL; // Soll-Position wird geschrieben |
209 | Soll_Position_East = EAST_MITTEL; // Soll-Position wird geschrieben |
211 | Soll_Position_East = EAST_MITTEL; // Soll-Position wird geschrieben |
210 | 212 | ||
211 | //++++++++++++++++++++++++++++++++++++ |
213 | //++++++++++++++++++++++++++++++++++++ |
212 | //Sorgt lediglich dafür, dass die LED2 langsam blinkt. Dauer ist durch die "> x" Abfrage festgelegt |
214 | //Sorgt lediglich dafür, dass die LED2 langsam blinkt. Dauer ist durch die "> x" Abfrage festgelegt |
213 | //++++++++++++++++++++++++++++++++++++ |
215 | //++++++++++++++++++++++++++++++++++++ |
214 | if(blinkcount_LED2 >= 200) // maximal erlaubt 255 wegen unsigned char |
216 | if(blinkcount_LED2 >= 200) // maximal erlaubt 255 wegen unsigned char |
215 | { |
217 | { |
216 | LED2_FLASH; |
218 | LED2_FLASH; |
217 | blinkcount_LED2 = 0; |
219 | blinkcount_LED2 = 0; |
218 | } |
220 | } |
219 | blinkcount_LED2++; |
221 | blinkcount_LED2++; |
220 | //++++++++++++++++++++++++++++++++++++ |
222 | //++++++++++++++++++++++++++++++++++++ |
221 | } |
223 | } |
222 | 224 | ||
223 | 225 | ||
224 | // wenn 3D-Fix, Schalter im mittleren Bereich (oder höher), neuer Mittelwert vorhanden, wenigstens einmal Sollposition gelernt und der Pilot nicht selbst steuert wird eine GPS-Funktion aktiviert |
226 | // wenn 3D-Fix, Schalter im mittleren Bereich (oder höher), neuer Mittelwert vorhanden, wenigstens einmal Sollposition gelernt und der Pilot nicht selbst steuert wird eine GPS-Funktion aktiviert |
225 | if (actualPos.GPSFix == 3 && Poti3 > 70 && NeuerMittelwert == 1 && Soll_Position_North != 0 && Soll_Position_East != 0 && StickNick <= GPS_StickSense && StickNick >= -GPS_StickSense && StickRoll <= GPS_StickSense && StickRoll >= -GPS_StickSense) |
227 | if (actualPos.GPSFix == 3 && Poti3 > 70 && NeuerMittelwert == 1 && Soll_Position_North != 0 && Soll_Position_East != 0 && StickNick <= GPS_StickSense && StickNick >= -GPS_StickSense && StickRoll <= GPS_StickSense && StickRoll >= -GPS_StickSense) |
226 | { |
228 | { |
227 | //Berechnung der GPS-Positionsabweichung je nach aktivierter GPS-Funktion (GPS_HOLD oder HOMING) |
229 | //Berechnung der GPS-Positionsabweichung je nach aktivierter GPS-Funktion (GPS_HOLD oder HOMING) |
228 | 230 | ||
229 | //DYNAMISCHES GPS-HOLD, wenn Schalter noch in Mittelstellung ist wird die zuletzt gelernte Soll_Position angeflogen |
231 | //DYNAMISCHES GPS-HOLD, wenn Schalter noch in Mittelstellung ist wird die zuletzt gelernte Soll_Position angeflogen |
230 | if(Poti3 < 150) |
232 | if(Poti3 < 150) |
231 | { |
233 | { |
232 | GPS_Positionsabweichung_North = (Soll_Position_North - NORTH_MITTEL); |
234 | GPS_Positionsabweichung_North = (Soll_Position_North - NORTH_MITTEL); |
233 | GPS_Positionsabweichung_East = (Soll_Position_East - EAST_MITTEL); |
235 | GPS_Positionsabweichung_East = (Soll_Position_East - EAST_MITTEL); |
234 | LED2_ON; |
236 | LED2_ON; |
235 | } |
237 | } |
236 | 238 | ||
237 | //andernfalls, wenn Schalter in max. Stellung und zusätzlich die GPS_Home_Positionen beim Motorenstarten gelernt |
239 | //andernfalls, wenn Schalter in max. Stellung und zusätzlich die GPS_Home_Positionen beim Motorenstarten gelernt |
238 | //werden konnten, wird zur GPS_Home_Position geflogen |
240 | //werden konnten, wird zur GPS_Home_Position geflogen |
239 | if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0) |
241 | if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0) |
240 | { |
242 | { |
241 | GPS_Positionsabweichung_North = (GPS_Home_North - NORTH_MITTEL); |
243 | GPS_Positionsabweichung_North = (GPS_Home_North - NORTH_MITTEL); |
242 | GPS_Positionsabweichung_East = (GPS_Home_East - EAST_MITTEL); |
244 | GPS_Positionsabweichung_East = (GPS_Home_East - EAST_MITTEL); |
243 | // durch das ständige Neulernen der Soll_Positionen während HOMING wird erreicht, dass der MK nach dem Umschalten |
245 | // durch das ständige Neulernen der Soll_Positionen während HOMING wird erreicht, dass der MK nach dem Umschalten |
244 | // von HOMING zurück zu GPS-HOLD nicht wieder an die alte Soll_Position zurück fliegt |
246 | // von HOMING zurück zu GPS-HOLD nicht wieder an die alte Soll_Position zurück fliegt |
245 | Soll_Position_North = NORTH_MITTEL; |
247 | Soll_Position_North = NORTH_MITTEL; |
246 | Soll_Position_East = EAST_MITTEL; |
248 | Soll_Position_East = EAST_MITTEL; |
247 | LED2_ON; |
249 | LED2_ON; |
248 | } |
250 | } |
249 | 251 | ||
250 | //verhindert, dass beim Umschalten von Mittelstellung (GPS_HOLD) auf Maximalstellung (HOMING) OHNE gelernte Homeposition |
252 | //verhindert, dass beim Umschalten von Mittelstellung (GPS_HOLD) auf Maximalstellung (HOMING) OHNE gelernte Homeposition |
251 | //die GPS Funktion unkontrolliert steuert. Daher wird GPS_HOLD aktiv gehalten, die Sollpositon wird bei Stickbefehlen |
253 | //die GPS Funktion unkontrolliert steuert. Daher wird GPS_HOLD aktiv gehalten, die Sollpositon wird bei Stickbefehlen |
252 | //jedoch nicht wie beim dynamischen GPS_HOLD überschrieben. Zusätzlich wird der Summer eingeschaltet. |
254 | //jedoch nicht wie beim dynamischen GPS_HOLD überschrieben. Zusätzlich wird der Summer eingeschaltet. |
253 | if (Poti3 >= 150 && GPS_Home_North == 0 && GPS_Home_East == 0) |
255 | if (Poti3 >= 150 && GPS_Home_North == 0 && GPS_Home_East == 0) |
254 | { |
256 | { |
255 | GPS_Positionsabweichung_North = (Soll_Position_North - NORTH_MITTEL); |
257 | GPS_Positionsabweichung_North = (Soll_Position_North - NORTH_MITTEL); |
256 | GPS_Positionsabweichung_East = (Soll_Position_East - EAST_MITTEL); |
258 | GPS_Positionsabweichung_East = (Soll_Position_East - EAST_MITTEL); |
257 | LED2_ON; |
259 | LED2_ON; |
258 | beeptime = 50; |
260 | beeptime = 50; |
259 | } |
261 | } |
260 | 262 | ||
261 | //GPS-Geschwindigkeiten |
263 | //GPS-Geschwindigkeiten |
262 | GPS_Geschwindigkeit_North = actualPos.velNorth; //actualPos.velNorth ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Nord-Süd Richtung |
264 | GPS_Geschwindigkeit_North = actualPos.velNorth; //actualPos.velNorth ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Nord-Süd Richtung |
263 | GPS_Geschwindigkeit_East = actualPos.velEast; //actualPos.velEast ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Ost-West Richtung |
265 | GPS_Geschwindigkeit_East = actualPos.velEast; //actualPos.velEast ist die Geschwindigkeiten aus dem GPS Empfänger entlang der Ost-West Richtung |
264 | 266 | ||
265 | 267 | ||
266 | //Berechnung der Einzelkomponenten des PD-Reglers |
268 | //Berechnung der Einzelkomponenten des PD-Reglers |
267 | P_Einfluss_North = (P_GPS_Verstaerkung * GPS_Positionsabweichung_North)*0.0005; //entspricht einer Skalierung von 1/2000 |
269 | P_Einfluss_North = (P_GPS_Verstaerkung * GPS_Positionsabweichung_North)*0.0005; //entspricht einer Skalierung von 1/2000 |
268 | D_Einfluss_North = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_North)*0.00333; //entspricht einer Skalierung von 1/300 |
270 | D_Einfluss_North = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_North)*0.00333; //entspricht einer Skalierung von 1/300 |
269 | 271 | ||
270 | P_Einfluss_East = (P_GPS_Verstaerkung * GPS_Positionsabweichung_East)*0.0005; //entspricht einer Skalierung von 1/2000 |
272 | P_Einfluss_East = (P_GPS_Verstaerkung * GPS_Positionsabweichung_East)*0.0005; //entspricht einer Skalierung von 1/2000 |
271 | D_Einfluss_East = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_East)*0.00333; //entspricht einer Skalierung von 1/300 |
273 | D_Einfluss_East = (D_GPS_Verstaerkung * GPS_Geschwindigkeit_East)*0.00333; //entspricht einer Skalierung von 1/300 |
272 | 274 | ||
273 | 275 | ||
274 | // Begrenzung des maximalen D-Anteils für pos. und neg. Werte. Grenze muss so geählt werden, |
276 | // Begrenzung des maximalen D-Anteils für pos. und neg. Werte. Grenze muss so geählt werden, |
275 | // dass das Limit im normalen Position-Hold nicht erreicht wird und somit keinen Einfluss hat. |
277 | // dass das Limit im normalen Position-Hold nicht erreicht wird und somit keinen Einfluss hat. |
276 | // Grund: Beim Zufliegen auf weiter entfernte Ziele bremst der D-Anteil kurz vor dem Ziel den Mikrokopter sehr stark ab. |
278 | // Grund: Beim Zufliegen auf weiter entfernte Ziele bremst der D-Anteil kurz vor dem Ziel den Mikrokopter sehr stark ab. |
277 | // Dieses Limit wirkt nur, wenn sich der MK im HOMING Modus befindet und sich noch weit (>2,5m) von der Home-Position weg befindet. |
279 | // Dieses Limit wirkt nur, wenn sich der MK im HOMING Modus befindet und sich noch weit (>2,5m) von der Home-Position weg befindet. |
278 | if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0 && (abs(GPS_Positionsabweichung_North) > 250 || abs(GPS_Positionsabweichung_East) > 250)) |
280 | if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0 && (abs(GPS_Positionsabweichung_North) > 250 || abs(GPS_Positionsabweichung_East) > 250)) |
279 | { |
281 | { |
280 | if (D_Einfluss_North > Limit_D_Anteil) D_Einfluss_North = Limit_D_Anteil; |
282 | if (D_Einfluss_North > Limit_D_Anteil) D_Einfluss_North = Limit_D_Anteil; |
281 | if (D_Einfluss_East > Limit_D_Anteil) D_Einfluss_East = Limit_D_Anteil; |
283 | if (D_Einfluss_East > Limit_D_Anteil) D_Einfluss_East = Limit_D_Anteil; |
282 | if (D_Einfluss_North < -1 * Limit_D_Anteil) D_Einfluss_North = -1 * Limit_D_Anteil; |
284 | if (D_Einfluss_North < -1 * Limit_D_Anteil) D_Einfluss_North = -1 * Limit_D_Anteil; |
283 | if (D_Einfluss_East < -1 * Limit_D_Anteil) D_Einfluss_East = -1 * Limit_D_Anteil; |
285 | if (D_Einfluss_East < -1 * Limit_D_Anteil) D_Einfluss_East = -1 * Limit_D_Anteil; |
284 | } |
286 | } |
285 | 287 | ||
286 | // PD-Regler |
288 | // PD-Regler |
287 | GPS_North = (-P_Einfluss_North + D_Einfluss_North); |
289 | GPS_North = (-P_Einfluss_North + D_Einfluss_North); |
288 | GPS_East = (P_Einfluss_East - D_Einfluss_East); |
290 | GPS_East = (P_Einfluss_East - D_Einfluss_East); |
289 | 291 | ||
290 | //Umrechnen vom globalen North- bzw. East- in das körperfeste X- bzw. Y- Koordinatensystem |
292 | //Umrechnen vom globalen North- bzw. East- in das körperfeste X- bzw. Y- Koordinatensystem |
291 | GPS_Nick = (-c_sin[KompassValue]*0.001*GPS_East + c_cos[KompassValue]*0.001*GPS_North); // die "0" kann später durch den Messwert des Kompasssensors (KompassValue/0.01745329251) ersetzt werden |
293 | GPS_Nick = (-c_sin[KompassValue]*0.001*GPS_East + c_cos[KompassValue]*0.001*GPS_North); // die "0" kann später durch den Messwert des Kompasssensors (KompassValue/0.01745329251) ersetzt werden |
292 | GPS_Roll = (c_cos[KompassValue]*0.001*GPS_East + c_sin[KompassValue]*0.001*GPS_North); // die "0" kann später durch den Messwert des Kompasssensors (KompassValue/180*3.1415926535) ersetzt werden |
294 | GPS_Roll = (c_cos[KompassValue]*0.001*GPS_East + c_sin[KompassValue]*0.001*GPS_North); // die "0" kann später durch den Messwert des Kompasssensors (KompassValue/180*3.1415926535) ersetzt werden |
293 | 295 | ||
294 | // Begrenzung des maximalen GPS Einflusses für positive und negative Werte |
296 | // Begrenzung des maximalen GPS Einflusses für positive und negative Werte |
295 | if (GPS_Nick > GPS_Limit) GPS_Nick = GPS_Limit; |
297 | if (GPS_Nick > GPS_Limit) GPS_Nick = GPS_Limit; |
296 | if (GPS_Roll > GPS_Limit) GPS_Roll = GPS_Limit; |
298 | if (GPS_Roll > GPS_Limit) GPS_Roll = GPS_Limit; |
297 | if (GPS_Nick < -1 * GPS_Limit) GPS_Nick = -1 * GPS_Limit; |
299 | if (GPS_Nick < -1 * GPS_Limit) GPS_Nick = -1 * GPS_Limit; |
298 | if (GPS_Roll < -1 * GPS_Limit) GPS_Roll = -1 * GPS_Limit; |
300 | if (GPS_Roll < -1 * GPS_Limit) GPS_Roll = -1 * GPS_Limit; |
299 | 301 | ||
300 | //Funktion wird dadurch nur alle 250 ms aufgerufen, wenn neue Mittelwerte aus den GPS-Daten vorliegen. |
302 | //Funktion wird dadurch nur alle 250 ms aufgerufen, wenn neue Mittelwerte aus den GPS-Daten vorliegen. |
301 | //FUNKTIONIERT MOMENTAN NOCH NICHT, DA STÄNDIG OHNE UNTERBRECHUNG NEUE MITTELWERTE BERECHNET WERDEN. |
303 | //FUNKTIONIERT MOMENTAN NOCH NICHT, DA STÄNDIG OHNE UNTERBRECHUNG NEUE MITTELWERTE BERECHNET WERDEN. |
302 | //GRUND: DIE ABFRAGE NACH NEUE GPS-DATEN ERFOLGT NOCH NICHT |
304 | //GRUND: DIE ABFRAGE NACH NEUE GPS-DATEN ERFOLGT NOCH NICHT |
303 | NeuerMittelwert = 0; |
305 | NeuerMittelwert = 0; |
304 | } |
306 | } |
305 | } |
307 | } |
306 | 308 | ||
307 | 309 |