WebSVN - FlightCtrl - Diff - Rev 626 and 628 - /branches/salvo_gps/Basis

 /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Peter Muehlenbrock
 Auswertung der Daten vom GPS im ublox Format
 Hold Modus mit PID Regler
 Rückstuerz zur Basis Funktion
-Stand 7.1.2008
+Stand 8.1.2008
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 */
 #include "main.h"
 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                          uint8_t *ptr_payload_data;
 static                          uint8_t *ptr_pac_status;
-static long int         dist_flown;
+static  int                     dist_flown;
 static unsigned int int_ovfl_cnt; // Zaehler fuer Overflows des Integrators
 GPS_ABS_POSITION_t              gps_act_position;               // Alle wichtigen Daten zusammengefasst
 GPS_ABS_POSITION_t              gps_home_position;      // Die Startposition, beim Kalibrieren ermittelt
 GPS_REL_POSITION_t              gps_rel_act_position;   // Die aktuelle relative Position bezogen auf Home Position
 GPS_REL_POSITION_t              gps_rel_hold_position;  // Die gespeicherte Sollposition fuer GPS_ Hold Mode
-GPS_REL_POSITION_t              gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
+static GPS_REL_POSITION_t               gps_rel_start_position; // Die gespeicherte Ausgangsposition fuer GPS_ Home Mode
 // Initialisierung
 void GPS_Neutral(void)
                 case GPS_CMD_REQ_HOLD: // Die Lageregelung soll aktiviert werden.
                         if (gps_state != GPS_CRTL_HOLD_ACTIVE)
+                        {
                                 cnt++;
-                                if (cnt > 500) // erst nach Verzoegerung
+                                if (cnt > 700) // erst nach Verzoegerung
+                                {
                                         cnt     =       0;
                                         // aktuelle positionsdaten abspeichern
                                         if (gps_rel_act_position.status > 0)
+                                {
                                         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
+                                        d3      = (dist_2home - dist_flown); // Restdistanz zum Ziel
+                                        debug_gp_2      = dist_2home;  // zum Debuggen
+                                        debug_gp_3      = dist_flown; // 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
+                                        {
                                                 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) gps_g2t_act_v++;    //Geschwindigkeit langsam erhoehen
+                                                        if (gps_g2t_act_v < GPS_G2T_V_MAX-2) gps_g2t_act_v +=2;    //Geschwindigkeit langsam erhoehen
-                                                        dist_flown              +=(long)gps_g2t_act_v; // Vorgabe der Strecke anhand der Geschwindigkeit
+                                                        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                                  = 0; // Integrator einsschalten
                                                 hold_fast                                               = 1; // Regler fuer schnellen Flug
-                                                dist_frm_start_east                             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
+                                                dist_frm_start_east                             = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000);
-                                                dist_frm_start_north                    = (int)((dist_flown * (long)cos_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
                                                 gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
+                                        }
                                         else if (d3 > GPS_G2T_DIST_HOLD)   //Das Ziel naehert sich, deswegen abbremsen
                                                         dist_flown++; //Auch ausserhalb der Toleranz langsam erhoehen
                                                         gps_sub_state   = GPS_HOME_RMPDWN_OUTOF_TOL;
+                                                }
                                                 hold_reset_int                                  = 0; // Integrator ausschalten
                                                 hold_fast                                               = 1; // Wieder normal regeln
-                                                dist_frm_start_east                             = (int)((dist_flown * (long)sin_i(hdng_2home))/1000);
+                                                dist_frm_start_east                             = (int)(((long)dist_flown * (long)sin_i(hdng_2home))/1000);
-                                                dist_frm_start_north                    = (int)((dist_flown * (long)cos_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
                                                 gps_rel_hold_position.utm_north = gps_rel_start_position.utm_north + dist_frm_start_north; //naechster Zielpunkt
+                                        }
                                         else  //Soll-Ziel fast erreicht, Jetzt noch Reste ausgleichen, weil Zielpunkt nicht exakt bestimmt werden konnte (Fehler in Winkelfkt)
+                                        {
                                 #define GPSINT_MAX 3000 // Neues Verfahren ab  30.12.2007  bei Integratoroverflow
                                 if ((abs(int_east) > GPSINT_MAX) || (abs(int_north)> GPSINT_MAX))
+                                {
-                                        if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen
-//                                      int_east        -= dist_east; auf alten Wert halten
-//                                      int_north   -= dist_north;
+                                        if (int_ovfl_cnt < 40) int_ovfl_cnt += 1; // Zahl der Overflows zaehlen
+                                }
                                 if (int_ovfl_cnt > 0) //bei Overflow Wert Integratorwert reduzieren
+                                {
                                         int_ovfl_cnt    -= 1;
                                 if (hold_reset_int > 0)  //Im Schnellen Mode Integrator abschalten
+                                {
                                         int_east        = 0;
                                         int_north       = 0;
+                                }
+                                if (hold_fast > 0)  //schneller Coming Home Modus
+                                {
+                                        amplfy_speed_east  = (Parameter_UserParam3/GPS_USR_PAR_FKT);
+                                        amplfy_speed_north = (Parameter_UserParam3/GPS_USR_PAR_FKT);
+                                }
+                                else  //langsamer Holdmodus
+                                {
+                                        // Verstaerkungsfaktor fuer Geschwindigkeit ermitteln um exponentielles Verhalten zu erzielen
+                                        amplfy_speed_east  = (((abs(speed_east)) *(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10;
+                                        amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10;
+                                        if (amplfy_speed_east  > (DIFF_Y_MAX *10)) amplfy_speed_east  = DIFF_Y_MAX *10; // Begrenzung
+                                        if (amplfy_speed_north > (DIFF_Y_MAX *10)) amplfy_speed_north = DIFF_Y_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
+                                }
-                                // Verstaerkungsfaktor fuer Geschwindigkeit ermitteln um exponentielles Verhalten zu erzielen
-                                amplfy_speed_east  = (((abs(speed_east)) *(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10;
-                                amplfy_speed_north = (((abs(speed_north))*(DIFF_Y_MAX-1)*10)/DIFF_X_MAX) +10;
-                                if (amplfy_speed_east  > (DIFF_Y_MAX *10)) amplfy_speed_east  = DIFF_Y_MAX *10; // Begrenzung
-                                if (amplfy_speed_north > (DIFF_Y_MAX *10)) amplfy_speed_north = DIFF_Y_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
                                 #define GPS_SPEED_SCALE 5
                                 speed_east  /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren
-                                speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren
-                                debug_gp_2      = speed_east;  // zum Debuggen
-                                debug_gp_3      = speed_north; // zum Debuggen
-                                debug_gp_4      = amplfy_speed_east;  // zum Debuggen
-                                debug_gp_5      = amplfy_speed_north; // zum Debuggen
+                                speed_north /= GPS_SPEED_SCALE; // Wegen overflow Gefahr Wert jetzt schon runterskalieren
+                                }
                                 GPS_dist_2trgt  = (int) dev;
                                 // Winkel und Distanz in Nick und Rollgroessen umrechnen
                                 n  = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000); //Rollwert bestimmen
                                 n1 = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000); //Nickwert bestimmen
-//                              GPS_Roll = (int) +( (dev * (long) sin_i(GPS_hdng_rel_2trgt))/1000);
-//                              GPS_Nick = (int) -( (dev * (long) cos_i(GPS_hdng_rel_2trgt))/1000);
                                 if (n > (GPS_NICKROLL_MAX * GPS_V)) n = (GPS_NICKROLL_MAX * GPS_V);
                                 else if (n < -(GPS_NICKROLL_MAX * GPS_V)) n = -(GPS_NICKROLL_MAX * GPS_V);
                                 if (n1 > (GPS_NICKROLL_MAX * GPS_V)) n1 = (GPS_NICKROLL_MAX * GPS_V);
 /*                              n                       = sin_i((GPS_Roll*90)/(GPS_NICKROLL_MAX * GPS_V));
                                 n_l                     = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
                                 GPS_Roll        = (int) n_l;
                                 n                       = sin_i((GPS_Nick*90)/(GPS_NICKROLL_MAX * GPS_V));
                                 n_l                     = ((long) GPS_NICKROLL_MAX  * (long) n)/1000;
-                                GPS_Nick        = (int) n_l;
+                                GPS_Nick        = (int) n_l;*/
-*/
                                 if ((abs(dist_east) > GPS_DIST_MAX) || (abs(dist_north) > GPS_DIST_MAX))  // bei zu grossem Abstand abbrechen
+                                {
                                         GPS_Roll        = 0;

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/Basis_v0067g/trunk/GPS.c @ 629 - Rev 626 → 628