Subversion Repositories FlightCtrl

Rev

Rev 184 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 184 Rev 185
Line 417... Line 417...
417
                                if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
417
                                if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);
418
                                else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
418
                                else if (GPS_Nick < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = -(GPS_NICKROLL_MAX * GPS_V);
Line 419... Line 419...
419
 
419
 
420
                                //Kleine Werte verstaerken, Grosse abschwaechen
420
                                //Kleine Werte verstaerken, Grosse abschwaechen
-
 
421
                                long int nick,roll;
421
                                long int nick,roll;
422
                                int n,r;
-
 
423
                                r                       = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));
-
 
424
                                n                       = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V));
-
 
425
                                roll            = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) r)/1000;
422
                                roll            = (((long) GPS_Roll) * ((long)sin_i(abs((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V)))))/1000;
426
                                nick            = ((long) (GPS_NICKROLL_MAX * GPS_V) * (long) n)/1000;
423
                                GPS_Roll        = (int) (roll / GPS_V);
-
 
424
                                nick            = (((long) GPS_Nick) * ((long)sin_i(abs((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V)))))/1000;
427
                                GPS_Roll        = (int) (roll / GPS_V);
Line 425... Line 428...
425
                                GPS_Nick        = (int) (nick / GPS_V);
428
                                GPS_Nick        = (int) (nick / GPS_V);
426
                                 
429
                                 
427
                                if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX))  // bei zu grossem Abstand abbrechen
430
                                if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX))  // bei zu grossem Abstand abbrechen
428
                                {
431
                                {
429
                                        GPS_Roll        = 0;
432
                                        GPS_Roll        = 0;
430
                                        GPS_Nick        = 0;
433
                                        GPS_Nick        = 0;
431
                                        gps_state       = GPS_CRTL_IDLE;
434
                                        gps_state       = GPS_CRTL_IDLE;
432
                                        return (GPS_STST_ERR);                                         
435
                                        return (GPS_STST_ERR);                                         
433
                                }
436
                                }
434
                                else // Distanz ok // Die Abfrage kann noch rausfliegen, weil vorher bereits begrenzung war
437
/*                              else // Distanz ok // kann spaeter entfallen, weil eigentlich schon begrenzt
435
                                {
438
                                {
436
                                        if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen
439
                                        if (GPS_Roll > GPS_NICKROLL_MAX) GPS_Roll = GPS_NICKROLL_MAX; //Auf Maxwerte begrenzen
437
                                        else if (GPS_Roll <  -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX;
440
                                        else if (GPS_Roll <  -GPS_NICKROLL_MAX) GPS_Roll = - GPS_NICKROLL_MAX;
438
                                        if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX;
441
                                        if (GPS_Nick > GPS_NICKROLL_MAX) GPS_Nick = GPS_NICKROLL_MAX;
439
                                        else if (GPS_Nick <  - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX;
442
                                        else if (GPS_Nick <  - GPS_NICKROLL_MAX) GPS_Nick = - GPS_NICKROLL_MAX;
440
                                        return (GPS_STST_OK);
443
                                return (GPS_STST_OK);
441
                                }                      
444
                                }*/                    
442
                        }
445
                        }
Line 443... Line 446...
443
                        else return (GPS_STST_OK);
446
                        else return (GPS_STST_OK);