WebSVN - FlightCtrl - Diff - Rev 225 and 236

 */
 /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Peter Muehlenbrock
 Auswertung der Daten vom GPS im ublox Format
 Hold Modus mit PID Regler
-Komm heim zu Papi Funktion
+Rückstuerz zur Basis Funktion
 Stand 6.10.2007
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 */
 #include "main.h"
 //#include "gps.h"
 static unsigned int rx_len;
 static unsigned int ptr_payload_data_end;
 unsigned int            gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
 signed hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
 signed gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt
-static short int hold_fast; //Flag fuer Hold Regler
+static short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
 static uint8_t *ptr_payload_data;
 static uint8_t *ptr_pac_status;
 long int dist_flown;
         static signed int int_east,int_north;   //Integrierer
         static signed int dist_north,dist_east;
         static signed int diff_east,diff_north;         // Differenzierer  (Differenz zum  vorhergehenden x bzw. y  Wert)
         static signed int diff_east_f,diff_north_f; // Differenzierer,  gefiltert
         signed int n,diff_v;
+        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;
         switch (cmd)
+        {
+                                {
                                         // Erst mal initialisieren
                                         cnt                             = 0;
                                         gps_tick                = 0;
                                         hold_fast               = 0;
+                                        hold_reset_int  = 0; // Integrator enablen
                                         int_east                = 0, int_north  = 0;
                                         gps_reg_x               = 0, gps_reg_y  = 0;
                                         dist_east               = 0, dist_north = 0;
                                         diff_east_f             = 0, diff_north_f= 0;
                                         diff_east               = 0, diff_north  = 0;
                                         dist_flown              = 0;
+                                        gps_g2t_act_v   = 0;
                                         gps_sub_state   = GPS_CRTL_IDLE;
                                         // aktuelle positionsdaten abspeichern
                                         if (gps_rel_act_position.status > 0)
+                                        {
                                                 gps_rel_start_position.utm_east = gps_rel_act_position.utm_east;
                                         cnt     =       0;
                                         // aktuelle positionsdaten abspeichern
                                         if (gps_rel_act_position.status > 0)
+                                        {
-                                                hold_fast       = 0;
+                                                hold_fast               = 0;
+                                                hold_reset_int  = 0; // Integrator enablen
                                                 int_east        = 0, int_north  = 0;
                                                 gps_reg_x       = 0, gps_reg_y  = 0;
                                                 dist_east       = 0, dist_north = 0;
                                                 diff_east_f     = 0, diff_north_f= 0;
                                                 diff_east       = 0, diff_north  = 0;
                                         if (d3  > GPS_G2T_DIST_MAX_STOP)
+                                        {
                                                 if ((d1 < GPS_G2T_TOL)  && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
+                                                {
                                                         hold_fast                               = 1; // Schnell Regeln
+                                                        if (gps_g2t_act_v < GPS_G2T_V_MAX) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
-                                                        dist_flown                              += (GPS_G2T_V_MAX); // Vorgabe der Strecke anhand der Geschwindigkeit
+                                                        dist_flown                              += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
                                                         dist_frm_start_east             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
                                                         dist_frm_start_north    = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
                                                         gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
                                                         gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
+                                                }
+                                                else    //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
+                                                {
+                                                        if (gps_g2t_act_v > 0) gps_g2t_act_v--;
-                                                else    hold_fast = 0; // Wieder normal regeln
+                                                        if (gps_g2t_act_v <= (GPS_G2T_V_MAX/2)) hold_fast = 0; // Wieder normal regeln
+                                                }
+                                        }
                                         else   //Schon ziemlich nahe am Ziel, deswegen abbremsen
+                                        {
-                                                hold_fast       = 0; // Wieder normal regeln
+                                                hold_fast               = 0; // Wieder normal regeln
                                                 if ((d1 < GPS_G2T_TOL)  && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
+                                                {
                                                         if (d3 > GPS_G2T_DIST_HOLD)
+                                                        {
+                                                                if (gps_g2t_act_v < GPS_G2T_V_RAMP_DWN) gps_g2t_act_v++; //Geschwindigkeit langsam erhoehen
-                                                                dist_flown                              += GPS_G2T_V_RAMP_DWN; // Vorgabe der Strecke anhand der Geschwindigkeit
+                                                                dist_flown                              += (long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
                                                                 dist_frm_start_east             = (dist_flown * (long)sin_i(hdng_2home))/1000;
                                                                 dist_frm_start_north    = (dist_flown * (long)cos_i(hdng_2home))/1000;
                                                                 gps_rel_hold_position.utm_east  = gps_rel_start_position.utm_east + (int)dist_frm_start_east; //naechster Zielpunkt
                                                                 gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
+                                                        }
                                                                 if (gps_rel_hold_position.utm_north > 1) gps_rel_hold_position.utm_north--;
                                                                 else if (gps_rel_hold_position.utm_north < -1 ) gps_rel_hold_position.utm_north++;
                                                                 if ((abs(gps_rel_hold_position.utm_east) <= 1) && (abs(gps_rel_hold_position.utm_north) <=1)) gps_sub_state     = GPS_HOME_FINISHED;
+                                                        }
+                                                }
+                                                else
+                                                {
+                                                        if (gps_g2t_act_v > 0) gps_g2t_act_v--;
+                                        }
+                                        }
                                         gps_state       = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
                                         return (GPS_STST_OK);
+                                }
                                 else return (GPS_STST_OK);
                                 diff_north      += dist_north;  // Differenz zur vorhergehenden North Position
 /*
                                 diff_east_f             = ((diff_east_f )/4) + (diff_east*3/4); //Differenzierer filtern
                                 diff_north_f    = ((diff_north_f)/4) + (diff_north*3/4); //Differenzierer filtern
 */
-                                #define GPSINT_MAX 2048 //neuer Wert ab 1.10.2007 Begrenzung
+                                #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;
+                                }
-                                if (hold_fast > 0) //Im Schnellen Mode Integrator abschalten
+                                if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten
+                                {
                                         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
                                 #define GPS_DIFF_MAX_V 20 //maximale Verstaerkung * 10
                                 #define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
-                                signed long dist;
+                                signed long dist,int_east1,int_north1;
                                 int phi;
                                 phi = arctan_i(abs(dist_north),abs(dist_east));
                                 dist = get_dist(dist_east,dist_north,phi); //Zunaechst Entfernung zum Ziel ermitteln
                                 diff_v = (int)((dist * (GPS_DIFF_MAX_V - 10)) / GPS_DIFF_MAX_D) +10; //Verstaerkung * 10
+                                if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
+                                if (hold_fast > 0) diff_v = 10;  //im schnellen Modus schwache Wirkung des Differenzierers
+                                //I Werte begrenzen
+                                #define INT1_MAX (20 * GPS_V)
+                                int_east1 =  ((((long)int_east)   * Parameter_UserParam2)/32);
+                                int_north =  ((((long)int_north)  * Parameter_UserParam2)/32);
+                                if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen
+                                else if (int_east1 < -INT1_MAX) int_east1 =  -INT1_MAX;
-                                if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
+                                if (int_north1 > INT1_MAX) int_north1 =  INT1_MAX; //begrenzen
-                                if (hold_fast > 0) diff_v = 10;  // im schnellen Modus keine staerkere Wirkung des Differenzierers
+                                else if (int_north1 < -INT1_MAX) int_north1 =  -INT1_MAX;
                                 //PID Regler Werte aufsummieren
-                                gps_reg_x = ((int_east  * Parameter_UserParam2)/32) + ((dist_east  * Parameter_UserParam1)/8)+ ((diff_east  * diff_v * Parameter_UserParam3)/10);  // I + P +D  Anteil X Achse
+                                gps_reg_x = ((int)int_east1  + ((dist_east  * Parameter_UserParam1)/8)+ ((diff_east  * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil X Achse
-                                gps_reg_y = ((int_north * Parameter_UserParam2)/32) + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10);  // I + P +D  Anteil Y Achse
+                                gps_reg_y = ((int)int_north1 + ((dist_north * Parameter_UserParam1)/8)+ ((diff_north * diff_v * Parameter_UserParam3)/10));  // I + P +D  Anteil Y Achse
                                 GPS_dist_2trgt  = (int) dev;
                                 // Winkel und Distanz in Nick und Rollgroessen umrechnen
                                 GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
                                 GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
                                 #define GPS_V 8 // auf Maximalwert normieren. Teilerfaktor ist 8
                                 if (GPS_Roll > (GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = (GPS_NICKROLL_MAX * GPS_V);
                                 else if (GPS_Roll < -(GPS_NICKROLL_MAX * GPS_V)) GPS_Roll = -(GPS_NICKROLL_MAX * GPS_V);
                                 if (GPS_Nick > (GPS_NICKROLL_MAX * GPS_V)) GPS_Nick = (GPS_NICKROLL_MAX * GPS_V);

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c @ 492 - Rev 225 → 236