WebSVN - FlightCtrl - Diff - Rev 1050 and 1052 - /branches/salvo_gps/Basis

 Hold Modus mit PID Regler
 Rückstuerz zur Basis Funktion
 Umstellung auf NaviParameter an Flight Version 00.70d
 GPS_V durch gps_gain ersetzt, damit Einstellung durch MK Tool möglich wird
-Stand 24.11.2008
+Stand 27.11.2008
-Aenderung 24.11.2008: gps_gain erhoeht
+Aenderung 27.11.2008: gps_gain erhoeht
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 */
 #include "main.h"
 static                  short int hold_fast,hold_reset_int; //Flags fuer Hold Regler
 static                  uint8_t *ptr_payload_data;
 static                  uint8_t *ptr_pac_status;
 static  int             dist_flown;
 //static unsigned int   int_ovfl_cnt; // Zaehler fuer Overflows des Integrators
-static  int             gps_quiet_cnt; //  Zaehler fuer GPS Off Time beim Kameraausloesen
+static  int                     gps_quiet_cnt;  //  Zaehler fuer GPS Off Time beim Kameraausloesen
-static  int             gps_gain;          //  // Teilerfaktor Regelabweichung zu Ausgabewert
+static  long int        limit_gain;     // Teilerfaktor Regelabweichung zu Ausgabewert
+static  long int        gps_gain        ;       // Verstaerkunsgfaktor*10
 short int Get_GPS_data(void);
 GPS_REL_POSITION_t              gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
 // Initialisierung
 void GPS_Neutral(void)
 {
-        ublox_msg_state                 =       UBLOX_IDLE;
+        ublox_msg_state                         =       UBLOX_IDLE;
-        gps_state                               =       GPS_CRTL_IDLE;
+        gps_state                                       =       GPS_CRTL_IDLE;
-        gps_sub_state                   =       GPS_CRTL_IDLE;
+        gps_sub_state                           =       GPS_CRTL_IDLE;
-        actual_pos.status               =       0;
+        actual_pos.status                       =       0;
-        actual_speed.status             =       0;
+        actual_speed.status                     =       0;
         actual_status.status            =       0;
         gps_home_position.status        =       0; // Noch keine gueltige Home Position
         gps_act_position.status         =       0;
         gps_rel_act_position.status     =       0;
-        GPS_Nick                        =       0;
+        GPS_Nick                                        =       0;
-        GPS_Roll                        =       0;
+        GPS_Roll                                        =       0;
-        gps_updte_flag                  =       0;
+        gps_updte_flag                          =       0;
-        gps_alive_cnt                   =       0;
+        gps_alive_cnt                           =       0;
+}
 short int Get_GPS_data(void)
+{
         short int n = 1;
         if (actual_pos.status == 0) return (1); //damit es schnell geht, wenn nix zu tun ist
+        gps_gain = Parameter_NaviGpsGain/10;
-        gps_gain = (Parameter_NaviGpsGain*8)/20; //maximal Wert ist 255*8/40
+        limit_gain = 8;
-        //      debug_gp_0      = (int)gps_gain;  // zum Debuggen
+        //      debug_gp_0      = (int)limit_gain;  // zum Debuggen
         if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
+        {
 */
 void Get_Ublox_Msg(uint8_t rx)
+{
         switch (ublox_msg_state)
+        {
                 case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
                         if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
                         else ublox_msg_state = UBLOX_IDLE;
                         break;
 //Zentrale Statemachine fuer alle GPS relevanten Regelungsablauefe
 short int GPS_CRTL(short int cmd)
+{
         static unsigned int cnt;                                                // Zaehler fuer diverse Verzoegerungen
         static long int         delta_north,delta_east;         // Mass fuer Distanz zur Sollposition
-        signed int              n;
+        signed int                      n;
         static signed int       gps_g2t_act_v; // Aktuelle Geschwindigkeitsvorgabe fuer Home Funktion
-        signed int              dist_frm_start_east,dist_frm_start_north;
+        signed int                      dist_frm_start_east,dist_frm_start_north;
-        int                     amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil
+        int                                     amplfy_speed_east,amplfy_speed_north; //Verstaerkungsfaktoren fuer D-Anteil
         static signed int       int_east,int_north;     //Integrierer
-        long int        speed_east,speed_north; //Aktuelle Geschwindigkeit
+        long int                        speed_east,speed_north; //Aktuelle Geschwindigkeit
-        signed long int_east1,int_north1;
+        signed long             int_east1,int_north1;
-        int                     dist_east,dist_north;
+        int                                     dist_east,dist_north;
-        int                     diff_p;                 //Vom Modus abhaengige zusaetzliche Verstaerkung
+        int                                     diff_p;                 //Vom Modus abhaengige zusaetzliche Verstaerkung
-        long            ni,ro;                  // Nick und Roll Zwischenwerte
+        long                            ni,ro;                  // Nick und Roll Zwischenwerte
+                        {
                                 gps_quiet_cnt++;
                                 // ab hier wird geregelt
                                 delta_east      = (long) (gps_rel_act_position.utm_east - gps_rel_hold_position.utm_east);
                                 delta_north     = (long) (gps_rel_act_position.utm_north - gps_rel_hold_position.utm_north);
-                                int_east        += (int)delta_east;
+                                int_east        += (int)(delta_east*gps_gain)/10;
-                                int_north       += (int)delta_north;
+                                int_north       += (int)(delta_north*gps_gain)/10;
                                 speed_east      =  actual_speed.speed_e;
                                 speed_north     =  actual_speed.speed_n;
                                 gps_updte_flag  = 0;  // Neue Werte koennen vom GPS geholt werden
                                 dist_east       = (int)delta_east; //merken
                                 dist_north      = (int)delta_north;
 //                              #define GPSINT_MAX 3000 // Neues Verfahren ab  30.12.2007  bei Integratoroverflow
                                 long int gpsintmax;
                                 if (Parameter_NaviGpsI > 0)
                                 {
-                                        gpsintmax = (GPS_NICKROLL_MAX * gps_gain * GPS_USR_PAR_FKT * ((32*3)/10))/(long)Parameter_NaviGpsI; //auf ungefeahren Maximalwert begrenzen
+                                        gpsintmax = (GPS_NICKROLL_MAX * limit_gain * GPS_USR_PAR_FKT * ((32*3)/10))/(long)Parameter_NaviGpsI; //auf ungefeahren Maximalwert begrenzen
                                         if ((abs(int_east) > (int)gpsintmax) || (abs(int_north)> (int)gpsintmax))
+                                        {
 //                                              //      = 1; // Zahl der Overflows zaehlen
 //                                              int_ovfl_cnt    -= 1;
                                 debug_gp_4      = (int)int_east;  // zum Debuggen
                                 debug_gp_5      = (int)int_north; // zum Debuggen
                                 //I Werte begrenzen
-                                #define INT1_MAX (GPS_NICKROLL_MAX * gps_gain*3)/10// auf 30 Prozent des maximalen Nick/Rollwert begrenzen
+                                #define INT1_MAX (GPS_NICKROLL_MAX * limit_gain*3)/10// auf 30 Prozent des maximalen Nick/Rollwert begrenzen
                                 int_east1  =  ((((long)int_east)   * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT;
                                 int_north1 =  ((((long)int_north)  * Parameter_NaviGpsI)/32)/GPS_USR_PAR_FKT;
                                 if (int_east1 > INT1_MAX) int_east1 =  INT1_MAX; //begrenzen
+                                {
                                         amplfy_speed_east  = DIFF_Y_F_MAX;
                                         amplfy_speed_north = DIFF_Y_F_MAX;
                                         amplfy_speed_east  *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
                                         amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
-                                        speed_east  = (speed_east   * (long)amplfy_speed_east) /50;
+                                        speed_east  = (speed_east   * (long)amplfy_speed_east*gps_gain) /500;
-                                        speed_north = (speed_north  * (long)amplfy_speed_north)/50;
+                                        speed_north = (speed_north  * (long)amplfy_speed_north*gps_gain)/500;
                                         // D Werte begrenzen
-                                        #define D_F_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
+                                        #define D_F_MAX (GPS_NICKROLL_MAX * limit_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
                                         if (speed_east  > D_F_MAX) speed_east = D_F_MAX;
                                         else if (speed_east < -D_F_MAX) speed_east = -D_F_MAX;
                                         if (speed_north > D_F_MAX) speed_north = D_F_MAX;
                                         else if (speed_north < -D_F_MAX) speed_north = -D_F_MAX;
+                                {
                                         amplfy_speed_east  = DIFF_Y_N_MAX;
                                         amplfy_speed_north = DIFF_Y_N_MAX;
                                         amplfy_speed_east  *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
                                         amplfy_speed_north *= (Parameter_NaviGpsD/GPS_USR_PAR_FKT);
-                                        speed_east  = (speed_east   * (long)amplfy_speed_east) /25;
+                                        speed_east  = (speed_east   * (long)amplfy_speed_east*gps_gain) /250;
-                                        speed_north = (speed_north  * (long)amplfy_speed_north)/25;
+                                        speed_north = (speed_north  * (long)amplfy_speed_north*gps_gain)/250;
                                         // D Werte begrenzen
-                                        #define D_N_MAX (GPS_NICKROLL_MAX * gps_gain*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen
+                                        #define D_N_MAX (GPS_NICKROLL_MAX * limit_gain*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen
                                         if (speed_east  > D_N_MAX) speed_east = D_N_MAX;
                                         else if (speed_east < -D_N_MAX) speed_east = -D_N_MAX;
                                         if (speed_north > D_N_MAX) speed_north = D_N_MAX;
                                         else if (speed_north < -D_N_MAX) speed_north = -D_N_MAX;
 //                              debug_gp_4      = (int)speed_east;      // zum Debuggen
 //                              debug_gp_5      = (int)speed_north; // zum Debuggen
                                 //P-Werte verstaerken
-                                delta_east      = (delta_east   * (long)diff_p)/(40);
+                                delta_east      = (delta_east   * (long)diff_p*gps_gain)/(400);
-                                delta_north     = (delta_north  * (long)diff_p)/(40);
+                                delta_north     = (delta_north  * (long)diff_p*gps_gain)/(400);
                                 if (hold_fast > 0)  //schneller Coming Home Modus
+                                {
                                         // P Werte begrenzen
-                                        #define P1_F_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
+                                        #define P1_F_MAX (GPS_NICKROLL_MAX * limit_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
                                         if (delta_east   > P1_F_MAX) delta_east = P1_F_MAX;
                                         else if (delta_east < -P1_F_MAX) delta_east = -P1_F_MAX;
                                         if (delta_north > P1_F_MAX) delta_north = P1_F_MAX;
                                         else if (delta_north < -P1_F_MAX) delta_north = -P1_F_MAX;
+                                }
                                 else // Hold modus
+                                {
                                         // P Werte begrenzen
-                                        #define P1_N_MAX (GPS_NICKROLL_MAX * gps_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
+                                        #define P1_N_MAX (GPS_NICKROLL_MAX * limit_gain*7)/10 // auf 70 Prozent des Maximalen Nick/Rollwert begrenzen
                                         if (delta_east   > P1_N_MAX) delta_east = P1_N_MAX;
                                         else if (delta_east < -P1_N_MAX) delta_east = -P1_N_MAX;
                                 debug_gp_0      = (int)gps_reg_x; // zum Debuggen
                                 debug_gp_1      = (int)gps_reg_y; // zum Debuggen
                                 // Werte fuer Nick und Roll direkt aus gps_reg_x und gps_reg_y bestimmen
                                 n  = GyroKomp_Int/GIER_GRAD_FAKTOR; //Ausrichtung Kopter
-                                ni = -((gps_reg_y * (long)cos_i(n)) + (gps_reg_x * (long)sin_i(n)))/(1000*gps_gain);
+                                ni = -((gps_reg_y * (long)cos_i(n)) + (gps_reg_x * (long)sin_i(n)))/(1000L*(long)limit_gain);
-                                ro =  ((gps_reg_x * (long)cos_i(n)) - (gps_reg_y * (long)sin_i(n)))/(1000*gps_gain);
+                                ro =  ((gps_reg_x * (long)cos_i(n)) - (gps_reg_y * (long)sin_i(n)))/(1000L*(long)limit_gain);
                                 if (ni > (GPS_NICKROLL_MAX )) ni = (GPS_NICKROLL_MAX);
                                 else if (ni < -(GPS_NICKROLL_MAX )) ni = -(GPS_NICKROLL_MAX );

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0070d/trunk/GPS.c @ 1059 - Rev 1050 → 1052