Subversion Repositories FlightCtrl

Rev

Rev 565 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 565 Rev 602
Line 62... Line 62...
62
static signed           gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt 
62
static signed           gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt 
63
static                          short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
63
static                          short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
64
static                          uint8_t *ptr_payload_data;
64
static                          uint8_t *ptr_payload_data;
65
static                          uint8_t *ptr_pac_status;
65
static                          uint8_t *ptr_pac_status;
66
long int                        dist_flown;
66
long int                        dist_flown;
-
 
67
unsigned int            int_ovfl_cnt; // Zaehler fuer Overflows des Integrators
-
 
68
signed int                      int_east,int_north;     //Integrierer 
-
 
69
 
Line 67... Line 70...
67
 
70
 
Line 68... Line 71...
68
short int Get_GPS_data(void);
71
short int Get_GPS_data(void);
69
 
72
 
Line 293... Line 296...
293
       
296
       
294
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
297
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
295
short int GPS_CRTL(short int cmd)
298
short int GPS_CRTL(short int cmd)
296
{
299
{
297
        static unsigned int cnt;                                        //Zaehler fuer diverse Verzoegerungen 
-
 
298
        static signed int int_east,int_north;   //Integrierer 
300
        static unsigned int cnt;                                        //Zaehler fuer diverse Verzoegerungen 
299
        static signed int dist_north,dist_east;
301
        static signed int dist_north,dist_east;
300
        static signed int diff_east,diff_north;         // Differenzierer  (Differenz zum  vorhergehenden x bzw. y  Wert)
302
        static signed int diff_east,diff_north;         // Differenzierer  (Differenz zum  vorhergehenden x bzw. y  Wert)
301
        static signed int diff_east_f,diff_north_f; // Differenzierer,  gefiltert
303
        static signed int diff_east_f,diff_north_f; // Differenzierer,  gefiltert
302
        signed int n,diff_v;
304
        signed int n,diff_v;
303
        static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
305
        static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
304
        long signed int dev,n_l;
306
        long signed int dev,n_l;
-
 
307
        signed int dist_frm_start_east,dist_frm_start_north;
305
        signed int dist_frm_start_east,dist_frm_start_north;
308
 
306
        switch (cmd)
309
        switch (cmd)
Line 307... Line 310...
307
        {
310
        {
308
 
311
 
Line 370... Line 373...
370
                                                int_east        = 0, int_north  = 0;
373
                                                int_east        = 0, int_north  = 0;
371
                                                gps_reg_x       = 0, gps_reg_y  = 0;
374
                                                gps_reg_x       = 0, gps_reg_y  = 0;
372
                                                dist_east       = 0, dist_north = 0;
375
                                                dist_east       = 0, dist_north = 0;
373
                                                diff_east_f     = 0, diff_north_f= 0;
376
                                                diff_east_f     = 0, diff_north_f= 0;
374
                                                diff_east       = 0, diff_north  = 0;
377
                                                diff_east       = 0, diff_north  = 0;
-
 
378
                                                int_ovfl_cnt = 0;
375
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
379
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
376
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
380
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
377
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten 
381
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten 
378
                                                gps_state                                               = GPS_CRTL_HOLD_ACTIVE;
382
                                                gps_state                                               = GPS_CRTL_HOLD_ACTIVE;
379
                                                return (GPS_STST_OK);                          
383
                                                return (GPS_STST_OK);                          
Line 499... Line 503...
499
                case GPS_CRTL_HOLD_ACTIVE:  // Hier werden die Daten fuer Nick und Roll errechnet
503
                case GPS_CRTL_HOLD_ACTIVE:  // Hier werden die Daten fuer Nick und Roll errechnet
500
                        if (gps_updte_flag >0)  // nur wenn neue GPS Daten vorliegen
504
                        if (gps_updte_flag >0)  // nur wenn neue GPS Daten vorliegen
501
                        {
505
                        {
502
                                gps_updte_flag = 0;
506
                                gps_updte_flag = 0;
503
                                // ab hier wird geregelt
507
                                // ab hier wird geregelt
504
                                diff_east       = -dist_east;     //Alten Wert fuer Differenzier schon mal abziehen
508
                                diff_east       = -dist_east;     //Alten Wert fuer Differenzierer schon mal abziehen
505
                                diff_north      = -dist_north;
509
                                diff_north      = -dist_north;
506
                                dist_east       = gps_rel_hold_position.utm_east  - gps_rel_act_position.utm_east;
510
                                dist_east       = gps_rel_hold_position.utm_east  - gps_rel_act_position.utm_east;
507
                                dist_north      = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
511
                                dist_north      = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
508
                                int_east        += dist_east;
512
                                int_east        += dist_east;
509
                                int_north   += dist_north;
513
                                int_north   += dist_north;
Line 519... Line 523...
519
                                {
523
                                {
520
                                        diff_east_f             = ((diff_east_f  * 1)/4) + ((diff_east *3)/4); //Differenzierer filtern 
524
                                        diff_east_f             = ((diff_east_f  * 1)/4) + ((diff_east *3)/4); //Differenzierer filtern 
521
                                        diff_north_f    = ((diff_north_f * 1)/4) + ((diff_north*3)/4); //Differenzierer filtern
525
                                        diff_north_f    = ((diff_north_f * 1)/4) + ((diff_north*3)/4); //Differenzierer filtern
522
                                }
526
                                }
Line 523... Line 527...
523
 
527
 
524
                                #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung 
528
                                #define GPSINT_MAX 3000 // Neues Verfahren ab  30.12.2007  bei Integratoroverflow
-
 
529
                                if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX))
-
 
530
                                {
-
 
531
                                        if (int_ovfl_cnt < 40) int_ovfl_cnt += 20; // Zahl der Overflows zaehlen
-
 
532
//                                      int_east        -= dist_east; auf alten Wert halten
-
 
533
//                                      int_north   -= dist_north;                                      
-
 
534
                                }
525
                                if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
535
                                if (int_ovfl_cnt > 0) //bei Overflow Wert Inetgratorwert reduzieren 
526
                                {
536
                                {
-
 
537
                                        int_ovfl_cnt    -= 1;
527
                                        int_east        -= dist_east;
538
                                        int_east                = (int_east*7)/8; // Wert reduzieren
528
                                        int_north   -= dist_north;                                     
539
                                        int_north       = (int_north*7)/8;                                     
-
 
540
                                }
529
                                }
541
 
530
                                if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten
542
                                if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten
531
                                {
543
                                {
532
                                        int_east        = 0;   
544
                                        int_east        = 0;   
533
                                        int_north       = 0;                                   
545
                                        int_north       = 0;                                   
Line -... Line 546...
-
 
546
                                }
534
                                }
547
 
535
 
548
 
536
                                // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
549
                                // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
537
                                // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
550
                                // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
538
                                signed long dist,int_east1,int_north1;
551
                                signed long dist,int_east1,int_north1;
Line 554... Line 567...
554
                                int diff_p;  //Vom Modus abhaengige zusaetzliche Verstaerkung
567
                                int diff_p;  //Vom Modus abhaengige zusaetzliche Verstaerkung
555
                                if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
568
                                if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
556
                                else diff_p = GPS_PROP_NRML_V;
569
                                else diff_p = GPS_PROP_NRML_V;
Line 557... Line 570...
557
 
570
 
558
                                //I Werte begrenzen
571
                                //I Werte begrenzen
559
                                #define INT1_MAX (20 * GPS_V)
572
                                #define INT1_MAX (GPS_NICKROLL_MAX * GPS_V)/2 //ab 30.12.2007 auf Halben Maximalen Nick/Rollwert begrenzen
560
                                int_east1  =  ((((long)int_east)   * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;
573
                                int_east1  =  ((((long)int_east)   * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;
561
                                int_north1 =  ((((long)int_north)  * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;  //Fehler behoben am 17.12.2007 vorher int_north= 
574
                                int_north1 =  ((((long)int_north)  * Parameter_UserParam2)/32)/GPS_USR_PAR_FKT;  //Fehler behoben am 17.12.2007 vorher int_north= 
562
                                if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen
575
                                if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen
563
                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
576
                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;