Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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