Subversion Repositories FlightCtrl

Rev

Rev 153 | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 153 Rev 156
Line 11... Line 11...
11 
*/
 11 
*/
12 
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 12 
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13 
Peter Muehlenbrock
 13 
Peter Muehlenbrock
14 
Auswertung der Daten vom GPS im ublox Format
 14 
Auswertung der Daten vom GPS im ublox Format
15 
Regelung fuer GPS noch nicht implementiert
 15 
Regelung fuer GPS noch nicht implementiert
16 
Stand 11.9.2007
 16 
Stand 14.9.2007
17 
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 17 
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18 
*/
 18 
*/
19 
#include "main.h"
 19 
#include "main.h"
- 
 
 20 
//#include "gps.h"
Line 20... Line 21...
20 
 
 21 
 
21 
// Defines fuer ublox Messageformat um Auswertung zu steuern
 22 
// Defines fuer ublox Messageformat um Auswertung zu steuern
22 
#define                 UBLOX_IDLE      0
 23 
#define                 UBLOX_IDLE      0
23 
#define                 UBLOX_SYNC1     1
 24 
#define                 UBLOX_SYNC1     1
Line 49... Line 50...
49 
static unsigned int ptr_payload_data_end;
 50 
static unsigned int ptr_payload_data_end;
Line 50... Line 51...
50 
 
 51 
 
51 
static uint8_t *ptr_payload_data;
 52 
static uint8_t *ptr_payload_data;
Line -... Line 53...
- 
 
 53 
static uint8_t *ptr_pac_status;
Line 52... Line 54...
52 
static uint8_t *ptr_pac_status;
 54 
 
53 
 
 55 
short int Get_GPS_data(void);
54 
 
 56 
 
Line 55... Line 57...
55 
NAV_POSUTM_t actual_pos;    // Aktuelle Nav Daten werden hier im ublox Format abgelegt
 57 
NAV_POSUTM_t actual_pos;    // Aktuelle Nav Daten werden hier im ublox Format abgelegt
- 
 
 58 
NAV_STATUS_t actual_status; // Aktueller Nav Status
- 
 
 59 
NAV_VELNED_t actual_speed;  // Aktueller Geschwindigkeits und Richtungsdaten
Line -... Line 60...
- 
 
 60 
 
56 
NAV_STATUS_t actual_status; // Aktueller Nav Status
 61 
GPS_ABS_POSITION_t              gps_act_position;               // Alle wichtigen Daten zusammengefasst
57 
NAV_VELNED_t actual_speed;  // Aktueller Geschwindigkeits und Richtungsdaten
 62 
GPS_ABS_POSITION_t              gps_home_position;      // Die Startposition, beim Kalibrieren ermittelt
58 
 
 63 
GPS_REL_POSITION_t              gps_rel_act_position;   // Die aktuelle relative Position bezogen auf Home Position
59 
GPS_POSITION_t gps_act_position;  // Alle wichtigen Daten zusammengefasst
 64 
 
60 
 
 65 
// Initialisierung
61 
void GPS_Neutral(void) // Initialisierung
 66 
void GPS_Neutral(void)
- 
 
 67 
{
62 
{
 68 
        ublox_msg_state                 = UBLOX_IDLE;
Line 63... Line 69...
63 
        ublox_msg_state = UBLOX_IDLE;
 69 
        actual_pos.status               = 0;
- 
 
 70 
        actual_speed.status             = 0;
64 
        actual_pos.status= 0;
 71 
        actual_status.status    = 0;
- 
 
 72 
        gps_home_position.status= 0; // Noch keine gueltige Home Position
- 
 
 73 
}
- 
 
 74 
 
- 
 
 75 
// Home Position sichern falls Daten verfuegbar sind.
- 
 
 76 
void GPS_Save_Home(void)
- 
 
 77 
{
- 
 
 78 
        short int n;
- 
 
 79 
        n = Get_GPS_data();
- 
 
 80 
        if (n == 0)   // Gueltige  und aktuelle Daten ?  
- 
 
 81 
        {
- 
 
 82 
                // Neue GPS Daten liegen vor
- 
 
 83 
//                beeptime                                              = 500; // Piepsen um korrekte Home Position anzuzeigen
- 
 
 84 
//                gps_act_position.status               = 0;
- 
 
 85 
                  gps_home_position.utm_east    = gps_act_position.utm_east;   
- 
 
 86 
                  gps_home_position.utm_north   = gps_act_position.utm_north;  
- 
 
 87 
                  gps_home_position.utm_alt             = gps_act_position.utm_alt;
- 
 
 88 
                  gps_home_position.status              = 1; // Home Position gueltig                  
- 
 
 89 
        }
- 
 
 90 
}
- 
 
 91 
 
- 
 
 92 
// Relative Position zur Home Position bestimmen
- 
 
 93 
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
- 
 
 94 
short int Get_Rel_Position(void)
- 
 
 95 
{
- 
 
 96 
        short int n = 0;
- 
 
 97 
        n = Get_GPS_data();
- 
 
 98 
        if (n >=1) return (n); // nix zu tun, weil keine neue Daten da sind
- 
 
 99 
        if  (gps_home_position.status > 0) //Nur wenn Home Position vorliegt
- 
 
 100 
        {
- 
 
 101 
                gps_rel_act_position.utm_east   = (int)  (gps_act_position.utm_east - gps_home_position.utm_east);
- 
 
 102 
                gps_rel_act_position.utm_north  = (int)  (gps_act_position.utm_north - gps_home_position.utm_north);
- 
 
 103 
                gps_rel_act_position.status     = 1; // gueltige Positionsdaten
- 
 
 104 
                n = 0;
- 
 
 105 
        }
- 
 
 106 
        else
- 
 
 107 
        {
- 
 
 108 
                n = 2; //keine gueltigen Daten vorhanden
- 
 
 109 
                gps_rel_act_position.status = 0; //keine gueltige Position weil keine home Position da ist.
- 
 
 110 
        }      
- 
 
 111 
        return (n);
- 
 
 112 
}
- 
 
 113 
 
65 
        actual_speed.status= 0;
 114 
// Daten aus aktuellen ublox Messages extrahieren
66 
        actual_status.status= 0;
 115 
// Rueckgabewert 0= Daten sind aktuell und gueltig. 1= Keine Aenderung. 2= Daten ungueltig
67 
}
 116 
short int Get_GPS_data(void)
68 
 
 117 
{
69 
void Get_GPS_data(void)  // Daten aus aktuellen ublox Messages extrahieren
118 
        short int n = 1;
70 
{
 119 
 
Line 79... Line 128...
79 
                        gps_act_position.utm_alt        = actual_pos.utm_alt/10;
 128 
                        gps_act_position.utm_alt        = actual_pos.utm_alt/10;
80 
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
 129 
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
81 
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
 130 
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
82 
                        gps_act_position.heading        = actual_speed.heading/100000;
 131 
                        gps_act_position.heading        = actual_speed.heading/100000;
83 
                        gps_act_position.status         = 1;
 132 
                        gps_act_position.status         = 1;
- 
 
 133 
                        n                                                       = 0; //Daten gueltig
84 
                }
 134 
                }
- 
 
 135 
                else
- 
 
 136 
                {
- 
 
 137 
                        gps_act_position.status = 0; //Keine gueltigen Daten
- 
 
 138 
                        n                                               = 2;
- 
 
 139 
                }
85 
                        actual_pos.status               = 0;
 140 
                actual_pos.status               = 0; //neue ublox Messages anfordern
86 
                        actual_status.status    = 0;
 141 
                actual_status.status    = 0;
87 
                        actual_speed.status     = 0;
 142 
                actual_speed.status     = 0;
88 
        }              
 143 
        }      
- 
 
 144 
        return (n);    
89 
}
 145 
}
Line 90... Line 146...
90 
 
 146 
 
91 
 
 147