WebSVN - FlightCtrl - Diff - Rev 631 and 632 - /branches/salvo_gps/Basis

 /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 von Peter Muehlenbrock alias Salvo
 Auswertung der Daten vom GPS im ublox Format
 Hold Modus mit PID Regler
 Rückstuerz zur Basis Funktion
-Stand 9.1.2008
+Stand 10.1.2008
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 */
 #include "main.h"
 short int                       gps_state,gps_sub_state; //Zustaende der Statemachine
 short int                       gps_updte_flag;
 static signed int       GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
 static signed int       GPS_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
 static signed int       GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
-static signed int       gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
+static signed int       gps_int_x,gps_int_y;
+static long signed  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
 static signed int       hdng_2home,dist_2home; //Richtung und Entfernung zur home Position
 static signed           gps_tick; //wird bei jedem Update durch das GPS Modul hochgezaehlt
         static signed int       gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
         long signed int         dev;
         signed int                      dist_frm_start_east,dist_frm_start_north;
         int                             amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil
         static signed int       int_east,int_north;     //Integrierer
-        int                             speed_east,speed_north; //Aktuelle Geschwindigkeit
+        long int                        speed_east,speed_north; //Aktuelle Geschwindigkeit
         signed long             int_east1,int_north1;
 //      signed long             dist;
         switch (cmd)
                                         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 - dist_flown); // Restdistanz zum Ziel
-                                        debug_gp_2      = dist_2home;           // zum Debuggen
+//                                      debug_gp_2      = dist_2home;           // zum Debuggen
-                                        debug_gp_3      = dist_flown;           // zum Debuggen
+//                                      debug_gp_3      = dist_flown;           // zum Debuggen
-                                        debug_gp_4      = hdng_2home;           // zum Debuggen
+//                                      debug_gp_4      = hdng_2home;           // zum Debuggen
 //                                      debug_gp_5      = amplfy_speed_north; // zum Debuggen
                                         if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
+                                        {
                 case GPS_CRTL_HOLD_ACTIVE:  // Hier werden die Daten fuer Nick und Roll errechnet
                         if (gps_updte_flag >0)  // nur wenn neue GPS Daten vorliegen
+                        {
                                 // ab hier wird geregelt
-                                dist_east       = gps_rel_hold_position.utm_east  - gps_rel_act_position.utm_east;
+                                dist_east       = gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east;
-                                dist_north      = gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north;
+                                dist_north      = gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north;
                                 int_east        += dist_east;
                                 int_north       += dist_north;
-                                speed_east      = (int) (-actual_speed.speed_e);
+                                speed_east      =  actual_speed.speed_e;
-                                speed_north     = (int) (-actual_speed.speed_n);
+                                speed_north     =  actual_speed.speed_n;
+                                gps_updte_flag  = 0;  // Neue Werte koennen vom GPS geholt werden
+                                debug_gp_2      = dist_east;            // zum Debuggen
+                                debug_gp_3      = dist_north;           // zum Debuggen
                                 gps_updte_flag  = 0;  // Neue Werte koennen vom GPS geholt werden
                                 #define GPSINT_MAX 3000 // Neues Verfahren ab  30.12.2007  bei Integratoroverflow
                                 if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX))
                                         amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_F_MAX-1)*10)/DIFF_X_F_MAX) +10;
                                         if (amplfy_speed_east  > (DIFF_Y_F_MAX *10)) amplfy_speed_east  = DIFF_Y_F_MAX *10; // Begrenzung
                                         if (amplfy_speed_north > (DIFF_Y_F_MAX *10)) amplfy_speed_north = DIFF_Y_F_MAX *10; // Begrenzung
                                         amplfy_speed_east  *= (Parameter_UserParam3/GPS_USR_PAR_FKT);
                                         amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT);
-                                        amplfy_speed_east   /= 10; //Faktor 10 wieder rausnehmen
-                                        amplfy_speed_north  /= 10; //Faktor 10 wieder rausnehmen
+                                }
                                 else  //langsamer Holdmodus
+                                {
                                         // Verstaerkungsfaktor fuer D-Anteil ermitteln um exponentielles Verhalten zu erzielen
                                         amplfy_speed_east  = (((abs(speed_east)) *(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10;
                                         amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_N_MAX-1)*10)/DIFF_X_N_MAX) +10;
                                         if (amplfy_speed_east  > (DIFF_Y_N_MAX *10)) amplfy_speed_east  = DIFF_Y_N_MAX *10; // Begrenzung
                                         if (amplfy_speed_north > (DIFF_Y_N_MAX *10)) amplfy_speed_north = DIFF_Y_N_MAX *10; // Begrenzung
                                         amplfy_speed_east  *= (Parameter_UserParam3/GPS_USR_PAR_FKT);
                                         amplfy_speed_north *= (Parameter_UserParam3/GPS_USR_PAR_FKT);
-                                        amplfy_speed_east   /= 10; //Faktor 10 wieder rausnehmen
+                                        speed_east  = speed_east  * abs(speed_east) /100; //quadrieren
-                                        amplfy_speed_north  /= 10; //Faktor 10 wieder rausnehmen
+                                        speed_north = speed_north * abs(speed_north) /100; //quadrieren
+                                }
+//                                      amplfy_speed_east  /= 10;
-                                #define GPS_SPEED_SCALE 5
+//                                      amplfy_speed_north /= 10;
-                                speed_east  /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren
+                                debug_gp_4      = (int)speed_east;              // zum Debuggen
-                                speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren
+                                debug_gp_5      = (int)speed_north; // zum Debuggen
                                 int diff_p;  //Vom Modus abhaengige zusaetzliche Verstaerkung
                                 if (hold_fast > 0) diff_p = GPS_PROP_FAST_V;
                                 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))+ ((speed_east  * amplfy_speed_east  )/(10/GPS_SPEED_SCALE)));  // I + P +D  Anteil X Achse
+                                gps_reg_x = -(int_east1  + (long)((dist_east  * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_east  * (long)amplfy_speed_east  )/(100L)));  // I + P +D  Anteil X Achse
-                                gps_reg_y = ((int)int_north1 + ((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_north * amplfy_speed_north )/(10/GPS_SPEED_SCALE)));  // I + P +D  Anteil Y Achse
+                                gps_reg_y = -(int_north1 + (long)((dist_north * (Parameter_UserParam1/GPS_USR_PAR_FKT) * diff_p)/(8*2))+ ((speed_north * (long)amplfy_speed_north )/(100L)));  // I + P +D  Anteil Y Achse
-                                debug_gp_0      = gps_reg_x;  // zum Debuggen
+                                debug_gp_0      = (int)gps_reg_x;  // zum Debuggen
-                                debug_gp_1      = gps_reg_y; // zum Debuggen
+                                debug_gp_1      = (int)gps_reg_y; // zum Debuggen
                                 //Ziel-Richtung bezogen auf Nordpol bestimmen
-                                GPS_hdng_abs_2trgt = arctan_i(gps_reg_x,gps_reg_y);
+                                GPS_hdng_abs_2trgt = arctan_i((int)gps_reg_x,(int)gps_reg_y);
                                 else if (GPS_hdng_rel_2trgt <-180)  GPS_hdng_rel_2trgt = 360 + GPS_hdng_rel_2trgt;
                                 // Regelabweichung aus x,y zu Ziel in Distanz umrechnen
                                 if (abs(gps_reg_x) > abs(gps_reg_y) )
                                 {
-                                        dev = (long)gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
+                                        dev = gps_reg_x; //Groesseren Wert wegen besserer Genauigkeit nehmen
                                         dev = abs((dev *1000) / (long) sin_i(GPS_hdng_abs_2trgt));
+                                }
                                 else
+                                {

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/GPS.c - Rev 631 → 632