Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
143 salvo 1
/*
2
This program (files gps.c and gps.h) is free software; you can redistribute it and/or modify
3
it under the terms of the GNU General Public License as published by the Free Software Foundation;
4
either version 3 of the License, or (at your option) any later version.  
5
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
6
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
7
GNU General Public License for more details. You should have received a copy of the GNU General Public License
8
along with this program. If not, see <http://www.gnu.org/licenses/>.
9
 
149 salvo 10
Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de
143 salvo 11
*/
12
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
Peter Muehlenbrock
14
Auswertung der Daten vom GPS im ublox Format
15
Regelung fuer GPS noch nicht implementiert
149 salvo 16
Stand 11.9.2007
143 salvo 17
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18
*/
1 ingob 19
#include "main.h"
20
 
143 salvo 21
// Defines fuer ublox Messageformat um Auswertung zu steuern
22
#define                 UBLOX_IDLE      0
23
#define                 UBLOX_SYNC1     1
24
#define                 UBLOX_SYNC2     2
25
#define                 UBLOX_CLASS     3
26
#define                 UBLOX_ID        4
27
#define                 UBLOX_LEN1      5
28
#define                 UBLOX_LEN2      6
29
#define                 UBLOX_CKA       7
30
#define                 UBLOX_CKB       8
31
#define                 UBLOX_PAYLOAD   9
32
 
33
// ublox Protokoll Identifier 
34
#define                 UBLOX_NAV_POSUTM                0x08
35
#define                 UBLOX_NAV_STATUS                0x03
36
#define                 UBLOX_NAV_VELED                 0x12
37
#define                 UBLOX_NAV_CLASS                 0x01
38
#define                 UBLOX_SYNCH1_CHAR               0xB5
39
#define                 UBLOX_SYNCH2_CHAR               0x62
40
 
1 ingob 41
signed int GPS_Nick = 0;
42
signed int GPS_Roll = 0;
143 salvo 43
static uint8_t ublox_msg_state = UBLOX_IDLE;
44
static uint8_t  chk_a =0; //Checksum
45
static uint8_t  chk_b =0;
46
 
47
static unsigned int rx_len;
48
unsigned int cnt0,cnt1,cnt2; //******Provisorisch
49
static unsigned int ptr_payload_data_end;
50
 
51
static uint8_t *ptr_payload_data;
52
static uint8_t *ptr_pac_status;
53
 
54
 
55
NAV_POSUTM_t actual_pos;    // Aktuelle Nav Daten werden hier im ublox Format abgelegt
56
NAV_STATUS_t actual_status; // Aktueller Nav Status
57
NAV_VELNED_t actual_speed;  // Aktueller Geschwindigkeits und Richtungsdaten
58
 
59
GPS_POSITION_t gps_act_position;  // Alle wichtigen Daten zusammengefasst
60
 
61
void GPS_Neutral(void) // Initialisierung
1 ingob 62
{
143 salvo 63
        ublox_msg_state = UBLOX_IDLE;
64
        actual_pos.status= 0;
65
        actual_speed.status= 0;
66
        actual_status.status= 0;
1 ingob 67
}
68
 
143 salvo 69
void Get_GPS_data(void)  // Daten aus aktuellen ublox Messages extrahieren 
1 ingob 70
{
143 salvo 71
        if (actual_pos.status == 0) return; //damit es schnell geht, wenn nix zu tun ist
72
        if ((actual_pos.status > 0) && (actual_status.status > 0) && (actual_speed.status > 0))
73
        {
74
                cnt1++; //**** noch Rausschmeissen
149 salvo 75
                if (((actual_status.gpsfix_type & 0x03) >=2) && ((actual_status.nav_status_flag & 0x01) >=1)) // nur wenn Daten aktuell und gueltig sind
143 salvo 76
                {
77
                        gps_act_position.utm_east       = actual_pos.utm_east/10;
78
                        gps_act_position.utm_north      = actual_pos.utm_north/10;
79
                        gps_act_position.utm_alt        = actual_pos.utm_alt/10;
80
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
81
                        gps_act_position.speed_gnd      = actual_speed.speed_gnd/10;
82
                        gps_act_position.heading        = actual_speed.heading/100000;
83
                        gps_act_position.status         = 1;
84
                }
85
                        actual_pos.status               = 0;
86
                        actual_status.status    = 0;
87
                        actual_speed.status     = 0;
88
        }              
1 ingob 89
}
90
 
91
 
143 salvo 92
/*
93
Daten vom GPS im ublox MSG Format auswerten
94
Die Routine wird bei jedem Empfang eines Zeichens vom GPS Modul  durch den UART IRQ aufgerufen
95
// Die UBX Messages NAV_POSUTM, NAV_STATUS und NAV_VALED muessen aktiviert sein
96
*/
97
void Get_Ublox_Msg(uint8_t rx)
98
{
1 ingob 99
 
143 salvo 100
        switch (ublox_msg_state)
101
        {
1 ingob 102
 
143 salvo 103
                case UBLOX_IDLE: // Zuerst Synchcharacters pruefen
104
                        if ( rx == UBLOX_SYNCH1_CHAR ) ublox_msg_state = UBLOX_SYNC1;
105
                        else ublox_msg_state = UBLOX_IDLE;
106
                        break;
107
 
108
                case UBLOX_SYNC1:
109
 
110
                        if (rx == UBLOX_SYNCH2_CHAR) ublox_msg_state = UBLOX_SYNC2;
111
                        else ublox_msg_state = UBLOX_IDLE;
112
                        chk_a = 0,chk_b = 0;
113
                        break;
114
 
115
                case UBLOX_SYNC2:
116
                        if (rx == UBLOX_NAV_CLASS) ublox_msg_state = UBLOX_CLASS;      
117
                        else ublox_msg_state = UBLOX_IDLE;
118
                        break;
119
 
120
                case UBLOX_CLASS: // Nur NAV Meldungen auswerten
121
                        switch (rx)
122
                        {
123
                                case UBLOX_NAV_POSUTM:
124
                                        ptr_pac_status  =       &actual_pos.status;
125
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE; //Abbruch weil Daten noch nicht verwendet wurden
126
                                        else
127
                                        {
128
                                                ptr_payload_data                = &actual_pos;
129
                                                ptr_payload_data_end    = &actual_pos.status;
130
                                                ublox_msg_state                 = UBLOX_LEN1;
131
                                        }
132
                                        break;
133
 
134
                                case UBLOX_NAV_STATUS:
135
                                        ptr_pac_status  =       &actual_status.status;
136
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
137
                                        else
138
                                        {
139
                                                ptr_payload_data                = &actual_status;
140
                                                ptr_payload_data_end    = &actual_status.status;
141
                                                ublox_msg_state                 = UBLOX_LEN1;
142
                                        }
143
                                        break;
144
 
145
                                case UBLOX_NAV_VELED:
146
                                        ptr_pac_status  =       &actual_speed.status;
147
                                        if (*ptr_pac_status > 0) ublox_msg_state = UBLOX_IDLE;
148
                                        else
149
                                        {
150
                                                ptr_payload_data                = &actual_speed;
151
                                                ptr_payload_data_end    = &actual_speed.status;
152
                                                ublox_msg_state                 = UBLOX_LEN1;
153
                                        }
154
                                        break;
155
 
156
                                default:
157
                                        ublox_msg_state = UBLOX_IDLE;
158
                                        break; 
159
                        }
160
                        chk_a   = UBLOX_NAV_CLASS + rx;
161
                        chk_b   = UBLOX_NAV_CLASS + chk_a;                     
162
                        break;
163
 
164
                case UBLOX_LEN1: // Laenge auswerten
165
                        rx_len  = rx;
166
                        chk_a   += rx;
167
                        chk_b   += chk_a;              
168
                        ublox_msg_state = UBLOX_LEN2;
169
                        break;
170
 
171
 
172
                case UBLOX_LEN2: // Laenge auswerten
173
                        rx_len = rx_len + (rx *256); // Laenge ermitteln
174
                        chk_a   += rx;
175
                        chk_b   += chk_a;      
176
                        ublox_msg_state = UBLOX_PAYLOAD;
177
                        break;
178
 
179
                case UBLOX_PAYLOAD: // jetzt Nutzdaten einlesen
180
                        if (rx_len > 0)
181
                        {
182
                                *ptr_payload_data = rx;
183
                                chk_a   += rx;
184
                                chk_b   += chk_a;
185
                                --rx_len;      
186
                                if ((rx_len > 0) && (ptr_payload_data <= ptr_payload_data_end))                          
187
                                {
188
                                        ptr_payload_data++;
189
                                        ublox_msg_state = UBLOX_PAYLOAD;
190
                            }
191
                                else ublox_msg_state = UBLOX_CKA;
192
                        }      
193
                        else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
194
                        break;
195
 
196
                case UBLOX_CKA: // Checksum pruefen
197
                        if (rx == chk_a) ublox_msg_state = UBLOX_CKB;                          
198
                        else ublox_msg_state = UBLOX_IDLE; // Abbruch wegen Fehler
199
                        break;
200
 
201
                case UBLOX_CKB: // Checksum pruefen
202
                        if (rx == chk_b) *ptr_pac_status = 1; // Paket ok
203
                        ublox_msg_state    = UBLOX_IDLE;
204
                        break;
205
 
206
                default:
207
                        ublox_msg_state = UBLOX_IDLE;          
208
                        break;
209
        }              
210
}
211