Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
143 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 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 for more details. You should have received a copy of the GNU General Public License
8
along with this program. If not, see <http://www.gnu.org/licenses/>.
9
 
149 salvo 10
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
143 salvo 11
*/
12
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
Peter Muehlenbrock
14
Auswertung der Daten vom GPS im ublox Format
182 salvo 15
Hold Modus mit PID Regler
197 salvo 16
Stand 2.10.2007
143 salvo 17
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18
*/
1 ingob 19
#include "main.h"
156 salvo 20
//#include "gps.h"
1 ingob 21
 
143 salvo 22
// Defines fuer ublox Messageformat um Auswertung zu steuern
183 salvo 23
#define                 UBLOX_IDLE              0
24
#define                 UBLOX_SYNC1             1
25
#define                 UBLOX_SYNC2             2
26
#define                 UBLOX_CLASS             3
27
#define                 UBLOX_ID                4
28
#define                 UBLOX_LEN1              5
29
#define                 UBLOX_LEN2              6
30
#define                 UBLOX_CKA               7
31
#define                 UBLOX_CKB               8
143 salvo 32
#define                 UBLOX_PAYLOAD   9
33
 
34
// ublox Protokoll Identifier 
35
#define                 UBLOX_NAV_POSUTM                0x08
36
#define                 UBLOX_NAV_STATUS                0x03
37
#define                 UBLOX_NAV_VELED                 0x12
38
#define                 UBLOX_NAV_CLASS                 0x01
39
#define                 UBLOX_SYNCH1_CHAR               0xB5
40
#define                 UBLOX_SYNCH2_CHAR               0x62
41
 
209 salvo 42
signed int      GPS_Nick = 0;
43
signed int      GPS_Roll = 0;
44
short int       ublox_msg_state = UBLOX_IDLE;
45
static          uint8_t chk_a =0; //Checksum
46
static          uint8_t chk_b =0;
47
short int       gps_state;
48
short int       gps_updte_flag;
49
signed int      GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
50
signed int      GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
51
signed int      GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel 
52
signed int      gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;                               
143 salvo 53
static unsigned int rx_len;
54
static unsigned int ptr_payload_data_end;
209 salvo 55
unsigned int            gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
143 salvo 56
 
57
static uint8_t *ptr_payload_data;
58
static uint8_t *ptr_pac_status;
59
 
156 salvo 60
short int Get_GPS_data(void);
143 salvo 61
 
62
NAV_POSUTM_t actual_pos;    // Aktuelle Nav Daten werden hier im ublox Format abgelegt
63
NAV_STATUS_t actual_status; // Aktueller Nav Status
64
NAV_VELNED_t actual_speed;  // Aktueller Geschwindigkeits und Richtungsdaten
65
 
156 salvo 66
GPS_ABS_POSITION_t              gps_act_position;               // Alle wichtigen Daten zusammengefasst
67
GPS_ABS_POSITION_t              gps_home_position;      // Die Startposition, beim Kalibrieren ermittelt
68
GPS_REL_POSITION_t              gps_rel_act_position;   // Die aktuelle relative Position bezogen auf Home Position
158 salvo 69
GPS_REL_POSITION_t              gps_rel_hold_position;  // Die gespeicherte Sollposition fuer GPS_ Hold Mode
143 salvo 70
 
156 salvo 71
// Initialisierung
72
void GPS_Neutral(void)
1 ingob 73
{
158 salvo 74
        ublox_msg_state                         =       UBLOX_IDLE;
75
        gps_state                                       =       GPS_CRTL_IDLE;
76
        actual_pos.status                       =       0;
77
        actual_speed.status                     =       0;
78
        actual_status.status            =       0;
79
        gps_home_position.status        =       0; // Noch keine gueltige Home Position
80
        gps_act_position.status         =       0;
81
        gps_rel_act_position.status     =       0;     
82
        GPS_Nick                                        =       0;
83
        GPS_Roll                                        =       0;
84
        gps_updte_flag                          =       0;
159 salvo 85
        gps_int_x                                       =       0;
86
        gps_int_y                                       =       0;
190 salvo 87
        gps_alive_cnt                           =       0;
1 ingob 88
}
89
 
156 salvo 90
// Home Position sichern falls Daten verfuegbar sind. 
91
void GPS_Save_Home(void)
1 ingob 92
{
156 salvo 93
        short int n;
94
        n = Get_GPS_data();
95
        if (n == 0)   // Gueltige  und aktuelle Daten ?   
96
        {
97
                // Neue GPS Daten liegen vor
157 salvo 98
                gps_home_position.utm_east      = gps_act_position.utm_east;   
99
                gps_home_position.utm_north     = gps_act_position.utm_north;  
100
                gps_home_position.utm_alt       = gps_act_position.utm_alt;
101
                gps_home_position.status        = 1; // Home Position gueltig                   
156 salvo 102
        }
103
}
104
 
105
// Relative Position zur Home Position bestimmen
106
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
107
short int Get_Rel_Position(void)
108
{
109
        short int n = 0;
110
        n = Get_GPS_data();
111
        if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
190 salvo 112
        if (gps_alive_cnt < 400) gps_alive_cnt += 300; // Timeoutzaehler. Wird in MotorreglerRoutine ueberwacht und dekrementiert
156 salvo 113
        if  (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
114
        {
115
                gps_rel_act_position.utm_east   = (int)  (gps_act_position.utm_east - gps_home_position.utm_east);
116
                gps_rel_act_position.utm_north  = (int)  (gps_act_position.utm_north - gps_home_position.utm_north);
117
                gps_rel_act_position.status     = 1; // gueltige Positionsdaten
118
                n = 0;
158 salvo 119
                gps_updte_flag = 1; // zeigt an, dass neue Daten vorliegen.
156 salvo 120
        }
121
        else
122
        {
123
                n = 2; //keine gueltigen Daten vorhanden
124
                gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
125
        }      
126
        return (n);
127
}
128
 
129
// Daten aus aktuellen ublox Messages extrahieren 
130
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
131
short int Get_GPS_data(void)
132
{
133
        short int n = 1;
134
 
135
        if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
143 salvo 136
        if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
137
        {
149 salvo 138
                if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
143 salvo 139
                {
140
                        gps_act_position.utm_east       = actual_pos.utm_east/10;
141
                        gps_act_position.utm_north      = actual_pos.utm_north/10;
142
                        gps_act_position.utm_alt        = actual_pos.utm_alt/10;
143
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
144
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
145
                        gps_act_position.heading        = actual_speed.heading/100000;
146
                        gps_act_position.status         = 1;
156 salvo 147
                        n                                                       = 0; //Daten gueltig
143 salvo 148
                }
156 salvo 149
                else
150
                {
151
                        gps_act_position.status = 0; //Keine gueltigen Daten
152
                        n                                               = 2;
153
                }
154
                actual_pos.status               = 0; //neue ublox Messages anfordern
155
                actual_status.status    = 0;
156
                actual_speed.status     = 0;
157
        }      
158
        return (n);    
1 ingob 159
}
160
 
161
 
143 salvo 162
/*
163
Daten vom GPS im ublox MSG Format auswerten
156 salvo 164
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
143 salvo 165
// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
166
*/
167
void Get_Ublox_Msg(uint8_t rx)
168
{
169
        switch (ublox_msg_state)
170
        {
1 ingob 171
 
143 salvo 172
                case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
173
                        if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
174
                        else ublox_msg_state = UBLOX_IDLE;
175
                        break;
176
 
177
                case UBLOX_SYNC1:
178
 
179
                        if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
180
                        else ublox_msg_state = UBLOX_IDLE;
181
                        chk_a = 0,chk_b = 0;
182
                        break;
183
 
184
                case UBLOX_SYNC2:
185
                        if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;      
186
                        else ublox_msg_state = UBLOX_IDLE;
187
                        break;
188
 
189
                case UBLOX_CLASS: // Nur NAV Meldungen auswerten
190
                        switch (rx)
191
                        {
192
                                case UBLOX_NAV_POSUTM:
193
                                        ptr_pac_status  =       &actual_pos.status;
194
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
195
                                        else
196
                                        {
197
                                                ptr_payload_data                = &actual_pos;
198
                                                ptr_payload_data_end    = &actual_pos.status;
199
                                                ublox_msg_state                 = UBLOX_LEN1;
200
                                        }
201
                                        break;
202
 
203
                                case UBLOX_NAV_STATUS:
204
                                        ptr_pac_status  =       &actual_status.status;
205
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
206
                                        else
207
                                        {
208
                                                ptr_payload_data                = &actual_status;
209
                                                ptr_payload_data_end    = &actual_status.status;
210
                                                ublox_msg_state                 = UBLOX_LEN1;
211
                                        }
212
                                        break;
213
 
214
                                case UBLOX_NAV_VELED:
156 salvo 215
                                        ptr_pac_status          =       &actual_speed.status;
143 salvo 216
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
217
                                        else
218
                                        {
219
                                                ptr_payload_data                = &actual_speed;
220
                                                ptr_payload_data_end    = &actual_speed.status;
221
                                                ublox_msg_state                 = UBLOX_LEN1;
222
                                        }
223
                                        break;
224
 
225
                                default:
226
                                        ublox_msg_state = UBLOX_IDLE;
227
                                        break; 
228
                        }
229
                        chk_a   = UBLOX_NAV_CLASS + rx;
230
                        chk_b   = UBLOX_NAV_CLASS + chk_a;                     
231
                        break;
232
 
233
                case UBLOX_LEN1: // Laenge auswerten
234
                        rx_len  = rx;
235
                        chk_a   += rx;
236
                        chk_b   += chk_a;              
237
                        ublox_msg_state = UBLOX_LEN2;
238
                        break;
239
 
240
 
241
                case UBLOX_LEN2: // Laenge auswerten
242
                        rx_len = rx_len + (rx *256); // Laenge ermitteln
243
                        chk_a   += rx;
244
                        chk_b   += chk_a;      
245
                        ublox_msg_state = UBLOX_PAYLOAD;
246
                        break;
247
 
248
                case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
249
                        if (rx_len > 0)
250
                        {
251
                                *ptr_payload_data = rx;
252
                                chk_a   += rx;
253
                                chk_b   += chk_a;
254
                                --rx_len;      
255
                                if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))                          
256
                                {
257
                                        ptr_payload_data++;
156 salvo 258
                                        ublox_msg_state = UBLOX_PAYLOAD;
143 salvo 259
                            }
260
                                else ublox_msg_state = UBLOX_CKA;
261
                        }      
262
                        else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
263
                        break;
264
 
265
                case UBLOX_CKA: // Checksum pruefen
266
                        if (rx == chk_a) ublox_msg_state = UBLOX_CKB;                          
267
                        else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
268
                        break;
269
 
270
                case UBLOX_CKB: // Checksum pruefen
271
                        if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
272
                        ublox_msg_state    = UBLOX_IDLE;
273
                        break;
274
 
275
                default:
276
                        ublox_msg_state = UBLOX_IDLE;          
277
                        break;
158 salvo 278
        }
143 salvo 279
}
158 salvo 280
 
281
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
282
short int GPS_CRTL(short int cmd)
283
{
184 salvo 284
        static unsigned int cnt;                                //Zaehler fuer diverse Verzoegerungen 
285
        static signed int int_east,int_north;   //Integrierer 
286
        static signed int dist_north,dist_east;
186 salvo 287
        static signed int diff_east,diff_north;         // Differenzierer  (Differenz zum  vorhergehenden x bzw. y  Wert)
288
        static signed int diff_east_f,diff_north_f; // Differenzierer,  gefiltert
197 salvo 289
        signed int n,diff_v;
290
        long signed int dev,n_l;
158 salvo 291
        switch (cmd)
292
        {
162 salvo 293
 
294
                case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. 
295
                // Noch einiges zu ueberlegen und zu tun
296
                   return(GPS_STST_PEND); // noch warten
297
                   break;
298
// ******************************
299
 
158 salvo 300
                case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
301
                        if (gps_state != GPS_CRTL_HOLD_ACTIVE)
302
                        {
303
                                cnt++;
160 salvo 304
                                if (cnt > 500) // erst nach Verzoegerung 
158 salvo 305
                                {
306
                                        cnt     =       0;
307
                                        // aktuelle positionsdaten abespeichern
308
                                        if (gps_rel_act_position.status > 0)
309
                                        {
186 salvo 310
                                                int_east                = 0;
311
                                                int_north               = 0;
312
                                                gps_reg_x               = 0;
313
                                                gps_reg_y               = 0;
314
                                                dist_east               = 0;
315
                                                dist_north              = 0;
316
                                                diff_east_f             = 0;
317
                                                diff_north_f    = 0;
190 salvo 318
                                                diff_east               = 0;
186 salvo 319
                                                diff_north              = 0;
158 salvo 320
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
321
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
322
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten 
323
                                                gps_state                                               = GPS_CRTL_HOLD_ACTIVE;
324
                                                return (GPS_STST_OK);                          
325
                                        }
326
                                        else
327
                                        {
328
                                                gps_rel_hold_position.status    =       0;  //Keine Daten verfuegbar
329
                                                gps_state                                               = GPS_CRTL_IDLE;
330
                                                return(GPS_STST_ERR); // Keine Daten da
331
                                        }
332
                                }
333
                                else return(GPS_STST_PEND); // noch warten
334
                        }
335
                        break;
336
 
162 salvo 337
                case GPS_CMD_STOP: // Lageregelung beenden
190 salvo 338
                        cnt                             =       0;
339
                        GPS_Nick                =       0;
340
                        GPS_Roll                =       0;
341
                        gps_int_x               =       0;
342
                        gps_int_y               =       0;
162 salvo 343
                        gps_state       =       GPS_CRTL_IDLE;
158 salvo 344
                        return (GPS_STST_OK);
345
                        break;
346
 
347
                default:
348
                        return (GPS_STST_ERR);
349
                        break;
350
        }
351
 
352
        switch (gps_state)
353
        {      
354
                case GPS_CRTL_IDLE:
355
                        cnt             =       0;
356
                        return (GPS_STST_OK);
357
                        break;
358
 
186 salvo 359
                case GPS_CRTL_HOLD_ACTIVE:    // Hier werden die Daten fuer Nick und Roll errechnet
159 salvo 360
                        if (gps_updte_flag == 1)  //nur wenn neue GPS Daten vorliegen
158 salvo 361
                        {
362
                                gps_updte_flag = 0;
363
                                // ab hier wird geregelt
186 salvo 364
                                diff_east       = -dist_east;     //Alten Wert fuer Differenzier schon mal abziehen
365
                                diff_north      = -dist_north;
190 salvo 366
                                dist_east       = gps_rel_hold_position.utm_east  - gps_rel_act_position.utm_east;
184 salvo 367
                                dist_north      = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
162 salvo 368
                                int_east        += dist_east;
369
                                int_north   += dist_north;
167 salvo 370
                                diff_east       += dist_east;   // Differenz zur vorhergehenden East Position
371
                                diff_north      += dist_north;  // Differenz zur vorhergehenden North Position
161 salvo 372
 
194 salvo 373
                                diff_east_f             = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern        
374
                                diff_north_f    = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern       
186 salvo 375
 
194 salvo 376
                                #define GPSINT_MAX 2048 //neuer Wert ab 1.10.2007 Begrenzung 
162 salvo 377
                                if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
378
                                {
379
                                        int_east        -= dist_east;
380
                                        int_north   -= dist_north;                                     
381
                                }
209 salvo 382
                                // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
383
                                // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
197 salvo 384
                                #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
209 salvo 385
                                #define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
197 salvo 386
                                signed long dist;
387
                                int phi;
209 salvo 388
                                phi = arctan_i(abs(dist_north),abs(dist_east));
197 salvo 389
                                if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln
390
                                {
391
                                        dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen
392
                                        dist = abs((dist *1000) / (long) sin_i(phi));
393
                                }
394
                                else
395
                                {
396
                                        dist = (long)dist_north;
397
                                        dist = abs((dist *1000) / (long) cos_i(phi));
398
                                }
399
                                diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
209 salvo 400
                                if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
161 salvo 401
 
186 salvo 402
                                //PID Regler Werte aufsummieren
197 salvo 403
                                gps_reg_x = ((int_east  * Parameter_UserParam2)/32) + ((dist_east  * Parameter_UserParam1)/8)+ ((diff_east_f  * diff_v * Parameter_UserParam3)/10);  // I + P +D  Anteil X Achse
404
                                gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north_f * diff_v * Parameter_UserParam3)/10);  // I + P +D  Anteil Y Achse
159 salvo 405
 
186 salvo 406
                                //Ziel-Richtung bezogen auf Nordpol bestimmen
194 salvo 407
                                GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
159 salvo 408
 
186 salvo 409
                                // in Winkel 0...360 Grad umrechnen
161 salvo 410
                                if ((gps_reg_x >= 0)) GPS_hdng_abs_2trgt = ( 90-GPS_hdng_abs_2trgt);
162 salvo 411
                                else  GPS_hdng_abs_2trgt = (270 - GPS_hdng_abs_2trgt);
161 salvo 412
 
159 salvo 413
                                // Relative Richtung in bezug auf Nordachse des Kopters errechen
414
                                n= GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
415
                                GPS_hdng_rel_2trgt      =       GPS_hdng_abs_2trgt - n;
167 salvo 416
                                if      ((GPS_hdng_rel_2trgt >180) && (GPS_hdng_abs_2trgt >=180)) GPS_hdng_rel_2trgt = GPS_hdng_rel_2trgt-360;
159 salvo 417
                                else if (GPS_hdng_rel_2trgt >180)  GPS_hdng_rel_2trgt = 360 - GPS_hdng_rel_2trgt;
418
                                else if (GPS_hdng_rel_2trgt <-180)  GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
419
 
184 salvo 420
                                // Regelabweichung aus x,y zu Ziel in Distanz umrechnen 
160 salvo 421
                                if (abs(gps_reg_x) > abs(gps_reg_y) )
422
                                {
197 salvo 423
                                        dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
424
                                        dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
160 salvo 425
                                }
426
                                else
427
                                {
197 salvo 428
                                        dev = (long)gps_reg_y;
429
                                        dev = abs((dev *1000) / (long) cos_i(GPS_hdng_abs_2trgt));
160 salvo 430
                                }
197 salvo 431
                                GPS_dist_2trgt  = (int) dev;
185 salvo 432
                                // Winkel und Distanz in Nick und Rollgroessen umrechnen
197 salvo 433
                                GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
434
                                GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
190 salvo 435
 
194 salvo 436
                                #define GPS_V 8 // auf Maximalwert normieren. Teilerfaktor ist 8
184 salvo 437
                                if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
438
                                else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
439
                                if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
440
                                else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
441
 
442
                                //Kleine Werte verstaerken, Grosse abschwaechen
186 salvo 443
                                n                       = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));
444
                                n_l                     = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
445
                                GPS_Roll        = (int) n_l;
185 salvo 446
                                n                       = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V));
186 salvo 447
                                n_l                     = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
448
                                GPS_Nick        = (int) n_l;
184 salvo 449
 
450
                                if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX))  // bei zu grossem Abstand abbrechen
451
                                {
452
                                        GPS_Roll        = 0;
453
                                        GPS_Nick        = 0;
454
                                        gps_state       = GPS_CRTL_IDLE;
186 salvo 455
                                        return (GPS_STST_ERR); 
456
                                        break;                                 
184 salvo 457
                                }
158 salvo 458
                        }
459
                        else return (GPS_STST_OK);
460
                        break;
461
 
462
                default:
463
                        gps_state       = GPS_CRTL_IDLE;
464
                        return (GPS_STST_ERR);
465
                        break;
466
        }      
467
        return (GPS_STST_ERR);
468
 
469
}
470