Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
886 killagreg 1
#include <inttypes.h>
2
#include "ubx.h"
3
#include "main.h"
4
#include <avr/io.h>
5
 
6
#include "uart.h"
7
 
8
// ubx protocol parser state machine
9
#define UBXSTATE_IDLE   0
10
#define UBXSTATE_SYNC1  1
11
#define UBXSTATE_SYNC2  2
12
#define UBXSTATE_CLASS  3
13
#define UBXSTATE_LEN1   4
14
#define UBXSTATE_LEN2   5
15
#define UBXSTATE_DATA   6
16
#define UBXSTATE_CKA    7
17
#define UBXSTATE_CKB    8
18
 
19
// ublox protocoll identifier
20
#define UBX_CLASS_NAV   0x01
21
 
22
#define UBX_ID_POSLLH   0x02
23
#define UBX_ID_SOL              0x06
24
#define UBX_ID_VELNED   0x12
25
 
26
#define UBX_SYNC1_CHAR  0xB5
27
#define UBX_SYNC2_CHAR  0x62
28
 
29
typedef struct {
30
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
31
        int32_t                 Frac;           // ns remainder of rounded ms above
32
        int16_t                 week;           // GPS week
33
        uint8_t                 GPSfix;         // GPSfix Type, range 0..6
34
        uint8_t                 Flags;          // Navigation Status Flags
35
        int32_t                 ECEF_X;         // cm ECEF X coordinate
36
        int32_t                 ECEF_Y;         // cm ECEF Y coordinate
37
        int32_t                 ECEF_Z;         // cm ECEF Z coordinate
38
        uint32_t                PAcc;           // cm 3D Position Accuracy Estimate
39
        int32_t                 ECEFVX;         // cm/s ECEF X velocity
40
        int32_t                 ECEFVY;         // cm/s ECEF Y velocity
41
        int32_t                 ECEFVZ;         // cm/s ECEF Z velocity
42
        uint32_t                SAcc;           // cm/s Speed Accuracy Estimate
43
        uint16_t                PDOP;           // 0.01 Position DOP
44
        uint8_t                 res1;           // reserved
45
        uint8_t                 numSV;          // Number of SVs used in navigation solution
46
        uint32_t                res2;           // reserved
936 killagreg 47
        Status_t                Status;
48
} UBX_SOL_t;
886 killagreg 49
 
50
typedef struct {
51
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
52
        int32_t                 LON;            // 1e-07 deg Longitude
53
        int32_t                 LAT;            // 1e-07 deg Latitude
54
        int32_t                 HEIGHT;         // mm Height above Ellipsoid
55
        int32_t                 HMSL;           // mm Height above mean sea level
56
        uint32_t                Hacc;           // mm Horizontal Accuracy Estimate
57
        uint32_t                Vacc;           // mm Vertical Accuracy Estimate
936 killagreg 58
        Status_t                Status;
59
} UBX_POSLLH_t;
886 killagreg 60
 
61
typedef struct {
62
        uint32_t                ITOW;           // ms  GPS Millisecond Time of Week
63
        int32_t                 VEL_N;          // cm/s  NED north velocity
64
        int32_t                 VEL_E;          // cm/s  NED east velocity
65
        int32_t                 VEL_D;          // cm/s  NED down velocity
66
        uint32_t                Speed;          // cm/s  Speed (3-D)
67
        uint32_t                GSpeed;         // cm/s  Ground Speed (2-D)
68
        int32_t                 Heading;        // 1e-05 deg  Heading 2-D
69
        uint32_t                SAcc;           // cm/s  Speed Accuracy Estimate
70
        uint32_t                CAcc;           // deg  Course / Heading Accuracy Estimate
936 killagreg 71
        Status_t                Status;
72
} UBX_VELNED_t;
886 killagreg 73
 
936 killagreg 74
UBX_SOL_t               UbxSol    = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
75
UBX_POSLLH_t    UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
76
UBX_VELNED_t    UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
886 killagreg 77
GPS_INFO_t      GPSInfo   = {0,0,0,0,0,0,0,0,0,0, INVALID};
78
 
79
volatile uint8_t GPSTimeout = 0;
80
 
81
void UpdateGPSInfo (void)
82
{
936 killagreg 83
 
84
        if ((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))
886 killagreg 85
        {
936 killagreg 86
                RED_FLASH;
87
                if(GPSInfo.status != NEWDATA)
88
                {
89
                        GPSInfo.status = INVALID;
90
                        // NAV SOL
91
                        GPSInfo.flags = UbxSol.Flags;
92
                        GPSInfo.satfix = UbxSol.GPSfix;
93
                        GPSInfo.satnum = UbxSol.numSV;
94
                        GPSInfo.PAcc = UbxSol.PAcc;
95
                        GPSInfo.VAcc = UbxSol.SAcc;
96
                        // NAV POSLLH
97
                        GPSInfo.longitude = UbxPosLlh.LON;
98
                        GPSInfo.latitude = UbxPosLlh.LAT;
99
                        GPSInfo.altitude = UbxPosLlh.HEIGHT;
100
 
101
                        GPSInfo.veleast = UbxVelNed.VEL_E;
102
                        GPSInfo.velnorth = UbxVelNed.VEL_N;
103
                        GPSInfo.veltop = -UbxVelNed.VEL_D;
104
                        GPSInfo.velground = UbxVelNed.GSpeed;
105
 
106
                        GPSInfo.status = NEWDATA;
107
 
108
                }
109
                // set state to collect new data
110
                UbxSol.Status = PROCESSED;                      // never update old data
111
                UbxPosLlh.Status = PROCESSED;           // never update old data
112
                UbxVelNed.Status = PROCESSED;           // never update old data
886 killagreg 113
        }
936 killagreg 114
 
115
 
886 killagreg 116
}
117
 
118
 
119
// this function should be called within the UART RX ISR
120
void ubx_parser(uint8_t c)
121
{
122
        static uint8_t ubxstate = UBXSTATE_IDLE;
123
        static uint8_t cka, ckb;
124
        static uint16_t msglen;
125
        static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered
126
 
127
        switch(ubxstate)
128
        {
129
                case UBXSTATE_IDLE: // check 1st sync byte
130
                        if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1;
131
                        else ubxstate = UBXSTATE_IDLE; // out of synchronization
132
                        break;
133
 
134
                case UBXSTATE_SYNC1: // check 2nd sync byte
135
                        if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2;
136
                        else ubxstate = UBXSTATE_IDLE; // out of synchronization
137
                        break;
138
 
139
                case UBXSTATE_SYNC2: // check msg class to be NAV
140
                        if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS;
141
                        else ubxstate = UBXSTATE_IDLE; // unsupported message class
142
                        break;
143
 
144
                case UBXSTATE_CLASS: // check message identifier
145
                        switch(c)
146
                        {
147
                                case UBX_ID_POSLLH: // geodetic position
936 killagreg 148
                                        ubxP =  (int8_t *)&UbxPosLlh; // data start pointer
149
                                        ubxEp = (int8_t *)(&UbxPosLlh + 1); // data end pointer
150
                                        ubxSp = (int8_t *)&UbxPosLlh.Status; // status pointer
886 killagreg 151
                                        break;
152
 
153
                                case UBX_ID_SOL: // navigation solution
936 killagreg 154
                                        ubxP =  (int8_t *)&UbxSol; // data start pointer
155
                                        ubxEp = (int8_t *)(&UbxSol + 1); // data end pointer
156
                                        ubxSp = (int8_t *)&UbxSol.Status; // status pointer
886 killagreg 157
                                        break;
158
 
159
                                case UBX_ID_VELNED: // velocity vector in tangent plane
936 killagreg 160
                                        ubxP =  (int8_t *)&UbxVelNed; // data start pointer
161
                                        ubxEp = (int8_t *)(&UbxVelNed + 1); // data end pointer
162
                                        ubxSp = (int8_t *)&UbxVelNed.Status; // status pointer
886 killagreg 163
                                        break;
164
 
165
                                default:                        // unsupported identifier
166
                                        ubxstate = UBXSTATE_IDLE;
167
                                        break;
168
                        }
169
                        if (ubxstate != UBXSTATE_IDLE)
170
                        {
171
                                ubxstate = UBXSTATE_LEN1;
172
                                cka = UBX_CLASS_NAV + c;
173
                                ckb = UBX_CLASS_NAV + cka;
174
                        }
175
                        break;
176
 
177
                case UBXSTATE_LEN1: // 1st message length byte
178
                        msglen = c;
179
                        cka += c;
180
                        ckb += cka;
181
                        ubxstate = UBXSTATE_LEN2;
182
                        break;
183
 
184
                case UBXSTATE_LEN2: // 2nd message length byte
185
                        msglen += ((uint16_t)c)<<8;
186
                        cka += c;
187
                        ckb += cka;
188
                        // if the old data are not processed so far then break parsing now
189
                        // to avoid writing new data in ISR during reading by another function
936 killagreg 190
                        if ( *ubxSp == NEWDATA )
911 killagreg 191
                        {
192
                                UpdateGPSInfo(); //update GPS info respectively
193
                                ubxstate = UBXSTATE_IDLE;
194
                        }
886 killagreg 195
                        else // data invalid or allready processd
196
                        {
197
                                *ubxSp = INVALID;
198
                                ubxstate = UBXSTATE_DATA;
199
                        }
200
                        break;
201
 
202
                case UBXSTATE_DATA:
203
                        if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left
204
                        cka += c;
205
                        ckb += cka;
206
                        if (--msglen == 0)      ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
207
                        break;
208
 
209
                case UBXSTATE_CKA:
210
                        if (c == cka) ubxstate = UBXSTATE_CKB;
211
                        else
212
                        {
213
                                *ubxSp = INVALID;
214
                                ubxstate = UBXSTATE_IDLE;
215
                        }
216
                        break;
217
 
218
                case UBXSTATE_CKB:
219
                        if (c == ckb)
220
                        {
936 killagreg 221
                                *ubxSp = NEWDATA; // new data are valid
886 killagreg 222
                                UpdateGPSInfo(); //update GPS info respectively
223
                                GPSTimeout = 255;
224
                        }
225
                        else
226
                        {       // if checksum not fit then set data invalid
227
                                *ubxSp = INVALID;
228
                        }
229
                        ubxstate = UBXSTATE_IDLE; // ready to parse new data
230
                        break;
231
 
232
                default: // unknown ubx state
233
                        ubxstate = UBXSTATE_IDLE;
234
                        break;
235
        }
236
 
237
}
238
 
239