WebSVN - FlightCtrl - Diff - Rev 266 and 291

 /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Peter Muehlenbrock
 Auswertung der Daten vom GPS im ublox Format
 Hold Modus mit PID Regler
 Rückstuerz zur Basis Funktion
-Stand 8.10.2007
+Stand 12.10.2007
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 */
 #include "main.h"
 //#include "gps.h"
                                                 gps_rel_start_position.utm_north= gps_rel_act_position.utm_north;
                                                 gps_rel_start_position.status   = 1; // gueltige Positionsdaten
                                                 gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
                                                 gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
                                                 gps_rel_hold_position.status    = 1; // gueltige Positionsdaten
-                                                //Richtung zur Home Positionbezogen auf Nordpol bestimmen
+                                                //Richtung zur Home Position bezogen auf Nordpol bestimmen
                                                 hdng_2home = arctan_i(-gps_rel_start_position.utm_east,-gps_rel_start_position.utm_north);
                                                 // in Winkel 0...360 Grad umrechnen
                                                 if (( gps_rel_start_position.utm_east < 0)) hdng_2home = ( 90-hdng_2home);
                                                 else  hdng_2home = (270 - hdng_2home);
-                                                dist_2home = (int)get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
+                                                dist_2home = (int) get_dist(gps_rel_start_position.utm_east,gps_rel_start_position.utm_north,hdng_2home); //Entfernung zur Home Position bestimmen
                                                 gps_state       = GPS_CRTL_HOME_ACTIVE;
                                                 return (GPS_STST_OK);
+                                        }
                                         else
+                                        {
                                         if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
+                                        {
                                                 if ((d1 < GPS_G2T_FAST_TOL)  && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
+                                                {
                                                         if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++;    //Geschwindigkeit langsam erhoehen
-                                                        dist_flown                                              +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
+                                                        dist_flown              +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
-                                                        gps_sub_state                                   = GPS_HOME_FAST_IN_TOL;
+                                                        gps_sub_state   = GPS_HOME_FAST_IN_TOL;
+                                                }
                                                 else    //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
+                                                {
                                                         if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
                                                         dist_flown++;                                                   //Auch ausserhalb der Toleranz langsam erhoehen
                                 dist_north      = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
                                 int_east        += dist_east;
                                 int_north   += dist_north;
                                 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  *3)/4) + (diff_east *1/4); //Differenzierer filtern
+                                        diff_north_f    = ((diff_north_f *3)/4) + (diff_north*1/4); //Differenzierer filtern
+                                }
+                                else // schwache Filterung
+                                {
+//                                      diff_east_f             = diff_east;
+//                                      diff_north_f    = diff_north;
-                                diff_east_f             = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern
+                                        diff_east_f             = ((diff_east_f  *2)/4) + (diff_east *2/4); //Differenzierer filtern
-                                diff_north_f    = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern
+                                        diff_north_f    = ((diff_north_f *2)/4) + (diff_north*2/4); //Differenzierer filtern
+                                }
-*/
                                 #define GPSINT_MAX 30000 //neuer Wert ab 7.10.2007 Begrenzung
                                 if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX)) //Bei zu hohem Wert Integrator auf Wert halten
+                                {
                                         int_east        -= dist_east;
                                         int_north   -= dist_north;
                                 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 * diff_p)/(8*2))+ ((diff_east  * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil X Achse
+                                gps_reg_x = ((int)int_east1  + ((dist_east  * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_east_f  * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil X Achse
-                                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
+                                gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1 * diff_p)/(8*2))+ ((diff_north_f * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil Y Achse
                                 //Ziel-Richtung bezogen auf Nordpol bestimmen

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c @ 492 - Rev 266 → 291