Subversion Repositories FlightCtrl

Rev

Rev 1539 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1538 killagreg 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) Holger Buss, Ingo Busker
3
// + Nur für den privaten Gebrauch
4
// + www.MikroKopter.com
5
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
8
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
9
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
10
// + bzgl. der Nutzungsbedingungen aufzunehmen.
11
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
12
// + Verkauf von Luftbildaufnahmen, usw.
13
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
15
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
16
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
18
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
19
// + eindeutig als Ursprung verlinkt werden
20
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
22
// + Benutzung auf eigene Gefahr
23
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
24
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
25
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
26
// + mit unserer Zustimmung zulässig
27
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
29
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
31
// + this list of conditions and the following disclaimer.
32
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
33
// +     from this software without specific prior written permission.
34
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
35
// +     for non-commercial use (directly or indirectly)
36
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
37
// +     with our written permission
38
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
39
// +     clearly linked as origin
40
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
41
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
42
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
43
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
44
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
45
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
46
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
47
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
48
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
49
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
50
// +  POSSIBILITY OF SUCH DAMAGE.
51
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
52
#include <inttypes.h>
53
#include "ubx.h"
54
#include "main.h"
55
#include <avr/io.h>
56
 
57
//#include "uart0.h"
58
 
59
// ubx protocol parser state machine
60
#define UBXSTATE_IDLE   0
61
#define UBXSTATE_SYNC1  1
62
#define UBXSTATE_SYNC2  2
63
#define UBXSTATE_CLASS  3
64
#define UBXSTATE_LEN1   4
65
#define UBXSTATE_LEN2   5
66
#define UBXSTATE_DATA   6
67
#define UBXSTATE_CKA    7
68
#define UBXSTATE_CKB    8
69
 
70
// ublox protocoll identifier
71
#define UBX_CLASS_NAV   0x01
72
 
73
#define UBX_ID_POSLLH   0x02
74
#define UBX_ID_SOL              0x06
75
#define UBX_ID_VELNED   0x12
76
 
77
#define UBX_SYNC1_CHAR  0xB5
78
#define UBX_SYNC2_CHAR  0x62
79
 
80
typedef struct {
81
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
82
        int32_t                 Frac;           // ns remainder of rounded ms above
83
        int16_t                 week;           // GPS week
84
        uint8_t                 GPSfix;         // GPSfix Type, range 0..6
85
        uint8_t                 Flags;          // Navigation Status Flags
86
        int32_t                 ECEF_X;         // cm ECEF X coordinate
87
        int32_t                 ECEF_Y;         // cm ECEF Y coordinate
88
        int32_t                 ECEF_Z;         // cm ECEF Z coordinate
89
        uint32_t                PAcc;           // cm 3D Position Accuracy Estimate
90
        int32_t                 ECEFVX;         // cm/s ECEF X velocity
91
        int32_t                 ECEFVY;         // cm/s ECEF Y velocity
92
        int32_t                 ECEFVZ;         // cm/s ECEF Z velocity
93
        uint32_t                SAcc;           // cm/s Speed Accuracy Estimate
94
        uint16_t                PDOP;           // 0.01 Position DOP
95
        uint8_t                 res1;           // reserved
96
        uint8_t                 numSV;          // Number of SVs used in navigation solution
97
        uint32_t                res2;           // reserved
98
        Status_t                Status;
99
} UBX_SOL_t;
100
 
101
typedef struct {
102
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
103
        int32_t                 LON;            // 1e-07 deg Longitude
104
        int32_t                 LAT;            // 1e-07 deg Latitude
105
        int32_t                 HEIGHT;         // mm Height above Ellipsoid
106
        int32_t                 HMSL;           // mm Height above mean sea level
107
        uint32_t                Hacc;           // mm Horizontal Accuracy Estimate
108
        uint32_t                Vacc;           // mm Vertical Accuracy Estimate
109
        Status_t                Status;
110
} UBX_POSLLH_t;
111
 
112
typedef struct {
113
        uint32_t                ITOW;           // ms  GPS Millisecond Time of Week
114
        int32_t                 VEL_N;          // cm/s  NED north velocity
115
        int32_t                 VEL_E;          // cm/s  NED east velocity
116
        int32_t                 VEL_D;          // cm/s  NED down velocity
117
        uint32_t                Speed;          // cm/s  Speed (3-D)
118
        uint32_t                GSpeed;         // cm/s  Ground Speed (2-D)
119
        int32_t                 Heading;        // 1e-05 deg  Heading 2-D
120
        uint32_t                SAcc;           // cm/s  Speed Accuracy Estimate
121
        uint32_t                CAcc;           // deg  Course / Heading Accuracy Estimate
122
        Status_t                Status;
123
} UBX_VELNED_t;
124
 
125
UBX_SOL_t               UbxSol    = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
126
UBX_POSLLH_t    UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
127
UBX_VELNED_t    UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
128
GPS_INFO_t      GPSInfo   = {0,0,0,0,0,0,0,0,0,0, INVALID};
129
 
130
volatile uint8_t GPSTimeout = 0;
131
 
132
void UpdateGPSInfo (void)
133
{
134
 
135
        if ((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))
136
        {
137
                RED_FLASH;
138
                if(GPSInfo.status != NEWDATA)
139
                {
140
                        GPSInfo.status = INVALID;
141
                        // NAV SOL
142
                        GPSInfo.flags = UbxSol.Flags;
143
                        GPSInfo.satfix = UbxSol.GPSfix;
144
                        GPSInfo.satnum = UbxSol.numSV;
145
                        GPSInfo.PAcc = UbxSol.PAcc;
146
                        GPSInfo.VAcc = UbxSol.SAcc;
147
                        // NAV POSLLH
148
                        GPSInfo.longitude = UbxPosLlh.LON;
149
                        GPSInfo.latitude = UbxPosLlh.LAT;
150
                        GPSInfo.altitude = UbxPosLlh.HEIGHT;
151
 
152
                        GPSInfo.veleast = UbxVelNed.VEL_E;
153
                        GPSInfo.velnorth = UbxVelNed.VEL_N;
154
                        GPSInfo.veltop = -UbxVelNed.VEL_D;
155
                        GPSInfo.velground = UbxVelNed.GSpeed;
156
 
157
                        GPSInfo.status = NEWDATA;
158
 
159
                }
160
                // set state to collect new data
161
                UbxSol.Status = PROCESSED;                      // never update old data
162
                UbxPosLlh.Status = PROCESSED;           // never update old data
163
                UbxVelNed.Status = PROCESSED;           // never update old data
164
        }
165
 
166
 
167
}
168
 
169
 
170
// this function should be called within the UART RX ISR
171
void ubx_parser(uint8_t c)
172
{
173
        static uint8_t ubxstate = UBXSTATE_IDLE;
174
        static uint8_t cka, ckb;
175
        static uint16_t msglen;
176
        static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered
177
 
178
        switch(ubxstate)
179
        {
180
                case UBXSTATE_IDLE: // check 1st sync byte
181
                        if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1;
182
                        else ubxstate = UBXSTATE_IDLE; // out of synchronization
183
                        break;
184
 
185
                case UBXSTATE_SYNC1: // check 2nd sync byte
186
                        if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2;
187
                        else ubxstate = UBXSTATE_IDLE; // out of synchronization
188
                        break;
189
 
190
                case UBXSTATE_SYNC2: // check msg class to be NAV
191
                        if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS;
192
                        else ubxstate = UBXSTATE_IDLE; // unsupported message class
193
                        break;
194
 
195
                case UBXSTATE_CLASS: // check message identifier
196
                        switch(c)
197
                        {
198
                                case UBX_ID_POSLLH: // geodetic position
199
                                        ubxP =  (int8_t *)&UbxPosLlh; // data start pointer
200
                                        ubxEp = (int8_t *)(&UbxPosLlh + 1); // data end pointer
201
                                        ubxSp = (int8_t *)&UbxPosLlh.Status; // status pointer
202
                                        break;
203
 
204
                                case UBX_ID_SOL: // navigation solution
205
                                        ubxP =  (int8_t *)&UbxSol; // data start pointer
206
                                        ubxEp = (int8_t *)(&UbxSol + 1); // data end pointer
207
                                        ubxSp = (int8_t *)&UbxSol.Status; // status pointer
208
                                        break;
209
 
210
                                case UBX_ID_VELNED: // velocity vector in tangent plane
211
                                        ubxP =  (int8_t *)&UbxVelNed; // data start pointer
212
                                        ubxEp = (int8_t *)(&UbxVelNed + 1); // data end pointer
213
                                        ubxSp = (int8_t *)&UbxVelNed.Status; // status pointer
214
                                        break;
215
 
216
                                default:                        // unsupported identifier
217
                                        ubxstate = UBXSTATE_IDLE;
218
                                        break;
219
                        }
220
                        if (ubxstate != UBXSTATE_IDLE)
221
                        {
222
                                ubxstate = UBXSTATE_LEN1;
223
                                cka = UBX_CLASS_NAV + c;
224
                                ckb = UBX_CLASS_NAV + cka;
225
                        }
226
                        break;
227
 
228
                case UBXSTATE_LEN1: // 1st message length byte
229
                        msglen = c;
230
                        cka += c;
231
                        ckb += cka;
232
                        ubxstate = UBXSTATE_LEN2;
233
                        break;
234
 
235
                case UBXSTATE_LEN2: // 2nd message length byte
236
                        msglen += ((uint16_t)c)<<8;
237
                        cka += c;
238
                        ckb += cka;
239
                        // if the old data are not processed so far then break parsing now
240
                        // to avoid writing new data in ISR during reading by another function
241
                        if ( *ubxSp == NEWDATA )
242
                        {
243
                                UpdateGPSInfo(); //update GPS info respectively
244
                                ubxstate = UBXSTATE_IDLE;
245
                        }
246
                        else // data invalid or allready processd
247
                        {
248
                                *ubxSp = INVALID;
249
                                ubxstate = UBXSTATE_DATA;
250
                        }
251
                        break;
252
 
253
                case UBXSTATE_DATA:
254
                        if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left
255
                        cka += c;
256
                        ckb += cka;
257
                        if (--msglen == 0)      ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
258
                        break;
259
 
260
                case UBXSTATE_CKA:
261
                        if (c == cka) ubxstate = UBXSTATE_CKB;
262
                        else
263
                        {
264
                                *ubxSp = INVALID;
265
                                ubxstate = UBXSTATE_IDLE;
266
                        }
267
                        break;
268
 
269
                case UBXSTATE_CKB:
270
                        if (c == ckb)
271
                        {
272
                                *ubxSp = NEWDATA; // new data are valid
273
                                UpdateGPSInfo(); //update GPS info respectively
274
                                GPSTimeout = 255;
275
                        }
276
                        else
277
                        {       // if checksum not fit then set data invalid
278
                                *ubxSp = INVALID;
279
                        }
280
                        ubxstate = UBXSTATE_IDLE; // ready to parse new data
281
                        break;
282
 
283
                default: // unknown ubx state
284
                        ubxstate = UBXSTATE_IDLE;
285
                        break;
286
        }
287
 
288
}
289
 
290