Subversion Repositories FlightCtrl

Rev

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

Rev 628 Rev 629
Line 10... Line 10...
10
If not, see <http://www.gnu.org/licenses/>.
10
If not, see <http://www.gnu.org/licenses/>.
Line 11... Line 11...
11
 
11
 
12
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
12
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
13
*/
13
*/
14
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15
Peter Muehlenbrock
15
von Peter Muehlenbrock alias Salvo
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
Line 19... Line 19...
19
Stand 8.1.2008
19
Stand 9.1.2008
20
 
20
 
21
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
22
*/
22
*/
Line 75... Line 75...
75
 
75
 
76
GPS_ABS_POSITION_t              gps_act_position;               // Alle wichtigen Daten zusammengefasst
76
GPS_ABS_POSITION_t              gps_act_position;               // Alle wichtigen Daten zusammengefasst
77
GPS_ABS_POSITION_t              gps_home_position;      // Die Startposition, beim Kalibrieren ermittelt
77
GPS_ABS_POSITION_t              gps_home_position;      // Die Startposition, beim Kalibrieren ermittelt
78
GPS_REL_POSITION_t              gps_rel_act_position;   // Die aktuelle relative Position bezogen auf Home Position
78
GPS_REL_POSITION_t              gps_rel_act_position;   // Die aktuelle relative Position bezogen auf Home Position
79
GPS_REL_POSITION_t              gps_rel_hold_position;  // Die gespeicherte Sollposition fuer GPS_ Hold Mode
79
GPS_REL_POSITION_t              gps_rel_hold_position;  // Die gespeicherte Sollposition fuer GPS_ Hold Mode
Line 80... Line 80...
80
static GPS_REL_POSITION_t               gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
80
GPS_REL_POSITION_t              gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
81
 
81
 
82
// Initialisierung
82
// Initialisierung
83
void GPS_Neutral(void)
83
void GPS_Neutral(void)
Line 313... Line 313...
313
 
313
 
314
                case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. 
314
                case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden. 
315
                        if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
315
                        if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
316
                        {
316
                        {
317
                                cnt++;
317
                                cnt++;
318
                                if (cnt > 200) // erst nach Verzoegerung 
318
                                if (cnt > 50) // erst nach Verzoegerung 
319
                                {
319
                                {
320
                                        // Erst mal initialisieren
320
                                        // Erst mal initialisieren
321
                                        cnt                             = 0;
321
                                        cnt             = 0;
322
                                        gps_tick                = 0;                                   
322
                                        gps_tick                = 0;                                   
Line 433... Line 433...
433
       
433
       
434
                                        if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
434
                                        if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
435
                                        {
435
                                        {
436
                                                if ((d1 < GPS_G2T_FAST_TOL)  && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
436
                                                if ((d1 < GPS_G2T_FAST_TOL)  && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
437
                                                {
437
                                                {
438
                                                        if (gps_g2t_act_v < GPS_G2T_V_MAX-2) gps_g2t_act_v +=2;    //Geschwindigkeit langsam erhoehen
438
                                                        if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit langsam erhoehen
439
                                                        dist_flown              +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
439
                                                        dist_flown              +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
440
                                                        gps_sub_state   = GPS_HOME_FAST_IN_TOL;
440
                                                        gps_sub_state   = GPS_HOME_FAST_IN_TOL;
441
                                                }
441
                                                }
442
                                                else    //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
442
                                                else    //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
443
                                                {
443
                                                {
444
                                                        if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
444
                                                        if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
445
                                                        dist_flown++;                                                   //Auch ausserhalb der Toleranz langsam erhoehen
445
//                                                      dist_flown++;                           //Auch ausserhalb der Toleranz langsam erhoehen
446
                                                        gps_sub_state   = GPS_HOME_FAST_OUTOF_TOL;
446
                                                        gps_sub_state   = GPS_HOME_FAST_OUTOF_TOL;
447
                                                }
447
                                                }
448
                                                hold_reset_int                                  = 0; // Integrator einsschalten  
448
                                                hold_reset_int                  = 0; // Integrator einsschalten  
449
                                                hold_fast                                               = 1; // Regler fuer schnellen Flug
449
                                                hold_fast                               = 1; // Regler fuer schnellen Flug
Line 462... Line 462...
462
                                                else
462
                                                else
463
                                                {
463
                                                {
464
                                                        dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
464
                                                        dist_flown++;   //Auch ausserhalb der Toleranz langsam erhoehen
465
                                                        gps_sub_state   = GPS_HOME_RMPDWN_OUTOF_TOL;
465
                                                        gps_sub_state   = GPS_HOME_RMPDWN_OUTOF_TOL;
466
                                                }                                      
466
                                                }                                      
467
                                                hold_reset_int                                  = 0; // Integrator ausschalten            
467
                                                hold_reset_int          = 0; // Integrator einsschalten           
468
                                                hold_fast                                               = 1; // Wieder normal regeln
468
                                                hold_fast                       = 1; // Regler fuer schnellen Flug
469
                                                dist_frm_start_east                             = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000);
469
                                                dist_frm_start_east     = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000);
470
                                                dist_frm_start_north                    = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000);
470
                                                dist_frm_start_north    = (int)(((long)dist_flown * (long)cos_i(hdng_2home))/1000);
471
                                                gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
471
                                                gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east  + dist_frm_start_east; //naechster Zielpunkt
472
                                                gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
472
                                                gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
473
                                        }                                                      
473
                                        }                                                      
Line 475... Line 475...
475
                                        {
475
                                        {
476
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
476
                                                if ((d1 < GPS_G2T_NRML_TOL)  && (d2 < GPS_G2T_NRML_TOL)) // Jetzt bis zum Zielpunkt regeln
477
                                                {
477
                                                {
478
                                                        gps_sub_state   = GPS_HOME_IN_TOL;
478
                                                        gps_sub_state   = GPS_HOME_IN_TOL;
479
                                                        hold_fast               = 0; // Wieder normal regeln
479
                                                        hold_fast               = 0;    // Wieder normal regeln
480
                                                        hold_reset_int  = 0; // Integrator wieder aktivieren              
480
                                                        hold_reset_int  = 0;    // Integrator einsschalten                
481
                                                        if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
481
                                                        if (gps_rel_hold_position.utm_east >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_east -= GPS_G2T_V_MIN;
482
                                                        else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
482
                                                        else if (gps_rel_hold_position.utm_east <= -GPS_G2T_V_MIN ) gps_rel_hold_position.utm_east += GPS_G2T_V_MIN;
483
                                                        if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
483
                                                        if (gps_rel_hold_position.utm_north >= GPS_G2T_V_MIN) gps_rel_hold_position.utm_north -= GPS_G2T_V_MIN;
484
                                                        else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
484
                                                        else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
485
                                                        if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
485
                                                        if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
Line 530... Line 530...
530
                                if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten
530
                                if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten
531
                                {
531
                                {
532
                                        int_east        = 0;   
532
                                        int_east        = 0;   
533
                                        int_north       = 0;                                   
533
                                        int_north       = 0;                                   
534
                                }
534
                                }
535
                                if (hold_fast > 0)  //schneller Coming Home Modus
535
                                if (hold_fast > 0)  //schneller Coming Home Modus Einfluss des D-Anteils verkleinern
536
                                {
536
                                {
537
                                        amplfy_speed_east  = (Parameter_UserParam3/GPS_USR_PAR_FKT);
537
                                        amplfy_speed_east  = (Parameter_UserParam3/GPS_USR_PAR_FKT);
538
                                        amplfy_speed_north = (Parameter_UserParam3/GPS_USR_PAR_FKT);
538
                                        amplfy_speed_north = (Parameter_UserParam3/GPS_USR_PAR_FKT);
539
                                }
539
                                }
540
                                else  //langsamer Holdmodus
540
                                else  //langsamer Holdmodus
Line 553... Line 553...
553
 
553
 
554
                                #define GPS_SPEED_SCALE 5
554
                                #define GPS_SPEED_SCALE 5
555
                                speed_east  /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren
555
                                speed_east  /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren
Line 556... Line -...
556
                                speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren
-
 
557
 
556
                                speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren
558
 
557
 
559
                                int diff_p;  //Vom Modus abhaengige zusaetzliche Verstaerkung
558
                                int diff_p;  //Vom Modus abhaengige zusaetzliche Verstaerkung
Line 560... Line 559...
560
                                if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
559
                                if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
Line 615... Line 614...
615
/*                              n                       = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));
614
/*                              n               = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));
616
                                n_l                     = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
615
                                n_l             = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
617
                                GPS_Roll        = (int) n_l;
616
                                GPS_Roll        = (int) n_l;
618
                                n                       = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V));
617
                                n               = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V));
619
                                n_l                     = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
618
                                n_l             = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
620
                                GPS_Nick        = (int) n_l;*/                           
619
                                GPS_Nick        = (int) n_l;
-
 
620
*/                               
Line 621... Line 621...
621
 
621
 
622
                                if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX))  // bei zu grossem Abstand abbrechen
622
                                if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX))  // bei zu grossem Abstand abbrechen
623
                                {
623
                                {
624
                                        GPS_Roll        = 0;
624
                                        GPS_Roll        = 0;