WebSVN - FlightCtrl - Diff - Rev 153 and 156

 */
 /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 Peter Muehlenbrock
 Auswertung der Daten vom GPS im ublox Format
 Regelung fuer GPS noch nicht implementiert
-Stand 11.9.2007
+Stand 14.9.2007
 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 */
 #include "main.h"
+//#include "gps.h"
 // Defines fuer ublox Messageformat um Auswertung zu steuern
 #define                 UBLOX_IDLE      0
 #define                 UBLOX_SYNC1     1
 static unsigned int ptr_payload_data_end;
 static uint8_t *ptr_payload_data;
+static uint8_t *ptr_pac_status;
 static uint8_t *ptr_pac_status;
 short int Get_GPS_data(void);
 NAV_POSUTM_t actual_pos;    // Aktuelle Nav Daten werden hier im ublox Format abgelegt
+NAV_STATUS_t actual_status; // Aktueller Nav Status
+NAV_VELNED_t actual_speed;  // Aktueller Geschwindigkeits und Richtungsdaten
-NAV_STATUS_t actual_status; // Aktueller Nav Status
+GPS_ABS_POSITION_t              gps_act_position;               // Alle wichtigen Daten zusammengefasst
-NAV_VELNED_t actual_speed;  // Aktueller Geschwindigkeits und Richtungsdaten
+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_POSITION_t gps_act_position;  // Alle wichtigen Daten zusammengefasst
+// Initialisierung
-void GPS_Neutral(void) // Initialisierung
+void GPS_Neutral(void)
+{
         ublox_msg_state                 = UBLOX_IDLE;
-        ublox_msg_state = UBLOX_IDLE;
+        actual_pos.status               = 0;
+        actual_speed.status             = 0;
-        actual_pos.status= 0;
+        actual_status.status    = 0;
+        gps_home_position.status= 0; // Noch keine gueltige Home Position
+}
+// Home Position sichern falls Daten verfuegbar sind.
+void GPS_Save_Home(void)
+{
+        short int n;
+        n = Get_GPS_data();
+        if (n == 0)   // Gueltige  und aktuelle Daten ?
+        {
+                // Neue GPS Daten liegen vor
+//                beeptime                                              = 500; // Piepsen um korrekte Home Position anzuzeigen
+//                gps_act_position.status               = 0;
+                  gps_home_position.utm_east    = gps_act_position.utm_east;
+                  gps_home_position.utm_north   = gps_act_position.utm_north;
+                  gps_home_position.utm_alt             = gps_act_position.utm_alt;
+                  gps_home_position.status              = 1; // Home Position gueltig
+        }
+}
+// Relative Position zur Home Position bestimmen
+// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
+short int Get_Rel_Position(void)
+{
+        short int n = 0;
+        n = Get_GPS_data();
+        if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
+        if  (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
+        {
+                gps_rel_act_position.utm_east   = (int)  (gps_act_position.utm_east - gps_home_position.utm_east);
+                gps_rel_act_position.utm_north  = (int)  (gps_act_position.utm_north - gps_home_position.utm_north);
+                gps_rel_act_position.status     = 1; // gueltige Positionsdaten
+                n = 0;
+        }
+        else
+        {
+                n = 2; //keine gueltigen Daten vorhanden
+                gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
+        }
+        return (n);
+}
-        actual_speed.status= 0;
+// Daten aus aktuellen ublox Messages extrahieren
-        actual_status.status= 0;
+// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
 short int Get_GPS_data(void)
+{
-void Get_GPS_data(void)  // Daten aus aktuellen ublox Messages extrahieren
+        short int n = 1;
                         gps_act_position.utm_alt        = actual_pos.utm_alt/10;
                         gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
                         gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
                         gps_act_position.heading        = actual_speed.heading/100000;
                         gps_act_position.status         = 1;
+                        n                                                       = 0; //Daten gueltig
+                }
+                else
+                {
+                        gps_act_position.status = 0; //Keine gueltigen Daten
+                        n                                               = 2;
+                }
-                        actual_pos.status               = 0;
+                actual_pos.status               = 0; //neue ublox Messages anfordern
-                        actual_status.status    = 0;
+                actual_status.status    = 0;
-                        actual_speed.status     = 0;
+                actual_speed.status     = 0;
         }
+        return (n);
+}
 /*
 Daten vom GPS im ublox MSG Format auswerten
-Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul  durch den UART IRQ aufgerufen
+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
 */
                                                 ublox_msg_state                 = UBLOX_LEN1;
+                                        }
                                         break;
                                 case UBLOX_NAV_VELED:
-                                        ptr_pac_status  =       &actual_speed.status;
+                                        ptr_pac_status          =       &actual_speed.status;
                                         if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
                                         else
+                                        {
                                                 ptr_payload_data                = &actual_speed;
                                 chk_b   += chk_a;
                                 --rx_len;
                                 if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))
+                                {
                                         ptr_payload_data++;
                                         ublox_msg_state = UBLOX_PAYLOAD;
+                            }
                                 else ublox_msg_state = UBLOX_CKA;
+                        }
                         else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
                         break;

Subversion Repositories FlightCtrl

(root)/branches/salvo_gps/GPS.c @ 156 - Rev 153 → 156