Subversion Repositories FlightCtrl

Rev

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