Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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