Subversion Repositories FlightCtrl

Rev

Rev 1048 | Rev 1050 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1003 salvo 1
/*
2
This program (files gps.c and gps.h) is free software; you can redistribute it and/or modify
3
it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;
4
either version 3 of the License, or (at your option) any later version.  
5
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
6
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
7
GNU General Public License and GNU Lesser General Public License for more details.
8
You should have received a copy of GNU General Public License (License_GPL.txt)  and
9
GNU Lesser General Public License (License_LGPL.txt) along with this program.
10
If not, see <http://www.gnu.org/licenses/>.
11
 
12
Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de
13
*/
14
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15
von Peter Muehlenbrock alias Salvo
16
Auswertung der Daten vom GPS im ublox Format
17
Hold Modus mit PID Regler
18
Rückstuerz zur Basis Funktion
19
Umstellung auf NaviParameter an Flight Version 00.70d
20
GPS_V durch gps_gain ersetzt, damit Einstellung durch MK Tool möglich wird
21
 
1039 salvo 22
Stand 20.11.2008
23
 
24
Aenderung 20.11.2008: gps_gain erhoeht
1003 salvo 25
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
26
*/
1 ingob 27
#include "main.h"
1003 salvo 28
#include "math.h"
29
//#include "gps.h"
1 ingob 30
 
1003 salvo 31
// Defines fuer ublox Messageformat um Auswertung zu steuern
32
#define                 UBLOX_IDLE      0
33
#define                 UBLOX_SYNC1     1
34
#define                 UBLOX_SYNC2     2
35
#define                 UBLOX_CLASS     3
36
#define                 UBLOX_ID        4
37
#define                 UBLOX_LEN1      5
38
#define                 UBLOX_LEN2      6
39
#define                 UBLOX_CKA       7
40
#define                 UBLOX_CKB       8
41
#define                 UBLOX_PAYLOAD   9
42
 
43
// ublox Protokoll Identifier 
44
#define                 UBLOX_NAV_POSUTM        0x08
45
#define                 UBLOX_NAV_STATUS        0x03
46
#define                 UBLOX_NAV_VELED         0x12
47
#define                 UBLOX_NAV_CLASS         0x01
48
#define                 UBLOX_SYNCH1_CHAR       0xB5
49
#define                 UBLOX_SYNCH2_CHAR       0x62
50
 
51
signed int              GPS_Nick = 0;
52
signed int              GPS_Roll = 0;
53
signed int              GPS_Nick2 = 0;
54
signed int              GPS_Roll2 = 0;
55
short int               ublox_msg_state = UBLOX_IDLE;
56
static                  uint8_t chk_a =0; //Checksum
57
static                  uint8_t chk_b =0;
58
short int               gps_state,gps_sub_state; //Zustaende der Statemachine
59
short int               gps_updte_flag;
60
static long signed  gps_reg_x,gps_reg_y;                               
61
static unsigned int rx_len;
62
static unsigned int ptr_payload_data_end;
63
unsigned int            gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
64
static signed int       hdng_2home,dist_2home; //Richtung und Entfernung zur home Position 
65
static signed           gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt 
66
static                  short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
67
static                  uint8_t *ptr_payload_data;
68
static                  uint8_t *ptr_pac_status;
69
static  int             dist_flown;
70
//static unsigned int   int_ovfl_cnt; // Zaehler fuer Overflows des Integrators
71
static  int             gps_quiet_cnt; //  Zaehler fuer GPS Off Time beim Kameraausloesen
72
static  int             gps_gain;          //  // Teilerfaktor Regelabweichung zu Ausgabewert
73
 
74
 
75
short int Get_GPS_data(void);
76
 
77
NAV_POSUTM_t actual_pos;    // Aktuelle Nav Daten werden hier im ublox Format abgelegt
78
NAV_STATUS_t actual_status; // Aktueller Nav Status
79
NAV_VELNED_t actual_speed;  // Aktueller Geschwindigkeits und Richtungsdaten
80
 
81
GPS_ABS_POSITION_t              gps_act_position;               // Alle wichtigen Daten zusammengefasst
82
GPS_ABS_POSITION_t              gps_home_position;      // Die Startposition, beim Kalibrieren ermittelt
83
GPS_REL_POSITION_t              gps_rel_act_position;   // Die aktuelle relative Position bezogen auf Home Position
84
GPS_REL_POSITION_t              gps_rel_hold_position;  // Die gespeicherte Sollposition fuer GPS_ Hold Mode
85
GPS_REL_POSITION_t              gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
86
 
87
// Initialisierung
1 ingob 88
void GPS_Neutral(void)
89
{
1003 salvo 90
        ublox_msg_state                 =       UBLOX_IDLE;
91
        gps_state                               =       GPS_CRTL_IDLE;
92
        gps_sub_state                   =       GPS_CRTL_IDLE;
93
        actual_pos.status               =       0;
94
        actual_speed.status             =       0;
95
        actual_status.status            =       0;
96
        gps_home_position.status        =       0; // Noch keine gueltige Home Position
97
        gps_act_position.status         =       0;
98
        gps_rel_act_position.status     =       0;     
99
        GPS_Nick                        =       0;
100
        GPS_Roll                        =       0;
101
        gps_updte_flag                  =       0;
102
        gps_alive_cnt                   =       0;
103
 
1 ingob 104
}
105
 
1003 salvo 106
// Home Position sichern falls Daten verfuegbar sind. 
107
void GPS_Save_Home(void)
1 ingob 108
{
1003 salvo 109
        short int n;
110
        n = Get_GPS_data();
111
        if (n == 0)   // Gueltige  und aktuelle Daten ?   
112
        {
113
                // Neue GPS Daten liegen vor
114
                gps_home_position.utm_east      = gps_act_position.utm_east;   
115
                gps_home_position.utm_north     = gps_act_position.utm_north;  
116
                gps_home_position.utm_alt       = gps_act_position.utm_alt;
117
                gps_home_position.status        = 1; // Home Position gueltig                   
118
        }
1 ingob 119
}
120
 
1003 salvo 121
// Relative Position zur Home Position bestimmen
122
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
123
short int Get_Rel_Position(void)
124
{
125
        short int n = 0;
126
        n = Get_GPS_data();
127
        if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
128
        if (gps_alive_cnt < 1000) gps_alive_cnt += 600; // Timeoutzaehler. Wird in Motorregler Routine ueberwacht und dekrementiert
129
        if  (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
130
        {
131
                gps_rel_act_position.utm_east   = (int)  (gps_act_position.utm_east - gps_home_position.utm_east);
132
                gps_rel_act_position.utm_north  = (int)  (gps_act_position.utm_north - gps_home_position.utm_north);
133
                gps_rel_act_position.utm_alt    = (int)  (gps_act_position.utm_alt - gps_home_position.utm_alt);
134
                gps_rel_act_position.status     = 1; // gueltige Positionsdaten
135
                n = 0;
136
                gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
137
        }
138
        else
139
        {
140
                n = 2; //keine gueltigen Daten vorhanden
141
                gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
142
        }      
143
        return (n);
144
}
1 ingob 145
 
1003 salvo 146
// Daten aus aktuellen ublox Messages extrahieren 
147
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
148
short int Get_GPS_data(void)
149
{
150
        short int n = 1;
1 ingob 151
 
1003 salvo 152
        if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
1049 salvo 153
        gps_gain = (Parameter_NaviGpsGain*8)/20; //maximal Wert ist 255*8/40
1039 salvo 154
        //      debug_gp_0      = (int)gps_gain;  // zum Debuggen
1 ingob 155
 
1003 salvo 156
        if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
157
        {
158
                if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
159
                {
160
                        actual_status.status            = 0;
161
                        gps_act_position.utm_east       = actual_pos.utm_east/10;
162
                        gps_act_position.utm_north      = actual_pos.utm_north/10;
163
                        gps_act_position.utm_alt        = actual_pos.utm_alt/10;
164
                        actual_pos.status                       = 0; //neue ublox Messages anfordern
165
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd;
166
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd;
167
                        gps_act_position.heading        = actual_speed.heading/100000;
168
                        actual_speed.status             = 0;
169
                        gps_act_position.status         = 1;
170
                        n                                                       = 0; //Daten gueltig
171
                }
172
                else
173
                {
174
                        gps_act_position.status = 0; //Keine gueltigen Daten
175
                        actual_speed.status             = 0;
176
                        actual_status.status    = 0;
177
                        actual_pos.status               = 0; //neue ublox Messages anfordern
178
                        n                                               = 2;
179
                }
180
        }      
181
        return (n);    
182
}
183
 
184
/*
185
Daten vom GPS im ublox MSG Format auswerten
186
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
187
// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
188
*/
189
void Get_Ublox_Msg(uint8_t rx)
190
{
191
        switch (ublox_msg_state)
192
        {
193
 
194
                case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
195
                        if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
196
                        else ublox_msg_state = UBLOX_IDLE;
197
                        break;
198
 
199
                case UBLOX_SYNC1:
200
 
201
                        if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
202
                        else ublox_msg_state = UBLOX_IDLE;
203
                        chk_a = 0,chk_b = 0;
204
                        break;
205
 
206
                case UBLOX_SYNC2:
207
                        if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;      
208
                        else ublox_msg_state = UBLOX_IDLE;
209
                        break;
210
 
211
                case UBLOX_CLASS: // Nur NAV Meldungen auswerten
212
                        switch (rx)
213
                        {
214
                                case UBLOX_NAV_POSUTM:
215
                                        ptr_pac_status  =       &actual_pos.status;
216
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
217
                                        else
218
                                        {
219
                                                ptr_payload_data                = &actual_pos;
220
                                                ptr_payload_data_end    = &actual_pos.status;
221
                                                ublox_msg_state                 = UBLOX_LEN1;
222
                                        }
223
                                        break;
224
 
225
                                case UBLOX_NAV_STATUS:
226
                                        ptr_pac_status  =       &actual_status.status;
227
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
228
                                        else
229
                                        {
230
                                                ptr_payload_data        = &actual_status;
231
                                                ptr_payload_data_end    = &actual_status.status;
232
                                                ublox_msg_state         = UBLOX_LEN1;
233
                                        }
234
                                        break;
235
 
236
                                case UBLOX_NAV_VELED:
237
                                        ptr_pac_status          =       &actual_speed.status;
238
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
239
                                        else
240
                                        {
241
                                                ptr_payload_data        = &actual_speed;
242
                                                ptr_payload_data_end    = &actual_speed.status;
243
                                                ublox_msg_state         = UBLOX_LEN1;
244
                                        }
245
                                        break;
246
 
247
                                default:
248
                                        ublox_msg_state = UBLOX_IDLE;
249
                                        break; 
250
                        }
251
                        chk_a   = UBLOX_NAV_CLASS + rx;
252
                        chk_b   = UBLOX_NAV_CLASS + chk_a;                     
253
                        break;
254
 
255
                case UBLOX_LEN1: // Laenge auswerten
256
                        rx_len  = rx;
257
                        chk_a   += rx;
258
                        chk_b   += chk_a;              
259
                        ublox_msg_state = UBLOX_LEN2;
260
                        break;
261
 
262
 
263
                case UBLOX_LEN2: // Laenge auswerten
264
                        rx_len = rx_len + (rx *256); // Laenge ermitteln
265
                        chk_a   += rx;
266
                        chk_b   += chk_a;      
267
                        ublox_msg_state = UBLOX_PAYLOAD;
268
                        break;
269
 
270
                case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
271
                        if (rx_len > 0)
272
                        {
273
                                *ptr_payload_data = rx;
274
                                chk_a   += rx;
275
                                chk_b   += chk_a;
276
                                --rx_len;      
277
                                if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))                          
278
                                {
279
                                        ptr_payload_data++;
280
                                        ublox_msg_state = UBLOX_PAYLOAD;
281
                            }
282
                                else ublox_msg_state = UBLOX_CKA;
283
                        }      
284
                        else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
285
                        break;
286
 
287
                case UBLOX_CKA: // Checksum pruefen
288
                        if (rx == chk_a) ublox_msg_state = UBLOX_CKB;                          
289
                        else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
290
                        break;
291
 
292
                case UBLOX_CKB: // Checksum pruefen
293
                        if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
294
                        ublox_msg_state    = UBLOX_IDLE;
295
                        break;
296
 
297
                default:
298
                        ublox_msg_state = UBLOX_IDLE;          
299
                        break;
300
        }
301
}
302
 
303
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
304
short int GPS_CRTL(short int cmd)
305
{
306
        static unsigned int cnt;                                                // Zaehler fuer diverse Verzoegerungen 
307
        static long int         delta_north,delta_east;         // Mass fuer Distanz zur Sollposition
308
        signed int              n;
309
        static signed int       gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
310
        signed int              dist_frm_start_east,dist_frm_start_north;
311
        int                     amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil 
312
        static signed int       int_east,int_north;     //Integrierer 
1039 salvo 313
        long int        speed_east,speed_north; //Aktuelle Geschwindigkeit 
314
        signed long int_east1,int_north1;
1003 salvo 315
        int                     dist_east,dist_north;
316
        int                     diff_p;                 //Vom Modus abhaengige zusaetzliche Verstaerkung
1039 salvo 317
        long            ni,ro;                  // Nick und Roll Zwischenwerte
1003 salvo 318
 
319
 
320
        switch (cmd)
321
        {
322
 
323
                case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. 
324
                        if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
325
                        {
326
                                cnt++;
327
                                if (cnt > 100) // erst nach Verzoegerung 
328
                                {
329
                                        // Erst mal initialisieren
330
                                        cnt             = 0;
331
                                        gps_tick        = 0;                                   
332
                                        hold_fast       = 0;
333
                                        hold_reset_int  = 0; // Integrator enablen
334
                                        int_east        = 0, int_north          = 0;
335
                                        gps_reg_x       = 0, gps_reg_y          = 0;
336
                                        delta_east      = 0, delta_north        = 0;
337
                                        dist_flown      = 0;
338
                                        gps_g2t_act_v   = 0;
339
                                        gps_sub_state   = GPS_CRTL_IDLE;
340
                                        // aktuelle positionsdaten abspeichern
341
                                        if (gps_rel_act_position.status > 0)
342
                                        {
343
                                                gps_rel_start_position.utm_east = gps_rel_act_position.utm_east;
344
                                                gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;
345
                                                gps_rel_start_position.status   = 1; // gueltige Positionsdaten 
346
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
347
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
348
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten 
349
                                                //Richtung zur Home Position bezogen auf Nordpol bestimmen
350
                                                hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);
351
                                                // in Winkel 0...360 Grad umrechnen
352
                                                if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
353
                                                else  hdng_2home = (270 - hdng_2home);
354
                                                dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
355
                                                gps_state       = GPS_CRTL_HOME_ACTIVE;
356
                                                return (GPS_STST_OK);                          
357
                                        }
358
                                        else
359
                                        {
360
                                                gps_rel_start_position.status   =       0;  //Keine Daten verfuegbar
361
                                                gps_state                       =       GPS_CRTL_IDLE;
362
                                                return(GPS_STST_ERR); // Keine Daten da
363
                                        }
364
                                }
365
                                else return(GPS_STST_PEND); // noch warten
366
                        }
367
                   break;
368
// ******************************
369
 
370
                case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
371
                        if (gps_state != GPS_CRTL_HOLD_ACTIVE)
372
                        {
373
                                cnt++;
374
                                if (cnt > 600) // erst nach Verzoegerung 
375
                                {
376
                                        cnt     =       0;
377
                                        // aktuelle positionsdaten abspeichern
378
                                        if (gps_rel_act_position.status > 0)
379
                                        {
380
                                                hold_fast               = 0;
381
                                                hold_reset_int          = 0; // Integrator enablen
382
                                                int_east                = 0, int_north  = 0;
383
                                                gps_reg_x               = 0, gps_reg_y  = 0;
384
                                                delta_east              = 0, delta_north        = 0;
385
                                                speed_east              = 0; speed_north= 0;
386
//                                              int_ovfl_cnt            = 0;
387
                                                gps_quiet_cnt           = 0;
388
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
389
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
390
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten 
391
                                                gps_state                                               = GPS_CRTL_HOLD_ACTIVE;
392
                                                return (GPS_STST_OK);                          
393
                                        }
394
                                        else
395
                                        {
396
                                                gps_rel_hold_position.status    = 0;  //Keine Daten verfuegbar
397
                                                gps_state                                               = GPS_CRTL_IDLE;
398
                                                return(GPS_STST_ERR); // Keine Daten da
399
                                        }
400
                                }
401
                                else return(GPS_STST_PEND); // noch warten
402
                        }
403
                        break;
404
 
405
                case GPS_CMD_STOP: // Lageregelung beenden
406
                        cnt                             =       0;
407
                        GPS_Nick                =       0;
408
                        GPS_Roll                =       0;
409
                        gps_sub_state   =       GPS_CRTL_IDLE;
410
                        gps_state               =       GPS_CRTL_IDLE;
411
                        return (GPS_STST_OK);
412
                        break;
413
 
414
                default:
415
                        return (GPS_STST_ERR);
416
                        break;
417
        }
418
 
419
        switch (gps_state)
420
        {      
421
                case GPS_CRTL_IDLE:
422
                        cnt             =       0;
423
                        return (GPS_STST_OK);
424
                        break;
425
 
426
                case GPS_CRTL_HOME_ACTIVE: // Rueckflug zur Basis
427
                //Der Sollwert des Lagereglers wird der Homeposition angenaehert
428
                        if (gps_rel_start_position.status >0)
429
                        {
430
                                if ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
431
                                {
432
                                        gps_tick++;
433
                                        int d1,d2,d3;
434
                                        d1      = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
435
                                        d2      = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
436
                                        d3      = (dist_2home - dist_flown); // Restdistanz zum Ziel    
437
 
438
                                        if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
439
                                        {
440
                                                if ((d1 < (GPS_G2T_FAST_TOL/2))  && (d2 < (GPS_G2T_FAST_TOL/2))) //voll Stoff weiter wenn Lage gut innerhalb der Toleranz
441
                                                {
442
                                                        if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit erhoehen
443
                                                        dist_flown              +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
444
                                                        gps_sub_state   = GPS_HOME_FAST_IN_TOL;
445
                                                }
446
                                                else if ((d1 < GPS_G2T_FAST_TOL)  && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
447
                                                {
448
                                                        if (gps_g2t_act_v > (GPS_G2T_V_MAX/2)) gps_g2t_act_v -= 1; //Geschwindigkeit auf Haelfte runter oder rauffahren
449
                                                        else if (gps_g2t_act_v < (GPS_G2T_V_MAX/2)) gps_g2t_act_v += 1;
450
                                                        dist_flown              +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
451
                                                        gps_sub_state   = GPS_HOME_FAST_IN_TOL;
452
                                                }
453
                                                else    //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
454
                                                {
455
                                                        if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
456
//                                                      dist_flown++;                           //Auch ausserhalb der Toleranz langsam erhoehen
457
                                                        gps_sub_state   = GPS_HOME_FAST_OUTOF_TOL;
458
                                                }
459
                                                hold_reset_int                  = 0; // Integrator einsschalten  
460
                                                hold_fast                               = 1; // Regler fuer schnellen Flug
461
                                                dist_frm_start_east             = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000);
462
                                                dist_frm_start_north    = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000);
463
                                                gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
464
                                                gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
465
                                        }
466
                                        else if (d3 > GPS_G2T_DIST_HOLD)   //Das Ziel naehert sich, deswegen abbremsen
467
                                        {
468
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL))  
469
                                                {
470
                                                        dist_flown              +=      GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
471
                                                        gps_sub_state   =       GPS_HOME_RMPDWN_IN_TOL;
472
                                                }
473
                                                else
474
                                                {
475
                                                        dist_flown++;   //Auch ausserhalb der Toleranz langsam erhoehen
476
                                                        gps_sub_state   = GPS_HOME_RMPDWN_OUTOF_TOL;
477
                                                }                                      
478
                                                hold_reset_int          = 0; // Integrator einsschalten           
479
                                                hold_fast                       = 1; // Regler fuer schnellen Flug
480
                                                dist_frm_start_east     = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000);
481
                                                dist_frm_start_north    = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000);
482
                                                gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
483
                                                gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
484
                                        }                                                      
485
                                        else  //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) 
486
                                        {
487
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
488
                                                {
489
                                                        gps_sub_state   = GPS_HOME_IN_TOL;
490
                                                        hold_fast               = 0;    // Wieder normal regeln
491
                                                        hold_reset_int  = 0;    // Integrator einsschalten                
492
                                                        if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
493
                                                        else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
494
                                                        if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
495
                                                        else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
496
                                                        if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
497
                                                        {
498
                                                                gps_rel_hold_position.utm_east  = 0;
499
                                                                gps_rel_hold_position.utm_north = 0;
500
                                                                gps_sub_state                   = GPS_HOME_FINISHED;
501
                                                        }
502
                                                }
503
                                                else gps_sub_state      = GPS_HOME_OUTOF_TOL;
504
                                        }                                      
505
                                }
506
                                gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
507
                                return (GPS_STST_OK);                                  
508
                        }
509
                        else  // Keine GPS Daten verfuegbar, deswegen Abbruch
510
                        {
511
                                gps_state       = GPS_CRTL_IDLE;       
512
                                return (GPS_STST_ERR);
513
                        }
514
                        break;
515
 
516
 
517
                case GPS_CRTL_HOLD_ACTIVE:  // Hier werden die Daten fuer Nick und Roll errechnet
518
                        if (gps_updte_flag >0)  // nur wenn neue GPS Daten vorliegen
519
                        {
520
                                gps_quiet_cnt++;
521
                                // ab hier wird geregelt
522
                                delta_east      = (long) (gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east);
523
                                delta_north     = (long) (gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north);
524
                                int_east        += (int)delta_east;
525
                                int_north       += (int)delta_north;
526
                                speed_east      =  actual_speed.speed_e;
527
                                speed_north     =  actual_speed.speed_n;
528
                                gps_updte_flag  = 0;  // Neue Werte koennen vom GPS geholt werden
529
                                dist_east       = (int)delta_east; //merken
530
                                dist_north      = (int)delta_north;
531
 
532
 
533
//                              #define GPSINT_MAX 3000 // Neues Verfahren ab  30.12.2007  bei Integratoroverflow
534
                                long int gpsintmax;
535
                                if (Parameter_NaviGpsI > 0)
536
                                {
537
                                        gpsintmax = (GPS_NICKROLL_MAX * gps_gain * GPS_USR_PAR_FKT * ((32*3)/10))/(long)Parameter_NaviGpsI; //auf ungefeahren Maximalwert begrenzen
538
                                        if ((abs(int_east) > (int)gpsintmax) || (abs(int_north)> (int)gpsintmax))
539
                                        {
540
//                                              //      = 1; // Zahl der Overflows zaehlen
541
//                                              int_ovfl_cnt    -= 1; 
542
                                                int_east        = (int_east * 6)/8; // Wert reduzieren
543
                                                int_north       = (int_north* 6)/8;                                    
544
                                        }
545
 
546
                                        if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten
547
                                        {
548
                                                int_east        = 0;   
549
                                                int_north       = 0;                                   
550
                                        }
551
                                }
552
                                else  // Integrator deaktiviert
553
                                {
554
                                        int_east  = 0;
555
                                        int_north = 0;
556
                                }
557
 
558
                                debug_gp_4      = (int)int_east;  // zum Debuggen
559
                                debug_gp_5      = (int)int_north; // zum Debuggen
560
 
561
                                //I Werte begrenzen 
562
                                #define INT1_MAX (GPS_NICKROLL_MAX * gps_gain*3)/10// auf 30 Prozent des maximalen Nick/Rollwert begrenzen
563
                                int_east1  =  ((((long)int_east)   * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT;
564
                                int_north1 =  ((((long)int_north)  * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT;  
565
                                if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen
566
                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
567
                                if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen
568
                                else if (int_north1 < -INT1_MAX) int_north1 =  -INT1_MAX;
569
 
570
                                if (hold_fast > 0)  //schneller Coming Home Modus 
571
                                {
572
                                        amplfy_speed_east  = DIFF_Y_F_MAX;
573
                                        amplfy_speed_north = DIFF_Y_F_MAX;
574
                                        amplfy_speed_east  *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
575
                                        amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
576
                                        speed_east  = (speed_east   * (long)amplfy_speed_east) /50;
577
                                        speed_north = (speed_north  * (long)amplfy_speed_north)/50;
578
                                        // D Werte begrenzen 
579
                                        #define D_F_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
580
                                        if (speed_east  > D_F_MAX) speed_east = D_F_MAX;
581
                                        else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX;
582
                                        if (speed_north > D_F_MAX) speed_north = D_F_MAX;
583
                                        else if (speed_north < -D_F_MAX) speed_north = -D_F_MAX;
584
 
585
                                        diff_p          = (Parameter_NaviGpsP * GPS_PROP_FAST_V)/GPS_USR_PAR_FKT; //Verstaerkung fuer P-Anteil
586
                                }
587
                                else  //langsamer Holdmodus
588
                                {
589
                                        amplfy_speed_east  = DIFF_Y_N_MAX;
590
                                        amplfy_speed_north = DIFF_Y_N_MAX;
591
                                        amplfy_speed_east  *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
592
                                        amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
593
                                        speed_east  = (speed_east   * (long)amplfy_speed_east) /25;
594
                                        speed_north = (speed_north  * (long)amplfy_speed_north)/25;
595
                                        // D Werte begrenzen 
596
                                        #define D_N_MAX (GPS_NICKROLL_MAX * gps_gain*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen
597
                                        if (speed_east  > D_N_MAX) speed_east = D_N_MAX;
598
                                        else if (speed_east < -D_N_MAX) speed_east = -D_N_MAX;
599
                                        if (speed_north > D_N_MAX) speed_north = D_N_MAX;
600
                                        else if (speed_north < -D_N_MAX) speed_north = -D_N_MAX;
601
 
602
                                        diff_p  = (Parameter_NaviGpsP * GPS_PROP_NRML_V)/GPS_USR_PAR_FKT; //Verstaerkung fuer P-Anteil
603
                                }
604
 
605
//                              debug_gp_4      = (int)speed_east;      // zum Debuggen
606
//                              debug_gp_5      = (int)speed_north; // zum Debuggen
607
 
608
                                //P-Werte verstaerken
609
                                delta_east      = (delta_east   * (long)diff_p)/(40);
610
                                delta_north     = (delta_north  * (long)diff_p)/(40);
611
 
612
                                if (hold_fast > 0)  //schneller Coming Home Modus 
613
                                {
614
                                        // P Werte begrenzen 
615
                                        #define P1_F_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
616
                                        if (delta_east   > P1_F_MAX) delta_east = P1_F_MAX;
617
                                        else if (delta_east < -P1_F_MAX) delta_east = -P1_F_MAX;
618
                                        if (delta_north > P1_F_MAX) delta_north = P1_F_MAX;
619
                                        else if (delta_north < -P1_F_MAX) delta_north = -P1_F_MAX;
620
                                }
621
                                else // Hold modus
622
                                {
623
                                        // P Werte begrenzen 
624
                                        #define P1_N_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
625
                                        if (delta_east   > P1_N_MAX) delta_east = P1_N_MAX;
626
                                        else if (delta_east < -P1_N_MAX) delta_east = -P1_N_MAX;
627
                                        if (delta_north > P1_N_MAX) delta_north = P1_N_MAX;
628
                                        else if (delta_north < -P1_N_MAX) delta_north = -P1_N_MAX;
629
                                }
630
 
631
                                debug_gp_2      = (int)delta_east;              // zum Debuggen
632
                                debug_gp_3      = (int)delta_north;             // zum Debuggen
633
 
634
 
635
                                //PID Regler Werte aufsummieren
636
                                gps_reg_x = -(int_east1  + delta_east  + speed_east);   // I + P +D  Anteil X Achse
637
                                gps_reg_y = -(int_north1 + delta_north + speed_north);  // I + P +D  Anteil Y Achse
638
                                debug_gp_0      = (int)gps_reg_x; // zum Debuggen
639
                                debug_gp_1      = (int)gps_reg_y; // zum Debuggen
640
 
641
                                // Werte fuer Nick und Roll direkt aus gps_reg_x und gps_reg_y bestimmen
642
                                n  = GyroKomp_Int/GIER_GRAD_FAKTOR; //Ausrichtung Kopter 
643
                                ni = -((gps_reg_y * (long)cos_i(n)) + (gps_reg_x * (long)sin_i(n)))/(1000*gps_gain);
644
                                ro =  ((gps_reg_x * (long)cos_i(n)) - (gps_reg_y * (long)sin_i(n)))/(1000*gps_gain);
645
 
646
                                if (ni > (GPS_NICKROLL_MAX )) ni = (GPS_NICKROLL_MAX);
647
                                else if (ni < -(GPS_NICKROLL_MAX )) ni = -(GPS_NICKROLL_MAX );
648
                                if (ro > (GPS_NICKROLL_MAX )) ro = (GPS_NICKROLL_MAX );
649
                                else if (ro < -(GPS_NICKROLL_MAX)) ro = -(GPS_NICKROLL_MAX );
650
 
651
                                if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX))  // bei zu grossem Abstand abbrechen
652
                                {
653
                                        GPS_Roll        = 0;
654
                                        GPS_Nick        = 0;
655
                                        gps_state       = GPS_CRTL_IDLE;
656
                                        return (GPS_STST_ERR); 
657
                                        break;                                 
658
                                }
659
                                else if ((PPM_in[7] > 100) && (CAM_GPS_QUIET > 0) && (gps_quiet_cnt <=4) )  // Wenn Fotoausloeser gedruckt wird, GPS Stellwerte kurzzeitig auf 0 setzen
660
                                {
661
                                        gps_quiet_cnt++;
662
                                        GPS_Roll = 0;
663
                                        GPS_Nick = 0;
664
                                        if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
665
                                        return (GPS_STST_OK);
666
                                }
667
                                else if ((PPM_in[7] < 50) && (CAM_GPS_QUIET > 0)  && (gps_quiet_cnt >= 4))
668
                                {
669
                                        gps_quiet_cnt = 0;
670
                                        if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
671
                                        return (GPS_STST_OK);
672
                                }
673
                                else
674
                                {
675
                                        GPS_Roll = (int)ro;
676
                                        GPS_Nick = (int)ni;
677
                                        if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
678
                                        return (GPS_STST_OK);
679
                                }
680
                        }
681
                        else
682
                        {
683
                                if ( cmd == GPS_CMD_REQ_HOME ) gps_state = GPS_CRTL_HOME_ACTIVE; // State umsetzen
684
                                return (GPS_STST_OK);
685
                        }
686
                        break;
687
 
688
                default:
689
                        gps_state = GPS_CRTL_IDLE;
690
                        return (GPS_STST_ERR);
691
                        break;
692
        }      
693
        return (GPS_STST_ERR);
694
 
695
}
696