WebSVN - FlightCtrl - Diff - Rev 613 and 615 - /branches/salvo_gps/Basis

 static                          uint8_t *ptr_pac_status;
 long int                        dist_flown;
 unsigned int            int_ovfl_cnt; // Zaehler fuer Overflows des Integrators
 signed int                      int_east,int_north;     //Integrierer
 signed int                      diff_east_f,diff_north_f; // Differenzierer,  gefiltert
-signed int                      diff_v;
+//signed int                    diff_v;
 signed long             dist;
 short int Get_GPS_data(void);
         static signed int diff_east,diff_north;         // Differenzierer  (Differenz zum  vorhergehenden x bzw. y  Wert)
         signed int n;
         static signed int gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
         long signed int dev,n_l;
         signed int dist_frm_start_east,dist_frm_start_north;
+        int diff_v_east,diff_v_north; //Verstaerkungsfaktoren fuer Differenzierer
         switch (cmd)
+        {
                                 diff_east       += dist_east;   // Differenz zur vorhergehenden East Position
                                 diff_north      += dist_north;  // Differenz zur vorhergehenden North Position
                                 if (hold_fast > 0) // wegen Sollpositionsspruengen im Fast Mode Differenzierer daempfen
                                 {
-                                        diff_east_f             = ((diff_east_f  *2)/3) + (diff_east *1/3); //Differenzierer filtern
+                                        diff_east_f             = (((diff_east_f  *2)/3) + ((diff_east *1*10)/3)); //Differenzierer filtern
-                                        diff_north_f    = ((diff_north_f *2)/3) + (diff_north*1/3); //Differenzierer filtern
+                                        diff_north_f    = (((diff_north_f *2)/3) + ((diff_north*1*10)/3)); //Differenzierer filtern
+                                }
                                 else // schwache Filterung
                                 {
-                                        diff_east_f             = ((diff_east_f  * 2)/4) + ((diff_east *2)/4); //Differenzierer filtern
+                                        diff_east_f             = (((diff_east_f  * 2)/4) + ((diff_east *2*10)/4)); //Differenzierer filtern
-                                        diff_north_f    = ((diff_north_f * 2)/4) + ((diff_north*2)/4); //Differenzierer filtern
+                                        diff_north_f    = (((diff_north_f * 2)/4) + ((diff_north*2*10)/4)); //Differenzierer filtern
+                                }
                                 #define GPSINT_MAX 3000 // Neues Verfahren ab  30.12.2007  bei Integratoroverflow
+                                {
                                         int_east        = 0;
                                         int_north       = 0;
+                                }
                                 // Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
                                 // desto groesser wird der Faktor. Es gibt aber einen Maximalwert. Bei 0 ist die Verstaerkung immer 1
                                 signed long int_east1,int_north1;
                                 if (hold_fast == 0)  // je Nach Modus andere Verstaerkungskurve fuer Differenzierer
+                                {
 //                                      diff_v = (int)((dist * (GPS_DIFF_NRML_MAX_V - 10)) / GPS_DIFF_NRML_MAX_D) +10; //Verstaerkung * 10
 //                                      if (diff_v > GPS_DIFF_NRML_MAX_V) diff_v = GPS_DIFF_NRML_MAX_V; //begrenzen
-                                        diff_v = GPS_DIFF_NRML_MAX_V ; //variable Versterkung raus 31.12.2007
+//                                      diff_v = GPS_DIFF_NRML_MAX_V ; //variable Versterkung raus 31.12.2007
+                                }
                                 else
+                                {
 //                                      diff_v = (int)((dist * (GPS_DIFF_FAST_MAX_V - 10)) / GPS_DIFF_FAST_MAX_D) +10; //Verstaerkung * 10
 //                                      if (diff_v > GPS_DIFF_FAST_MAX_V) diff_v = GPS_DIFF_FAST_MAX_V; //begrenzen
-                                        diff_v = GPS_DIFF_FAST_MAX_V ; //variable Versterkung raus 31.12.2007
+//                                      diff_v = GPS_DIFF_FAST_MAX_V ; //variable Versterkung raus 31.12.2007
+                                }
+                                diff_v_east  = (((abs(diff_east_f)) *(DIFF_Y_MAX-1))/DIFF_X_MAX) +10;
+                                diff_v_north = (((abs(diff_north_f))*(DIFF_Y_MAX-1))/DIFF_X_MAX) +10;
+                                if (diff_v_east  > (DIFF_Y_MAX *10)) diff_v_east  = DIFF_Y_MAX *10; // Begrenzung
+                                if (diff_v_north > (DIFF_Y_MAX *10)) diff_v_north = DIFF_Y_MAX *10; // Begrenzung
+                                diff_v_east             *=      2;
+                                diff_v_north    *=      2;
+                                debug_gp_2      = diff_v_east;  // zum Debuggen
+                                debug_gp_3      = diff_v_north; // zum Debuggen
+                                debug_gp_4      = diff_east_f;  // zum Debuggen
                                 debug_gp_5      = diff_north_f; // zum Debuggen
                                 else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
                                 if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen
                                 else if (int_north1 < -INT1_MAX) int_north1 =  -INT1_MAX;
                                 //PID Regler Werte aufsummieren
-                                gps_reg_x = ((int)int_east1  + ((dist_east  * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((diff_east_f  * diff_v * (Parameter_UserParam3/GPS_USR_PAR_FKT))/10));  // I + P +D  Anteil X Achse
+                                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
+                                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
+                                debug_gp_0      = gps_reg_x;  // zum Debuggen
-                                gps_reg_y = ((int)int_north1 + ((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((diff_north_f * diff_v * (Parameter_UserParam3/GPS_USR_PAR_FKT))/10));  // I + P +D  Anteil Y Achse
+                                debug_gp_1      = gps_reg_y; // zum Debuggen
                                 //Ziel-Richtung bezogen auf Nordpol bestimmen

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/GPS.c - Rev 613 → 615