Rev 796 | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
796 | joko | 1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
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. |
||
4 | // + Personen- und/der Sachschäden ist jegliche Haftung ausgeschlossen |
||
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
6 | #include "main.h" |
||
7 | |||
8 | |||
9 | long Soll_Position_North = 0; |
||
10 | long Soll_Position_East = 0; |
||
11 | |||
12 | long GPS_Positionsabweichung_North = 0; |
||
13 | long GPS_Positionsabweichung_East = 0; |
||
14 | |||
15 | long P_Einfluss_North = 0; |
||
16 | long D_Einfluss_North = 0; |
||
17 | |||
18 | long P_Einfluss_East = 0; |
||
19 | long D_Einfluss_East = 0; |
||
20 | |||
21 | |||
22 | signed int GPS_North = 0; |
||
23 | signed int GPS_East = 0; |
||
24 | signed int GPS_Nick = 0; |
||
25 | signed int GPS_Roll = 0; |
||
26 | signed int GPS_Nick_tmp = 0; //(250308Kr) |
||
27 | signed int GPS_Roll_tmp = 0; //(250308Kr) |
||
28 | signed int GPS_Nick_ACC = 0; //(250308Kr) |
||
29 | signed int GPS_Roll_ACC = 0; //(250308Kr) |
||
30 | |||
31 | char gethome = 0; //Flag ob GPS_Home_Position gelernt ist //(280807Kr) |
||
32 | long GPS_Home_North = 0; |
||
33 | long GPS_Home_East = 0; |
||
34 | |||
35 | unsigned char blinkcount_LED2 = 0; //Hilfszähler für die blinkende LED (010907Kr) |
||
36 | |||
37 | #define GPS_Limit 35 //(031207Kr) |
||
38 | #define Limit_D_Anteil 50 //(031207Kr) |
||
39 | #define GPS_StickSense 12 //(241207Kr) |
||
40 | #define Limit_ACC_Anteil 25 //(250308Kr) |
||
41 | |||
42 | void gps_main(void) |
||
43 | { |
||
44 | |||
45 | //wenn kein 3D-Fix vorhanden, über das Poti jedoch irgendeine GPS Funktion aktiv ist, werden die GPS-Steuerbefehle |
||
46 | //auf Null gesetzt, um unkontrolliertes Wegfliegen durch das GPS zu verhindern. Dadurch wird abgesichert |
||
47 | //(auch im laufenden Flug), dass bei einem Verlust der GPS-Verbindung GPS deaktivert wird |
||
48 | if (actualPos.GPSFix < 3 && Poti3 > 70) |
||
49 | { |
||
50 | GPS_Nick = 0; |
||
51 | GPS_Roll = 0; |
||
52 | LED2_OFF; |
||
53 | beeptime = 50; |
||
54 | } |
||
55 | |||
56 | |||
57 | |||
58 | // speichert die GPS_Home_Position beim 3D-Fix, MotorEINschalten und wenn noch nicht zu weit weggeflogen wurde |
||
59 | // (modell_fliegt_gps < 2000) einmalig ab. Die Flugzeitabfrage sichert ab, dass bei einem Losfliegen ohne 3D-Fix |
||
60 | // die Home_Positinen irgendwann später dort gelernt werden, wo dann zu ersten mal ein 3D-Fix vorhanden ist |
||
61 | if (actualPos.GPSFix == 3 && MotorenEin == 1 && gethome == 0 && modell_fliegt_gps < 2000) |
||
62 | { |
||
63 | GPS_Home_North = actualPos.northing; |
||
64 | GPS_Home_East = actualPos.easting; |
||
65 | // Muss hier auch eingelernt werden, damit falls das Poti3 beim Motorenstarten in |
||
66 | // Mittelstellung sein sollte, trotzdem schon eine Sollposition vorliegt. Andernfalls würde |
||
67 | // GPS_Nick und _Roll maximal werden bis zum ersten Mal Stick_Nick und _Roll gesteuert würde |
||
68 | Soll_Position_North = actualPos.northing; |
||
69 | Soll_Position_East = actualPos.easting; |
||
70 | beeptime = 50; |
||
71 | gethome = 1; |
||
72 | } |
||
73 | |||
74 | |||
75 | //schaltet bei Verwendung des GPS/MM3-Boards von Walter, GPS an RX von UART0 //(050208Kr) |
||
76 | if (MotorenEin == 1) |
||
77 | { |
||
78 | SWITCH_GPS_2_RX; |
||
79 | } |
||
80 | |||
81 | |||
82 | //löscht die GPS_Home_Position beim MotorAUSschalten wieder und resetet das Flag gethome //(280807Kr) |
||
83 | //zusätzlich wird bei Verwendung des GPS/MM3-Boards von Walter BT wieder an RX von UART0 geschaltet //(050208Kr) |
||
84 | if (MotorenEin == 0) |
||
85 | { |
||
86 | SWITCH_BT_2_RX; |
||
87 | GPS_Home_North = 0; |
||
88 | GPS_Home_East = 0; |
||
89 | gethome = 0; |
||
90 | modell_fliegt_gps = 0; |
||
91 | } |
||
92 | |||
93 | |||
94 | |||
95 | |||
96 | // wenn Schalter ausgeschaltet, dann wird die LED2 ausgeschaltet und das GPS Steuerkommando gelöscht |
||
97 | if(Poti3 <= 70) |
||
98 | { |
||
99 | // Sollpositionen müssen auch hier beschrieben werden, damit ausgeschlossen ist, dass falls gelandet |
||
100 | // wird und die GPS-Funktion über Poti3 deaktivert und anschließend wieder aktiviert wird keine Soll-Positionen vorliegen. |
||
101 | Soll_Position_North = actualPos.northing; |
||
102 | Soll_Position_East = actualPos.easting; |
||
103 | |||
104 | GPS_Nick = 0; |
||
105 | GPS_Roll = 0; |
||
106 | LED2_OFF; |
||
107 | } |
||
108 | |||
109 | |||
110 | // wenn 3D-Fix, Schalter im mittleren Bereich und vom Pilot gesteuert wird, wird ständig neue GPS-Sollposition eingelernt |
||
111 | if(actualPos.GPSFix == 3 && Poti3 > 70 && Poti3 < 150 && (StickNick > GPS_StickSense || StickNick < -GPS_StickSense || StickRoll > GPS_StickSense || StickRoll < -GPS_StickSense)) |
||
112 | { |
||
113 | GPS_Nick = 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert |
||
114 | GPS_Roll = 0; //wird genullt, damit das letzte GPS Kommando nicht ständig gegen den Piloten ansteuert |
||
115 | |||
116 | Soll_Position_North = actualPos.northing; // Soll-Position wird geschrieben |
||
117 | Soll_Position_East = actualPos.easting; // Soll-Position wird geschrieben |
||
118 | |||
119 | //++++++++++++++++++++++++++++++++++++ |
||
120 | //Sorgt lediglich dafür, dass die LED2 langsam blinkt. Dauer ist durch die "> x" Abfrage festgelegt |
||
121 | //++++++++++++++++++++++++++++++++++++ |
||
122 | if(blinkcount_LED2 >= 200) // maximal erlaubt 255 wegen unsigned char |
||
123 | { |
||
124 | LED2_FLASH; |
||
125 | blinkcount_LED2 = 0; |
||
126 | } |
||
127 | blinkcount_LED2++; |
||
128 | //++++++++++++++++++++++++++++++++++++ |
||
129 | } |
||
130 | |||
131 | |||
132 | // wenn 3D-Fix, Schalter im mittleren Bereich (oder höher), neuer Mittelwert vorhanden, wenigstens einmal Sollposition |
||
133 | // gelernt und der Pilot nicht selbst steuert wird eine GPS-Funktion aktiviert |
||
134 | if (actualPos.GPSFix == 3 && Poti3 > 70 && Soll_Position_North != 0 && Soll_Position_East != 0 && StickNick <= GPS_StickSense && StickNick >= -GPS_StickSense && StickRoll <= GPS_StickSense && StickRoll >= -GPS_StickSense) |
||
135 | { |
||
136 | //Berechnung der GPS-Positionsabweichung je nach aktivierter GPS-Funktion (GPS_HOLD oder HOMING) |
||
137 | |||
138 | //DYNAMISCHES GPS-HOLD, wenn Schalter in Mittelstellung ist UND KEINE Notlandung, wird die zuletzt gelernte Soll_Position angeflogen |
||
139 | if(Poti3 < 150 && Notlandung == 0) //(050208Kr) |
||
140 | { |
||
141 | GPS_Positionsabweichung_North = (Soll_Position_North - actualPos.northing); |
||
142 | GPS_Positionsabweichung_East = (Soll_Position_East - actualPos.easting); |
||
143 | LED2_ON; |
||
144 | } |
||
145 | |||
146 | //andernfalls, wenn Schalter in max. Stellung ODER Notlandung und zusätzlich die GPS_Home_Positionen beim Motorenstarten gelernt |
||
147 | //werden konnten, wird zur GPS_Home_Position geflogen |
||
148 | if ((Poti3 >= 150 || Notlandung == 1) && GPS_Home_North != 0 && GPS_Home_East != 0) //(050208Kr) |
||
149 | { |
||
150 | GPS_Positionsabweichung_North = (GPS_Home_North - actualPos.northing); |
||
151 | GPS_Positionsabweichung_East = (GPS_Home_East - actualPos.easting); |
||
152 | // durch das ständige Neulernen der Soll_Positionen während HOMING wird erreicht, dass der MK nach dem Umschalten |
||
153 | // von HOMING zurück zu GPS-HOLD nicht wieder an die alte Soll_Position zurück fliegt |
||
154 | Soll_Position_North = actualPos.northing; |
||
155 | Soll_Position_East = actualPos.easting; |
||
156 | LED2_ON; |
||
157 | } |
||
158 | |||
159 | //verhindert, dass beim Umschalten von "GPS-HOLD" auf "HOMING" oder bei einer Notlandung OHNE gelernte Homeposition |
||
160 | //die GPS Funktion unkontrolliert steuert. Daher wird GPS_HOLD aktiv gehalten, die Sollpositon wird bei Stickbefehlen |
||
161 | //jedoch nicht wie beim dynamischen GPS_HOLD überschrieben. Zusätzlich wird der Summer eingeschaltet. |
||
162 | if ((Poti3 >= 150 || Notlandung == 1) && GPS_Home_North == 0 && GPS_Home_East == 0) //(050208Kr) |
||
163 | { |
||
164 | GPS_Positionsabweichung_North = (Soll_Position_North - actualPos.northing); |
||
165 | GPS_Positionsabweichung_East = (Soll_Position_East - actualPos.easting); |
||
166 | LED2_ON; |
||
167 | beeptime = 50; |
||
168 | } |
||
169 | |||
170 | |||
171 | |||
172 | //Berechnung der Einzelkomponenten des PD-Reglers |
||
173 | P_Einfluss_North = (P_GPS_Verstaerkung * GPS_Positionsabweichung_North) / 2048; |
||
174 | D_Einfluss_North = (D_GPS_Verstaerkung * actualPos.velNorth) / 256; |
||
175 | |||
176 | P_Einfluss_East = (P_GPS_Verstaerkung * GPS_Positionsabweichung_East) / 2048; |
||
177 | D_Einfluss_East = (D_GPS_Verstaerkung * actualPos.velEast) / 256; |
||
178 | |||
179 | |||
180 | // Begrenzung des maximalen D-Anteils für pos. und neg. Werte. Grenze muss so geählt werden, |
||
181 | // dass das Limit im normalen Position-Hold nicht erreicht wird und somit keinen Einfluss hat. |
||
182 | // Grund: Beim Zufliegen auf weiter entfernte Ziele bremst der D-Anteil kurz vor dem Ziel den Mikrokopter sehr stark ab. |
||
183 | // Dieses Limit wirkt nur, wenn sich der MK im HOMING Modus befindet und sich noch weit (>5m) von der Home-Position weg befindet. |
||
184 | if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0 && (abs(GPS_Positionsabweichung_North) > 500 || abs(GPS_Positionsabweichung_East) > 500)) |
||
185 | { |
||
186 | if (D_Einfluss_North > Limit_D_Anteil) D_Einfluss_North = Limit_D_Anteil; |
||
187 | if (D_Einfluss_East > Limit_D_Anteil) D_Einfluss_East = Limit_D_Anteil; |
||
188 | if (D_Einfluss_North < -Limit_D_Anteil) D_Einfluss_North = -Limit_D_Anteil; |
||
189 | if (D_Einfluss_East < -Limit_D_Anteil) D_Einfluss_East = -Limit_D_Anteil; |
||
190 | } |
||
191 | |||
192 | //PD-Regler |
||
193 | GPS_North = (-P_Einfluss_North + D_Einfluss_North); |
||
194 | GPS_East = (P_Einfluss_East - D_Einfluss_East); |
||
195 | |||
196 | |||
197 | //Diese Variante funktioniert genausogut und ist noch optimierter von der Ausührungszeit und Genauigkeit |
||
198 | GPS_Nick_tmp = (((long)-sin_i(KompassValue)*(long)GPS_East) + ((long)cos_i(KompassValue)*(long)GPS_North))/4096; |
||
199 | GPS_Roll_tmp = (((long)cos_i(KompassValue)*(long)GPS_East) + ((long)sin_i(KompassValue)*(long)GPS_North))/4096; |
||
200 | |||
201 | //*************************************** |
||
202 | //Einflüsse der ACC Sensoren (körperfest) |
||
203 | GPS_Nick_ACC = ((long)ACC_GPS_Verstaerkung * (long)Mittelwert_AccNick) / 1024; //mindestens durch 23,34 dividiert, um Überläufe zu verhindern |
||
204 | GPS_Roll_ACC = ((long)ACC_GPS_Verstaerkung * (long)Mittelwert_AccRoll) / 1024; //mindestens durch 23,34 dividiert, um Überläufe zu verhindern |
||
205 | |||
206 | if (GPS_Nick_ACC > Limit_ACC_Anteil) GPS_Nick_ACC = Limit_ACC_Anteil; |
||
207 | if (GPS_Roll_ACC > Limit_ACC_Anteil) GPS_Roll_ACC = Limit_ACC_Anteil; |
||
208 | if (GPS_Nick_ACC < -Limit_ACC_Anteil) GPS_Nick_ACC = -Limit_ACC_Anteil; |
||
209 | if (GPS_Roll_ACC < -Limit_ACC_Anteil) GPS_Roll_ACC = -Limit_ACC_Anteil; |
||
210 | |||
211 | GPS_Nick = GPS_Nick_tmp + GPS_Nick_ACC; |
||
212 | GPS_Roll = GPS_Roll_tmp + GPS_Roll_ACC; |
||
213 | //*************************************** |
||
214 | |||
215 | // Begrenzung des maximalen GPS Einflusses für positive und negative Werte |
||
216 | if (GPS_Nick > GPS_Limit) GPS_Nick = GPS_Limit; |
||
217 | if (GPS_Roll > GPS_Limit) GPS_Roll = GPS_Limit; |
||
218 | |||
219 | if (GPS_Nick < -GPS_Limit) GPS_Nick = -GPS_Limit; |
||
220 | if (GPS_Roll < -GPS_Limit) GPS_Roll = -GPS_Limit; |
||
221 | |||
222 | //newData_navPosUtm = 0; |
||
223 | } |
||
224 | } |
||
225 | |||
226 | //*************************** |
||
227 | // TO-DO: |
||
228 | // 1.) Vorzeichen von GPS_NICK_ACC und GPS_ROLL_ACC experimentell ermitteln |
||
229 | // 2.) Teilerfaktor (momentan 512) für sinnvollen Wertebereich ermitteln |
||
230 | // 3.) Testen ob Mittelwert_ACCXXX die richtige Signalgröße ist oder evntl. doch z.B. AdWertAccXXXX, ... |
||
231 | |||
232 | // 4.) Zeitverzögerung für P_Einfluss_XXXX nach neutralen Knüppeln integrieren |
||
233 | // 5.) Modularisieren / Unterfunktionen |
||
234 | // 6.) I-Anteil in die GPS-Regelung integrieren |
||
235 | // 7.) Updaterate für die zeitaufwändige Kompassberechnung auf 5Hz reduzieren |
||
236 | //*************************** |