Subversion Repositories FlightCtrl

Rev

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

Rev 622 Rev 626
Line 14... Line 14...
14 
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 14 
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15 
Peter Muehlenbrock
 15 
Peter Muehlenbrock
16 
Auswertung der Daten vom GPS im ublox Format
 16 
Auswertung der Daten vom GPS im ublox Format
17 
Hold Modus mit PID Regler
 17 
Hold Modus mit PID Regler
18 
Rückstuerz zur Basis Funktion
 18 
Rückstuerz zur Basis Funktion
19 
Stand 24.10.2007
 19 
Stand 7.1.2008
20 
Anederung: 24.10. Altitude in relativer Position jetzt auch drin
 - 
 
- 
 
 20 
 
21 
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 21 
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
22 
*/
 22 
*/
23 
#include "main.h"
 23 
#include "main.h"
24 
#include "math.h"
 24 
#include "math.h"
25 
//#include "gps.h"
 25 
//#include "gps.h"
Line 296... Line 296...
296 
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
 296 
//Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
297 
short int GPS_CRTL(short int cmd)
 297 
short int GPS_CRTL(short int cmd)
298 
{
 298 
{
299 
        static unsigned int cnt;                                                //Zaehler fuer diverse Verzoegerungen
299 
        static unsigned int cnt;                                                //Zaehler fuer diverse Verzoegerungen
300 
        static signed int       dist_north,dist_east;           // Distanz zur Sollpoistion
 300 
        static signed int       dist_north,dist_east;           // Distanz zur Sollpoistion
301 
        static signed int       diff_east,diff_north;           // Differenzierer  (Differenz zum  vorhergehenden x bzw. y  Wert)
 - 
 
302 
        static signed int       diff_east_f,diff_north_f;       // Differenzierer, gefiltert
 - 
 
303 
        signed int                      n,n1;
 301 
        signed int                      n,n1;
304 
        static signed int       gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
 302 
        static signed int       gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
305 
        long signed int         dev;
 303 
        long signed int         dev;
306 
        signed int                      dist_frm_start_east,dist_frm_start_north;
 304 
        signed int                      dist_frm_start_east,dist_frm_start_north;
307 
        int                             diff_v_east,diff_v_north; //Verstaerkungsfaktoren fuer Differenzierer
 305 
        int                             amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil
308 
        static signed int       int_east,int_north;     //Integrierer
306 
        static signed int       int_east,int_north;     //Integrierer
309 
        int                             speed_east,speed_north; //Aktuelle Geschwindigkeit
307 
        int                             speed_east,speed_north; //Aktuelle Geschwindigkeit
310 
        signed long             int_east1,int_north1;
 308 
        signed long             int_east1,int_north1;
311 
//      signed long             dist;
 309 
//      signed long             dist;
Line 325... Line 323...
325 
                                        hold_fast               = 0;
 323 
                                        hold_fast               = 0;
326 
                                        hold_reset_int  = 0; // Integrator enablen
 324 
                                        hold_reset_int  = 0; // Integrator enablen
327 
                                        int_east                = 0, int_north  = 0;
 325 
                                        int_east                = 0, int_north  = 0;
328 
                                        gps_reg_x               = 0, gps_reg_y  = 0;
 326 
                                        gps_reg_x               = 0, gps_reg_y  = 0;
329 
                                        dist_east               = 0, dist_north = 0;
 327 
                                        dist_east               = 0, dist_north = 0;
330 
                                        diff_east_f             = 0, diff_north_f= 0;
 - 
 
331 
                                        diff_east               = 0, diff_north  = 0;  
 - 
 
332 
                                        dist_flown              = 0;
 328 
                                        dist_flown              = 0;
333 
                                        gps_g2t_act_v   = 0;
 329 
                                        gps_g2t_act_v   = 0;
334 
                                        gps_sub_state   = GPS_CRTL_IDLE;
 330 
                                        gps_sub_state   = GPS_CRTL_IDLE;
335 
                                        // aktuelle positionsdaten abspeichern
 331 
                                        // aktuelle positionsdaten abspeichern
336 
                                        if (gps_rel_act_position.status > 0)
 332 
                                        if (gps_rel_act_position.status > 0)
Line 375... Line 371...
375 
                                                hold_fast               = 0;
 371 
                                                hold_fast               = 0;
376 
                                                hold_reset_int  = 0; // Integrator enablen
 372 
                                                hold_reset_int  = 0; // Integrator enablen
377 
                                                int_east        = 0, int_north  = 0;
 373 
                                                int_east        = 0, int_north  = 0;
378 
                                                gps_reg_x       = 0, gps_reg_y  = 0;
 374 
                                                gps_reg_x       = 0, gps_reg_y  = 0;
379 
                                                dist_east       = 0, dist_north = 0;
 375 
                                                dist_east       = 0, dist_north = 0;
380 
                                                diff_east_f     = 0, diff_north_f= 0;
 - 
 
381 
                                                diff_east       = 0, diff_north  = 0;
 - 
 
382 
                                                speed_east      = 0; speed_north= 0;
 376 
                                                speed_east      = 0; speed_north= 0;
383 
                                                int_ovfl_cnt = 0;
 377 
                                                int_ovfl_cnt = 0;
384 
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
 378 
                                                gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
385 
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
 379 
                                                gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
386 
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten
380 
                                                gps_rel_hold_position.status    = 1; // gueltige Positionsdaten
Line 507... Line 501...
507 
 
 501 
 
508 
                case GPS_CRTL_HOLD_ACTIVE:  // Hier werden die Daten fuer Nick und Roll errechnet
 502 
                case GPS_CRTL_HOLD_ACTIVE:  // Hier werden die Daten fuer Nick und Roll errechnet
509 
                        if (gps_updte_flag >0)  // nur wenn neue GPS Daten vorliegen
 503 
                        if (gps_updte_flag >0)  // nur wenn neue GPS Daten vorliegen
510 
                        {
 504 
                        {
511 
                                // ab hier wird geregelt
 - 
 
512 
                                diff_east       = dist_east;      //Alten Wert fuer Differenzierer schon mal addieren
 - 
 
513 
                                diff_north      = dist_north;
 505 
                                // ab hier wird geregelt
514 
                                dist_east       = gps_rel_hold_position.utm_east  - gps_rel_act_position.utm_east;
 506 
                                dist_east       = gps_rel_hold_position.utm_east  - gps_rel_act_position.utm_east;
515 
                                dist_north      = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
 507 
                                dist_north      = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
516 
                                int_east        += dist_east;
 508 
                                int_east        += dist_east;
517 
                                int_north   += dist_north;
 - 
 
518 
                                diff_east       -= dist_east;   // Differenz zur vorhergehenden East Position
 - 
 
519 
                                diff_north      -= dist_north;  // Differenz zur vorhergehenden North Position
 509 
                                int_north   += dist_north;
520 
                                speed_east      =  (int) actual_speed.speed_e;
 510 
                                speed_east      =  (int) (-actual_speed.speed_e);
521 
                                speed_north     =  (int) actual_speed.speed_n;
 511 
                                speed_north     =  (int) (-actual_speed.speed_n);
Line 522... Line -...
522 
                                gps_updte_flag = 0;  // Neue Werte koennen vom GPS geholt werden
 - 
 
523 
 
 - 
 
524 
                                debug_gp_2      = speed_east;  // zum Debuggen
 - 
 
525 
                                debug_gp_3      = speed_north; // zum Debuggen
 - 
 
526 
                               
 - 
 
527 
                                if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen
 - 
 
528 
                                {
 - 
 
529 
                                        diff_east_f             = (((diff_east_f  *2)/3) + ((diff_east *1*10)/3)); //Differenzierer filtern    
- 
 
530 
                                        diff_north_f    = (((diff_north_f *2)/3) + ((diff_north*1*10)/3)); //Differenzierer filtern
 - 
 
531 
                                }      
 - 
 
532 
                                else // schwache Filterung
 - 
 
533 
                                {
 - 
 
534 
                                        diff_east_f             = (((diff_east_f  * 2)/4) + ((diff_east *2*10)/4)); //Differenzierer filtern   
- 
 
535 
                                        diff_north_f    = (((diff_north_f * 2)/4) + ((diff_north*2*10)/4)); //Differenzierer filtern
 - 
 
536 
                                }
 - 
 
537 
 
 - 
 
538 
                                // Differenz aus Distanz durch eche geschwindigkeit aus VELNED Msg ersetzten
 - 
 
539 
                                diff_east_f     = -speed_east;
 - 
 
540 
                                diff_north_f = speed_north;
 512 
                                gps_updte_flag = 0;  // Neue Werte koennen vom GPS geholt werden
541 
 
 513 
 
542 
                                #define GPSINT_MAX 3000 // Neues Verfahren ab  30.12.2007  bei Integratoroverflow
 514 
                                #define GPSINT_MAX 3000 // Neues Verfahren ab  30.12.2007  bei Integratoroverflow
543 
                                if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX))
 515 
                                if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX))
544 
                                {
 516 
                                {
Line 557... Line 529...
557 
                                {
 529 
                                {
558 
                                        int_east        = 0;   
 530 
                                        int_east        = 0;   
559 
                                        int_north       = 0;                                   
 531 
                                        int_north       = 0;                                   
560 
                                }
 532 
                                }
Line -... Line 533...
- 
 
 533 
 
- 
 
 534 
                                // Verstaerkungsfaktor fuer Geschwindigkeit ermitteln um exponentielles Verhalten zu erzielen
- 
 
 535 
                                amplfy_speed_east  = (((abs(speed_east)) *(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10;
- 
 
 536 
                                amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10;
- 
 
 537 
                                if (amplfy_speed_east  > (DIFF_Y_MAX *10)) amplfy_speed_east  = DIFF_Y_MAX *10; // Begrenzung
- 
 
 538 
                                if (amplfy_speed_north > (DIFF_Y_MAX *10)) amplfy_speed_north = DIFF_Y_MAX *10; // Begrenzung
- 
 
 539 
                                amplfy_speed_east  *= (Parameter_UserParam3/GPS_USR_PAR_FKT);
- 
 
 540 
                                amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT);
- 
 
 541 
                                amplfy_speed_east   /= 10; //Faktor 10 wieder rausnehmen
561 
 
 542 
                                amplfy_speed_north  /= 10; //Faktor 10 wieder rausnehmen
- 
 
 543 
                                #define GPS_SPEED_SCALE 5
- 
 
 544 
                                speed_east  /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren
- 
 
 545 
                                speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren
562 
//                              int phi;
 546 
                                debug_gp_2      = speed_east;  // zum Debuggen
- 
 
 547 
                                debug_gp_3      = speed_north; // zum Debuggen
563 
//                              phi  = arctan_i(abs(dist_north),abs(dist_east));
548 
                                debug_gp_4      = amplfy_speed_east;  // zum Debuggen
Line 564... Line -...
564 
//                              dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
 - 
 
565 
 
 - 
 
566 
                                if (hold_fast == 0)  // je Nach Modus andere Verstaerkungskurve fuer Differenzierer
 - 
 
567 
                                {
 - 
 
568 
//                                      diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10
 - 
 
569 
//                                      if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen
 - 
 
570 
//                                      diff_v = GPS_DIFF_NRML_MAX_V ; //variable Versterkung raus 31.12.2007
 - 
 
571 
                                }
 - 
 
572 
                                else
 - 
 
573 
                                {
 - 
 
574 
//                                      diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10
 - 
 
575 
//                                      if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen
 - 
 
576 
//                                      diff_v = GPS_DIFF_FAST_MAX_V ; //variable Versterkung raus 31.12.2007
 - 
 
577 
                                }
 - 
 
578 
                                diff_v_east  = (((abs(diff_east_f)) *(DIFF_Y_MAX-1))/DIFF_X_MAX) +10;
 - 
 
579 
                                diff_v_north = (((abs(diff_north_f))*(DIFF_Y_MAX-1))/DIFF_X_MAX) +10;
 - 
 
580 
                                if (diff_v_east  > (DIFF_Y_MAX *10)) diff_v_east  = DIFF_Y_MAX *10; // Begrenzung
 - 
 
581 
                                if (diff_v_north > (DIFF_Y_MAX *10)) diff_v_north = DIFF_Y_MAX *10; // Begrenzung
 - 
 
582 
//                              diff_v_east             *=      2;
 - 
 
583 
//                              diff_v_north    *=      2;
 - 
 
584 
 
 - 
 
585 
//                              debug_gp_2      = diff_v_east;  // zum Debuggen
 - 
 
586 
//                              debug_gp_3      = diff_v_north; // zum Debuggen
 - 
 
Line 587... Line 549...
587 
//                              debug_gp_4      = diff_east_f;  // zum Debuggen
 549 
                                debug_gp_5      = amplfy_speed_north; // zum Debuggen
588 
//                              debug_gp_5      = diff_north_f; // zum Debuggen
 550 
 
589 
 
 551 
 
Line 599... Line 561...
599 
                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
 561 
                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
600 
                                if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen
 562 
                                if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen
601 
                                else if (int_north1 < -INT1_MAX) int_north1 =  -INT1_MAX;
 563 
                                else if (int_north1 < -INT1_MAX) int_north1 =  -INT1_MAX;
Line 602... Line 564...
602 
 
 564 
 
603 
                                //PID Regler Werte aufsummieren
 565 
                                //PID Regler Werte aufsummieren
604 
                                gps_reg_x = ((int)int_east1  + ((dist_east  * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((diff_east_f  * diff_v_east * (Parameter_UserParam3/GPS_USR_PAR_FKT))/100));  // I + P +D  Anteil X Achse
 566 
                                gps_reg_x = ((int)int_east1  + ((dist_east  * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_east  * amplfy_speed_east  )/(10/GPS_SPEED_SCALE)));  // I + P +D  Anteil X Achse
605 
                                gps_reg_y = ((int)int_north1 + ((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((diff_north_f * diff_v_north * (Parameter_UserParam3/GPS_USR_PAR_FKT))/100));  // I + P +D  Anteil Y Achse
 567 
                                gps_reg_y = ((int)int_north1 + ((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_north * amplfy_speed_north )/(10/GPS_SPEED_SCALE)));  // I + P +D  Anteil Y Achse
606 
                                debug_gp_0      = gps_reg_x;  // zum Debuggen
 568 
                                debug_gp_0      = gps_reg_x;  // zum Debuggen
Line 607... Line 569...
607 
                                debug_gp_1      = gps_reg_y; // zum Debuggen
 569 
                                debug_gp_1      = gps_reg_y; // zum Debuggen
608 
 
 570