WebSVN - FlightCtrl - Diff - Rev 224 and 225

 /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Peter Muehlenbrock
 Auswertung der Daten vom GPS im ublox Format
 Hold Modus mit PID Regler
 Komm heim zu Papi Funktion
-Stand 5.10.2007
+Stand 6.10.2007
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 */
 #include "main.h"
 //#include "gps.h"
 signed int      GPS_Nick = 0;
 signed int      GPS_Roll = 0;
 short int       ublox_msg_state = UBLOX_IDLE;
 static          uint8_t chk_a =0; //Checksum
 static          uint8_t chk_b =0;
-short int       gps_state,gps_sub_state;
+short int       gps_state,gps_sub_state; //Zustaende der Statemachine
 short int       gps_updte_flag;
 signed int      GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
 signed int      GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
 signed int      GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
 signed int      gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
 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 uint8_t *ptr_payload_data;
 static uint8_t *ptr_pac_status;
-long int dist_flown,dist_frm_start_east,dist_frm_start_north;
+long int dist_flown;
 short int Get_GPS_data(void);
         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;
         long signed int dev,n_l;
+        signed int dist_frm_start_east,dist_frm_start_north;
         switch (cmd)
+        {
                 case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
                                 if (cnt > 500) // erst nach Verzoegerung
+                                {
                                         // Erst mal initialisieren
                                         cnt                             = 0;
                                         gps_tick                = 0;
+                                        hold_fast               = 0;
                                         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;
+                                {
                                         cnt     =       0;
                                         // aktuelle positionsdaten abspeichern
                                         if (gps_rel_act_position.status > 0)
+                                        {
+                                                hold_fast       = 0;
                                                 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 ((gps_updte_flag > 0) && (gps_sub_state !=GPS_HOME_FINISHED)) // nur wenn neue GPS Daten vorliegen und nicht schon alles fertig ist
+                                {
                                         gps_tick++;
                                         int d1,d2,d3;
+                                        d1      = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
+                                        d2      = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
                                         d3      = (dist_2home - (int)dist_flown); // Restdistanz zum Ziel
-                                        d1 = abs (gps_rel_hold_position.utm_east - gps_rel_act_position.utm_east );
-                                        d2 = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
                                         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
                                                         dist_flown                              += (GPS_G2T_V_MAX); // Vorgabe der Strecke anhand der Geschwindigkeit
-                                                        dist_frm_start_east             = (dist_flown * (long)sin_i(hdng_2home))/1000;
+                                                        dist_frm_start_east             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
-                                                        dist_frm_start_north    = (dist_flown * (long)cos_i(hdng_2home))/1000;
+                                                        dist_frm_start_north    = (int)((dist_flown * (long)cos_i(hdng_2home))/1000);
-                                                        gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + (int)dist_frm_start_north; //naechster Zielpunkt
+                                                        gps_rel_hold_position.utm_east = gps_rel_start_position.utm_east + dist_frm_start_east; //naechster Zielpunkt
-                                                        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 + dist_frm_start_north; //naechster Zielpunkt
+                                                }
                                                 else    hold_fast = 0; // Wieder normal regeln
+                                        }
                                         else   //Schon ziemlich nahe am Ziel, deswegen abbremsen
+                                        {
                                                 hold_fast       = 0; // Wieder normal regeln
                                                 if ((d1 < GPS_G2T_TOL)  && (d2 < GPS_G2T_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
                                                 {
-                                                        if (d3 > 0)
+                                                        if (d3 > GPS_G2T_DIST_HOLD)
+                                                        {
                                                                 dist_flown                              += GPS_G2T_V_RAMP_DWN; // 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;
-                                                                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
-                                                                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
                                                         }
-                                                        else //Ziel erreicht, Regelung beenden
+                                                        else //Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
                                                         {
+                                                                if (gps_rel_hold_position.utm_east > 1) gps_rel_hold_position.utm_east--;
-                                                                gps_rel_hold_position.utm_east  = 0;
+                                                                else if (gps_rel_hold_position.utm_east <-1 ) gps_rel_hold_position.utm_east++;
-                                                                gps_rel_hold_position.utm_north = 0;
+                                                                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++;
-                                                                gps_sub_state                                   = GPS_HOME_FINISHED;
+                                                                if ((abs(gps_rel_hold_position.utm_east) <= 1) && (abs(gps_rel_hold_position.utm_north) <=1)) gps_sub_state     = GPS_HOME_FINISHED;
                                                         }
-                                                }
+                                                }
+                                        }
                                         gps_state       = GPS_CRTL_HOLD_ACTIVE; //Zwischensprung
                                         return (GPS_STST_OK);
+                                }
                                 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
+                                {
+                                        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;
                                 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 (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
+                                if (hold_fast > 0) diff_v = 10;  // im schnellen Modus keine staerkere Wirkung des Differenzierers
                                 //PID Regler Werte aufsummieren

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c @ 232 - Rev 224 → 225