WebSVN - FlightCtrl - Diff - Rev 197 and 209

 #define                 UBLOX_NAV_VELED                 0x12
 #define                 UBLOX_NAV_CLASS                 0x01
 #define                 UBLOX_SYNCH1_CHAR               0xB5
 #define                 UBLOX_SYNCH2_CHAR               0x62
-signed int GPS_Nick = 0;
+signed int      GPS_Nick = 0;
-signed int GPS_Roll = 0;
+signed int      GPS_Roll = 0;
-short int ublox_msg_state = UBLOX_IDLE;
+short int       ublox_msg_state = UBLOX_IDLE;
-static uint8_t  chk_a =0; //Checksum
+static          uint8_t chk_a =0; //Checksum
-static uint8_t  chk_b =0;
+static          uint8_t chk_b =0;
-short int gps_state;
+short int       gps_state;
-short int  gps_updte_flag;
+short int       gps_updte_flag;
-signed int GPS_hdng_abs_2trgt; //Winkel zum Ziel bezogen auf Nordpol
+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_hdng_rel_2trgt; //Winkel zum Ziel bezogen auf Nordachse des Kopters
-signed int GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
+signed int      GPS_dist_2trgt; //vorzeichenlose Distanz zum Ziel
-signed int gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
-signed int ;
+signed int      gps_int_x,gps_int_y,gps_reg_x,gps_reg_y;
-static unsigned int rx_len;
-unsigned int cnt0,cnt1,cnt2; //******Provisorisch
+static unsigned int rx_len;
 static unsigned int ptr_payload_data_end;
-unsigned int gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
+unsigned int            gps_alive_cnt; // Wird bei jedem gueltigen GPS Telegramm hochgezaehlt
 static uint8_t *ptr_payload_data;
         GPS_Roll                                        =       0;
         gps_updte_flag                          =       0;
         gps_int_x                                       =       0;
         gps_int_y                                       =       0;
         gps_alive_cnt                           =       0;
+}
 // Home Position sichern falls Daten verfuegbar sind.
 void GPS_Save_Home(void)
         short int n = 1;
         if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
         if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
-        {
                 cnt1++; //**** noch Rausschmeissen
                 if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
+                {
                         gps_act_position.utm_east       = actual_pos.utm_east/10;
                         gps_act_position.utm_north      = actual_pos.utm_north/10;
 Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul durch den UART IRQ aufgerufen
 // Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
 */
 void Get_Ublox_Msg(uint8_t rx)
+{
         switch (ublox_msg_state)
+        {
                 case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
                                 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;
+                                }
-                                //Variable Verstarkung fuer Differenzierer ermitteln. Je weiter vom Ziel wir entfernt sind
+                                // 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 1
+                                // 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 40 //Entfernung bei der maximale Verstaerkung erreicht wird in (dm)
+                                #define GPS_DIFF_MAX_D 30 //Entfernung bei der maximale Verstaerkung erreicht wird in 10cm
                                 signed long dist;
                                 int phi;
                                 phi = arctan_i(abs(dist_north),abs(dist_east));
                                 if (abs(dist_east) > abs(dist_north) ) //Zunaechst Entfernung zum Ziel ermitteln
+                                {
                                         dist = (long)dist_east; //Groesseren Wert wegen besserer Genauigkeit nehmen
                                         dist = abs((dist *1000) / (long) sin_i(phi));
+                                }
+                                {
                                         dist = (long)dist_north;
                                         dist = abs((dist *1000) / (long) cos_i(phi));
+                                }
                                 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;
+                                if (diff_v > GPS_DIFF_MAX_V) diff_v = GPS_DIFF_MAX_V; //begrenzen
                                 //PID Regler Werte aufsummieren
                                 gps_reg_x = ((int_east  * Parameter_UserParam2)/32) + ((dist_east  * Parameter_UserParam1)/8)+ ((diff_east_f  * diff_v * Parameter_UserParam3)/10);  // I + P +D  Anteil X Achse

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c - Rev 197 → 209