Subversion Repositories FlightCtrl

Rev

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

Rev 258 Rev 266
Line 78... Line 78...
78
// Initialisierung
78
// Initialisierung
79
void GPS_Neutral(void)
79
void GPS_Neutral(void)
80
{
80
{
81
        ublox_msg_state                         =       UBLOX_IDLE;
81
        ublox_msg_state                         =       UBLOX_IDLE;
82
        gps_state                                       =       GPS_CRTL_IDLE;
82
        gps_state                                       =       GPS_CRTL_IDLE;
-
 
83
        gps_sub_state                           =       GPS_CRTL_IDLE;
83
        actual_pos.status                       =       0;
84
        actual_pos.status                       =       0;
84
        actual_speed.status                     =       0;
85
        actual_speed.status                     =       0;
85
        actual_status.status            =       0;
86
        actual_status.status            =       0;
86
        gps_home_position.status        =       0; // Noch keine gueltige Home Position
87
        gps_home_position.status        =       0; // Noch keine gueltige Home Position
87
        gps_act_position.status         =       0;
88
        gps_act_position.status         =       0;
Line 330... Line 331...
330
                                                //Richtung zur Home Positionbezogen auf Nordpol bestimmen
331
                                                //Richtung zur Home Positionbezogen auf Nordpol bestimmen
331
                                                hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);
332
                                                hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);
332
                                                // in Winkel 0...360 Grad umrechnen
333
                                                // in Winkel 0...360 Grad umrechnen
333
                                                if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
334
                                                if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
334
                                                else  hdng_2home = (270 - hdng_2home);
335
                                                else  hdng_2home = (270 - hdng_2home);
335
                                                dist_2home = get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
336
                                                dist_2home = (int)get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
336
                                                gps_state       = GPS_CRTL_HOME_ACTIVE;
337
                                                gps_state       = GPS_CRTL_HOME_ACTIVE;
337
                                                return (GPS_STST_OK);                          
338
                                                return (GPS_STST_OK);                          
338
                                        }
339
                                        }
339
                                        else
340
                                        else
340
                                        {
341
                                        {
Line 386... Line 387...
386
                        cnt                             =       0;
387
                        cnt                             =       0;
387
                        GPS_Nick                =       0;
388
                        GPS_Nick                =       0;
388
                        GPS_Roll                =       0;
389
                        GPS_Roll                =       0;
389
                        gps_int_x               =       0;
390
                        gps_int_x               =       0;
390
                        gps_int_y               =       0;
391
                        gps_int_y               =       0;
-
 
392
                        gps_sub_state   =       GPS_CRTL_IDLE;
391
                        gps_state       =       GPS_CRTL_IDLE;
393
                        gps_state               =       GPS_CRTL_IDLE;
392
                        return (GPS_STST_OK);
394
                        return (GPS_STST_OK);
393
                        break;
395
                        break;
Line 394... Line 396...
394
 
396
 
395
                default:
397
                default:
Line 416... Line 418...
416
                                        d2      = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
418
                                        d2      = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
417
                                        d3      = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel       
419
                                        d3      = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel       
Line 418... Line 420...
418
       
420
       
419
                                        if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
421
                                        if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
420
                                        {
-
 
421
//                                              hold_fast               = 1; // Schnell Regeln
-
 
422
                                                hold_reset_int  = 1; // Integrator ausschalten            
422
                                        {
423
                                                if ((d1 < GPS_G2T_FAST_TOL)  && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
423
                                                if ((d1 < GPS_G2T_FAST_TOL)  && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
424
                                                {
424
                                                {
425
                                                        if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
425
                                                        if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++;    //Geschwindigkeit langsam erhoehen
426
                                                        dist_flown                                              +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
-
 
427
                                                        dist_frm_start_east                             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
-
 
428
                                                        dist_frm_start_north                    = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
-
 
429
                                                        gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
-
 
430
                                                        gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
426
                                                        dist_flown                                              +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
431
                                                        gps_sub_state                                   = GPS_HOME_FAST_IN_TOL;
427
                                                        gps_sub_state                                   = GPS_HOME_FAST_IN_TOL;
432
                                                }
428
                                                }
433
                                                else    //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
429
                                                else    //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
434
                                                {
430
                                                {
-
 
431
                                                        if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
435
                                                        if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
432
                                                        dist_flown++;                                                   //Auch ausserhalb der Toleranz langsam erhoehen
436
                                                        gps_sub_state   = GPS_HOME_FAST_OUTOF_TOL;
433
                                                        gps_sub_state   = GPS_HOME_FAST_OUTOF_TOL;
-
 
434
                                                }
-
 
435
                                                hold_fast                                               = 1; // Regler fuer schnellen Flug
-
 
436
                                                hold_reset_int                                  = 1; // Integrator ausschalten            
-
 
437
                                                dist_frm_start_east                             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
-
 
438
                                                dist_frm_start_north                    = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
-
 
439
                                                gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
437
                                                }
440
                                                gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
438
                                        }
441
                                        }
439
                                        else if (d3 > GPS_G2T_DIST_HOLD)   //Das Ziel naehert sich, deswegen abbremsen
442
                                        else if (d3 > GPS_G2T_DIST_HOLD)   //Das Ziel naehert sich, deswegen abbremsen
440
                                        {
-
 
441
                                                hold_reset_int  = 0; // Integrator einschalten            
443
                                        {
442
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
444
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL))  
443
                                                {
-
 
444
                                                        if (gps_g2t_act_v > GPS_G2T_V_RAMP_DWN) gps_g2t_act_v = GPS_G2T_V_RAMP_DWN; // Geschwindigkeit zu hoch
-
 
445
                                                        else  gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
445
                                                {
446
                                                        dist_flown                                              +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
-
 
447
                                                        dist_frm_start_east                             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
-
 
448
                                                        dist_frm_start_north                    = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
-
 
449
                                                        gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
-
 
450
                                                        gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
446
                                                        dist_flown              +=      GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
451
                                                        gps_sub_state   = GPS_HOME_RMPDWN_IN_TOL;
447
                                                        gps_sub_state   =       GPS_HOME_RMPDWN_IN_TOL;
452
                                                }
448
                                                }
453
                                                else
449
                                                else
454
                                                {
450
                                                {
455
                                                        if (gps_g2t_act_v > 1) gps_g2t_act_v--;
451
                                                        dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
456
                                                        gps_sub_state   = GPS_HOME_RMPDWN_OUTOF_TOL;
452
                                                        gps_sub_state   = GPS_HOME_RMPDWN_OUTOF_TOL;
-
 
453
                                                }                                      
-
 
454
                                                hold_fast                                               = 0; // Wieder normal regeln
-
 
455
                                                hold_reset_int                                  = 1; // Integrator ausschalten            
-
 
456
                                                dist_frm_start_east                             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
-
 
457
                                                dist_frm_start_north                    = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
-
 
458
                                                gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
457
                                                }                                      
459
                                                gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
458
                                        }                                                      
460
                                        }                                                      
459
                                        else  //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) 
461
                                        else  //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt) 
460
                                        {
462
                                        {
461
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
463
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
Line 472... Line 474...
472
                                                                gps_rel_hold_position.utm_east  = 0;
474
                                                                gps_rel_hold_position.utm_east  = 0;
473
                                                                gps_rel_hold_position.utm_north = 0;
475
                                                                gps_rel_hold_position.utm_north = 0;
474
                                                                gps_sub_state                                   = GPS_HOME_FINISHED;
476
                                                                gps_sub_state                                   = GPS_HOME_FINISHED;
475
                                                        }
477
                                                        }
476
                                                }
478
                                                }
-
 
479
                                                else gps_sub_state      = GPS_HOME_OUTOF_TOL;
477
                                        }                                      
480
                                        }                                      
478
                                }
481
                                }
479
                                gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
482
                                gps_state = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
480
                                return (GPS_STST_OK);                                  
483
                                return (GPS_STST_OK);                                  
481
                        }
484
                        }
Line 518... Line 521...
518
 
521
 
519
                                // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
522
                                // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
520
                                // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
523
                                // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
521
                                signed long dist,int_east1,int_north1;
524
                                signed long dist,int_east1,int_north1;
522
                                int phi;
525
                                int phi;
523
                                phi = arctan_i(abs(dist_north),abs(dist_east));
526
                                phi  = arctan_i(abs(dist_north),abs(dist_east));
Line 524... Line 527...
524
                                dist = (long) (get_dist(dist_east,dist_north,phi)); //Zunaechst Entfernung zum Ziel ermitteln
527
                                dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
525
 
528
 
526
                                if (hold_fast == 0)  // je Nach Modus andere Verstaerkungskurve fuer Differenzierer
529
                                if (hold_fast == 0)  // je Nach Modus andere Verstaerkungskurve fuer Differenzierer
527
                                {
530
                                {
Line 532... Line 535...
532
                                {
535
                                {
533
                                        diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10
536
                                        diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10
534
                                        if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen
537
                                        if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen
535
                                }
538
                                }
Line -... Line 539...
-
 
539
 
-
 
540
                                int diff_p;  //Vom Modus abhaengige zusaetzliche Verstaerkung
-
 
541
                                if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
-
 
542
                                else diff_p = GPS_PROP_NRML_V;
536
 
543
 
537
                                //I Werte begrenzen
544
                                //I Werte begrenzen
538
                                #define INT1_MAX (20 * GPS_V)
545
                                #define INT1_MAX (20 * GPS_V)
539
                                int_east1 =  ((((long)int_east)   * Parameter_UserParam2)/32);
546
                                int_east1 =  ((((long)int_east)   * Parameter_UserParam2)/32);
540
                                int_north =  ((((long)int_north)  * Parameter_UserParam2)/32);
547
                                int_north =  ((((long)int_north)  * Parameter_UserParam2)/32);
541
                                if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen
548
                                if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen
542
                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
549
                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
543
                                if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen
550
                                if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen
Line 544... Line -...
544
                                else if (int_north1 < -INT1_MAX) int_north1 =  -INT1_MAX;
-
 
545
 
-
 
546
                                int diff_p;
-
 
547
                                if (hold_fast == 0) diff_p = 1;  
-
 
548
                                else diff_p = 2; //im schnellen Modus Proportionalanteil staerken
551
                                else if (int_north1 < -INT1_MAX) int_north1 =  -INT1_MAX;
549
 
552
 
550
                                //PID Regler Werte aufsummieren
553
                                //PID Regler Werte aufsummieren
Line 551... Line 554...
551
                                gps_reg_x = ((int)int_east1  + ((dist_east  * Parameter_UserParam1 * diff_p)/8)+ ((diff_east  * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil X Achse
554
                                gps_reg_x = ((int)int_east1  + ((dist_east  * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east  * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil X Achse
552
                                gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil Y Achse
555
                                gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_north * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil Y Achse
Line 553... Line 556...
553
 
556