WebSVN - FlightCtrl - Diff - Rev 677 and 698 - /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 20.1.2008
+Stand 29.1.2008
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 */
 #include "main.h"
 #include "math.h"
 //#include "gps.h"
 // Defines fuer ublox Messageformat um Auswertung zu steuern
-#define                 UBLOX_IDLE              0
+#define                 UBLOX_IDLE      0
-#define                 UBLOX_SYNC1             1
+#define                 UBLOX_SYNC1     1
-#define                 UBLOX_SYNC2             2
+#define                 UBLOX_SYNC2     2
-#define                 UBLOX_CLASS             3
+#define                 UBLOX_CLASS     3
-#define                 UBLOX_ID                4
+#define                 UBLOX_ID        4
-#define                 UBLOX_LEN1              5
+#define                 UBLOX_LEN1      5
-#define                 UBLOX_LEN2              6
+#define                 UBLOX_LEN2      6
-#define                 UBLOX_CKA               7
+#define                 UBLOX_CKA       7
-#define                 UBLOX_CKB               8
+#define                 UBLOX_CKB       8
 #define                 UBLOX_PAYLOAD   9
 // ublox Protokoll Identifier
-#define                 UBLOX_NAV_POSUTM                0x08
+#define                 UBLOX_NAV_POSUTM        0x08
-#define                 UBLOX_NAV_STATUS                0x03
+#define                 UBLOX_NAV_STATUS        0x03
-#define                 UBLOX_NAV_VELED                 0x12
+#define                 UBLOX_NAV_VELED         0x12
-#define                 UBLOX_NAV_CLASS                 0x01
+#define                 UBLOX_NAV_CLASS         0x01
-#define                 UBLOX_SYNCH1_CHAR               0xB5
+#define                 UBLOX_SYNCH1_CHAR       0xB5
-#define                 UBLOX_SYNCH2_CHAR               0x62
+#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,gps_sub_state; //Zustaende der Statemachine
+short int               gps_state,gps_sub_state; //Zustaende der Statemachine
-short int                       gps_updte_flag;
+short int               gps_updte_flag;
 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                          short int hold_fast,hold_reset_int; //Flags 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_payload_data;
-static                          uint8_t *ptr_pac_status;
+static                  uint8_t *ptr_pac_status;
-static  int                     dist_flown;
+static  int             dist_flown;
-static unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators
+//static unsigned int   int_ovfl_cnt; // Zaehler fuer Overflows des Integrators
 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;
+}
 // Home Position sichern falls Daten verfuegbar sind.
+                {
                         actual_status.status            = 0;
                         gps_act_position.utm_east       = actual_pos.utm_east/10;
                         gps_act_position.utm_north      = actual_pos.utm_north/10;
                         gps_act_position.utm_alt        = actual_pos.utm_alt/10;
-                        actual_pos.status                       = 0; //neue ublox Messages anfordern
+                        actual_pos.status               = 0; //neue ublox Messages anfordern
                         gps_act_position.speed_gnd      = actual_speed.speed_gnd;
                         gps_act_position.speed_gnd      = actual_speed.speed_gnd;
                         gps_act_position.heading        = actual_speed.heading/100000;
                         actual_speed.status             = 0;
                         gps_act_position.status         = 1;
                         n                                                       = 0; //Daten gueltig
+                }
                 else
+                {
                         gps_act_position.status = 0; //Keine gueltigen Daten
                         actual_speed.status     = 0;
-                        actual_status.status    = 0;
+                        actual_status.status    = 0;
-                        actual_pos.status               = 0; //neue ublox Messages anfordern
+                        actual_pos.status       = 0; //neue ublox Messages anfordern
-                        n                                               = 2;
+                        n                       = 2;
+                }
+        }
         return (n);
+}
                                 case UBLOX_NAV_STATUS:
                                         ptr_pac_status  =       &actual_status.status;
                                         if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
                                         else
+                                        {
-                                                ptr_payload_data                = &actual_status;
+                                                ptr_payload_data        = &actual_status;
                                                 ptr_payload_data_end    = &actual_status.status;
-                                                ublox_msg_state                 = UBLOX_LEN1;
+                                                ublox_msg_state         = UBLOX_LEN1;
+                                        }
                                         break;
                                 case UBLOX_NAV_VELED:
                                         ptr_pac_status          =       &actual_speed.status;
                                         if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
                                         else
                                         {
-                                                ptr_payload_data                = &actual_speed;
+                                                ptr_payload_data        = &actual_speed;
                                                 ptr_payload_data_end    = &actual_speed.status;
-                                                ublox_msg_state                 = UBLOX_LEN1;
+                                                ublox_msg_state         = UBLOX_LEN1;
+                                        }
                                         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;
-        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
         switch (cmd)
+        {
                 case GPS_CMD_REQ_HOME: // Es soll zum Startpunkt zurueckgeflogen werden.
                         if ((gps_state != GPS_CRTL_HOLD_ACTIVE) && (gps_state != GPS_CRTL_HOME_ACTIVE))
+                        {
                                 cnt++;
-                                if (cnt > 50) // erst nach Verzoegerung
+                                if (cnt > 100) // erst nach Verzoegerung
                                 {
                                         // Erst mal initialisieren
                                         cnt             = 0;
-                                        gps_tick                = 0;
+                                        gps_tick        = 0;
-                                        hold_fast               = 0;
+                                        hold_fast       = 0;
                                         hold_reset_int  = 0; // Integrator enablen
-                                        int_east                = 0, int_north  = 0;
+                                        int_east        = 0, int_north          = 0;
-                                        gps_reg_x               = 0, gps_reg_y  = 0;
+                                        gps_reg_x       = 0, gps_reg_y          = 0;
-                                        delta_east              = 0, delta_north        = 0;
+                                        delta_east      = 0, delta_north        = 0;
-                                        dist_flown              = 0;
+                                        dist_flown      = 0;
                                         gps_g2t_act_v   = 0;
                                         gps_sub_state   = GPS_CRTL_IDLE;
                                         cnt     =       0;
                                         // aktuelle positionsdaten abspeichern
                                         if (gps_rel_act_position.status > 0)
+                                        {
                                                 hold_fast               = 0;
-                                                hold_reset_int  = 0; // Integrator enablen
+                                                hold_reset_int          = 0; // Integrator enablen
                                                 int_east                = 0, int_north  = 0;
                                                 gps_reg_x               = 0, gps_reg_y  = 0;
                                                 delta_east              = 0, delta_north        = 0;
                                                 speed_east              = 0; speed_north= 0;
-                                                int_ovfl_cnt    = 0;
+//                                              int_ovfl_cnt            = 0;
-                                                gps_quiet_cnt   = 0;
+                                                gps_quiet_cnt           = 0;
                                                 gps_rel_hold_position.utm_east  = gps_rel_act_position.utm_east;
                                                 gps_rel_hold_position.utm_north = gps_rel_act_position.utm_north;
                                                 gps_rel_hold_position.status    = 1; // gueltige Positionsdaten
                                                 gps_state                                               = GPS_CRTL_HOLD_ACTIVE;
                                                 return (GPS_STST_OK);
                                 else return(GPS_STST_PEND); // noch warten
+                        }
                         break;
                 case GPS_CMD_STOP: // Lageregelung beenden
-                        cnt                             =       0;
+                        cnt                     =       0;
                         GPS_Nick                =       0;
                         GPS_Roll                =       0;
-                        gps_sub_state   =       GPS_CRTL_IDLE;
+                        gps_sub_state           =       GPS_CRTL_IDLE;
                         gps_state               =       GPS_CRTL_IDLE;
                         return (GPS_STST_OK);
                         break;
                                         d2      = abs (gps_rel_hold_position.utm_north - gps_rel_act_position.utm_north );
                                         d3      = (dist_2home - dist_flown); // Restdistanz zum Ziel
                                         if (d3 > GPS_G2T_DIST_MAX_STOP) // Schneller Rueckflug, noch weit weg vom Ziel
                                         {
-                                                if ((d1 < GPS_G2T_FAST_TOL/2)  && (d2 < GPS_G2T_FAST_TOL/2)) //voll Stoff weiter wenn Lage gut innerhalb der Toleranz
+                                                if ((d1 < (GPS_G2T_FAST_TOL/2))  && (d2 < (GPS_G2T_FAST_TOL/2))) //voll Stoff weiter wenn Lage gut innerhalb der Toleranz
+                                                {
                                                         if (gps_g2t_act_v < GPS_G2T_V_MAX-3) gps_g2t_act_v += 4; //Geschwindigkeit erhoehen
                                                         dist_flown              +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
                                                         gps_sub_state   = GPS_HOME_FAST_IN_TOL;
+                                                }
                                                 else if ((d1 < GPS_G2T_FAST_TOL)  && (d2 < GPS_G2T_FAST_TOL)) //nur weiter wenn Lage innerhalb der Toleranz
                                                 {
-                                                        if (gps_g2t_act_v > GPS_G2T_V_MAX/2) gps_g2t_act_v -= 1; //Geschwindigkeit auf Haeflte runter oder rauffahren
+                                                        if (gps_g2t_act_v > (GPS_G2T_V_MAX/2)) gps_g2t_act_v -= 1; //Geschwindigkeit auf Haelfte runter oder rauffahren
-                                                        else if (gps_g2t_act_v < GPS_G2T_V_MAX/2) gps_g2t_act_v += 1;
+                                                        else if (gps_g2t_act_v < (GPS_G2T_V_MAX/2)) gps_g2t_act_v += 1;
                                                         dist_flown              +=gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
                                                         gps_sub_state   = GPS_HOME_FAST_IN_TOL;
+                                                }
                                                 else    //Den Lageregler in Ruhe arbeiten lassen weil ausserhalb der Toleranz
+                                                {
                                                         if (gps_g2t_act_v > 1) gps_g2t_act_v--; // Geschwindigkeit reduzieren
 //                                                      dist_flown++;                           //Auch ausserhalb der Toleranz langsam erhoehen
                                                         gps_sub_state   = GPS_HOME_FAST_OUTOF_TOL;
                                                 }
-                                                hold_reset_int                  = 1; // Integrator aussschalten
+                                                hold_reset_int                  = 0; // Integrator einsschalten
                                                 hold_fast                               = 1; // Regler fuer schnellen Flug
                                                 dist_frm_start_east             = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000);
                                                 dist_frm_start_north    = (int)(((long)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
                                                         else if (gps_rel_hold_position.utm_north <= - GPS_G2T_V_MIN ) gps_rel_hold_position.utm_north += GPS_G2T_V_MIN;
                                                         if ((abs(gps_rel_hold_position.utm_east) <= GPS_G2T_V_MIN) && (abs(gps_rel_hold_position.utm_north) <=GPS_G2T_V_MIN))
+                                                        {
                                                                 gps_rel_hold_position.utm_east  = 0;
                                                                 gps_rel_hold_position.utm_north = 0;
-                                                                gps_sub_state                                   = GPS_HOME_FINISHED;
+                                                                gps_sub_state                   = GPS_HOME_FINISHED;
+                                                        }
+                                                }
                                                 else gps_sub_state      = GPS_HOME_OUTOF_TOL;
+                                        }
+                                }
 //                              #define GPSINT_MAX 3000 // Neues Verfahren ab  30.12.2007  bei Integratoroverflow
                                 long int gpsintmax;
                                 if (Parameter_UserParam2 > 0)
                                 {
-                                        gpsintmax = (GPS_NICKROLL_MAX * GPS_V * GPS_USR_PAR_FKT * ((32*4)/10))/(long)Parameter_UserParam2; //auf ungefeahren Maximalwert begrenzen
+                                        gpsintmax = (GPS_NICKROLL_MAX * GPS_V * GPS_USR_PAR_FKT * ((32*3)/10))/(long)Parameter_UserParam2; //auf ungefeahren Maximalwert begrenzen
                                         if ((abs(int_east) > (int)gpsintmax) || (abs(int_north)> (int)gpsintmax))
                                         {
-                                                int_ovfl_cnt += 1; // Zahl der Overflows zaehlen
-                                        }
-                                        if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren
-                                        {
+//                                              //      = 1; // Zahl der Overflows zaehlen
-                                                int_ovfl_cnt    -= 1;
+//                                              int_ovfl_cnt    -= 1;
-                                                int_east                = (int_east*7)/8; // Wert reduzieren
+                                                int_east        = (int_east * 6)/8; // Wert reduzieren
-                                                int_north       = (int_north*7)/8;
+                                                int_north       = (int_north* 6)/8;
+                                        }
                                         if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten
                                         {
-                                                int_east        = 0;
+                                                int_east        = 0;
                                                 int_north       = 0;
+                                        }
+                                }
                                 else  // Integrator deaktiviert
+                                {
                                         int_east  = 0;
                                         int_north = 0;
+                                }
-                                debug_gp_4      = (int)int_east;        // zum Debuggen
+                                debug_gp_4      = (int)int_east;  // zum Debuggen
                                 debug_gp_5      = (int)int_north; // zum Debuggen
 //                              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)/(20);
+                                delta_east      = (delta_east   * (long)diff_p)/(40);
-                                delta_north     = (delta_north  * (long)diff_p)/(20);
+                                delta_north     = (delta_north  * (long)diff_p)/(40);
                                 if (hold_fast > 0)  //schneller Coming Home Modus
+                                {
                                         // P Werte begrenzen
-                                        #define P1_F_MAX (GPS_NICKROLL_MAX * GPS_V*8)/10 // auf 80 Prozent des Maximalen Nick/Rollwert begrenzen
+                                        #define P1_F_MAX (GPS_NICKROLL_MAX * GPS_V*7)/10 // auf 80 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;

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/GPS.c @ 704 - Rev 677 → 698