Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
684 killagreg 1
#include <inttypes.h>
2
#include "ubx.h"
3
 
4
// ubx protocol parser state machine
5
#define UBXSTATE_IDLE   0
6
#define UBXSTATE_SYNC1  1
7
#define UBXSTATE_SYNC2  2
8
#define UBXSTATE_CLASS  3
9
#define UBXSTATE_LEN1   4
10
#define UBXSTATE_LEN2   5
11
#define UBXSTATE_DATA   6
12
#define UBXSTATE_CKA    7
13
#define UBXSTATE_CKB    8
14
 
15
// ublox protocoll identifier
16
#define UBX_CLASS_NAV   0x01
17
#define UBX_ID_POSLLH   0x02
18
#define UBX_ID_POSUTM   0x08
19
#define UBX_ID_STATUS   0x03
20
#define UBX_ID_VELNED   0x12
21
#define UBX_SYNC1_CHAR  0xB5
22
#define UBX_SYNC2_CHAR  0x62
23
 
24
 
25
typedef struct {
26
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
27
        uint8_t                 GPSfix;         // GPSfix Type, range 0..6
28
        uint8_t                 Flags;          // Navigation Status Flags
29
        uint8_t                 DiffS;          // Differential Status
30
        uint8_t                 res;            // reserved
31
        uint32_t                TTFF;           // Time to first fix (millisecond time tag)
32
        uint32_t                MSSS;           // Milliseconds since Startup / Reset
33
        uint8_t                 packetStatus;
34
} GPS_STATUS_t;
35
 
36
typedef struct {
37
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
38
        int32_t                 LON;            // 1e-07 deg Longitude
39
        int32_t                 LAT;            // 1e-07 deg Latitude
40
        int32_t                 HEIGHT;         // mm Height above Ellipsoid
41
        int32_t                 HMSL;           // mm Height above mean sea level
42
        uint32_t                Hacc;           // mm Horizontal Accuracy Estimate
43
        uint32_t                Vacc;           // mm Vertical Accuracy Estimate
44
        uint8_t                 packetStatus;
45
} GPS_POSLLH_t;
46
 
47
typedef struct {
48
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
49
        int32_t                 EAST;           // cm  UTM Easting
50
        int32_t                 NORTH;          // cm  UTM Nording
51
        int32_t                 ALT;            // cm  altitude
52
        uint8_t                 ZONE;           // UTM zone number
53
        uint8_t                 HEM;            // Hemisphere Indicator (0=North, 1=South)
54
        uint8_t                 packetStatus;
55
} GPS_POSUTM_t;
56
 
57
typedef struct {
58
        uint32_t                ITOW;           // ms  GPS Millisecond Time of Week
59
        int32_t                 VEL_N;          // cm/s  NED north velocity
60
        int32_t                 VEL_E;          // cm/s  NED east velocity
61
        int32_t                 VEL_D;          // cm/s  NED down velocity
62
        uint32_t                Speed;          // cm/s  Speed (3-D)
63
        uint32_t                GSpeed;         // cm/s  Ground Speed (2-D)
64
        int32_t                 Heading;        // 1e-05 deg  Heading 2-D
65
        uint32_t                SAcc;           // cm/s  Speed Accuracy Estimate
66
        uint32_t                CAcc;           // deg  Course / Heading Accuracy Estimate
67
        uint8_t                 packetStatus;
68
} GPS_VELNED_t;
69
 
70
 
71
GPS_STATUS_t    GpsStatus;
72
GPS_POSLLH_t    GpsPosLlh;
73
GPS_POSUTM_t    GpsPosUtm;
74
GPS_VELNED_t    GpsVelNed;
75
 
76
GPS_INFO_t actualPos;
77
 
78
void UpdateGPSInfo (void)
79
{
80
        if (GpsStatus.packetStatus == VALID)                    // valid packet
81
        {
82
                actualPos.satfix = GpsStatus.GPSfix;
83
                GpsStatus.packetStatus = PROCESSED;                     // never update old data
84
        }
85
        if (GpsPosLlh.packetStatus == VALID)                    // valid packet
86
        {
87
                actualPos.longitude = GpsPosLlh.LON;
88
                actualPos.latitude = GpsPosLlh.LAT;
89
                actualPos.altitude = GpsPosLlh.HEIGHT;
90
                GpsPosLlh.packetStatus = PROCESSED;                     // never update old data
91
        }
92
        if (GpsPosUtm.packetStatus == VALID)                    // valid packet
93
        {
94
                actualPos.utmeast = GpsPosUtm.EAST;
95
                actualPos.utmnorth = GpsPosUtm.NORTH;
96
                actualPos.utmalt = GpsPosUtm.ALT;
97
                GpsPosUtm.packetStatus = PROCESSED;                     // never update old data
98
        }
99
        if (GpsVelNed.packetStatus == VALID)                    // valid packet
100
        {
101
                actualPos.veleast = GpsVelNed.VEL_E;
102
                actualPos.velnorth = GpsVelNed.VEL_N;
103
                actualPos.veltop = -GpsVelNed.VEL_D;
104
                GpsPosUtm.packetStatus = PROCESSED;                     // never update old data
105
        }
106
}
107
 
108
// this function should be called within the UART RX ISR
109
void ubx_parser(uint8_t c)
110
{
111
        static uint8_t ubxstate = UBXSTATE_IDLE;
112
        static uint8_t cka, ckb;
113
        static uint16_t msglen;
114
        static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered
115
 
116
        switch(ubxstate)
117
        {
118
                case UBXSTATE_IDLE: // check 1st sync byte
119
                        if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1;
120
                        else ubxstate = UBXSTATE_IDLE; // out of synchronization
121
                        break;
122
 
123
                case UBXSTATE_SYNC1: // check 2nd sync byte
124
                        if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2;
125
                        else ubxstate = UBXSTATE_IDLE; // out of synchronization
126
                        break;
127
 
128
                case UBXSTATE_SYNC2: // check msg class to be NAV
129
                        if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS;
130
                        else ubxstate = UBXSTATE_IDLE; // unsupported message class
131
                        break;
132
 
133
                case UBXSTATE_CLASS: // check message identifier
134
                        switch(c)
135
                        {
136
                                case UBX_ID_POSUTM: // utm position
137
                                        ubxP =  (int8_t *)&GpsPosUtm; // data start pointer
138
                                        ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer
139
                                        ubxSp = (int8_t *)&GpsPosUtm.packetStatus; // status pointer
140
 
141
                                        break;
142
 
143
                                case UBX_ID_POSLLH: // geodetic position
144
                                        ubxP =  (int8_t *)&GpsStatus; // data start pointer
145
                                        ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer
146
                                        ubxSp = (int8_t *)&GpsStatus.packetStatus; // status pointer
147
                                        break;
148
 
149
                                case UBX_ID_STATUS: // receiver status
150
                                        ubxP =  (int8_t *)&GpsStatus; // data start pointer
151
                                        ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer
152
                                        ubxSp = (int8_t *)&GpsStatus.packetStatus; // status pointer
153
                                        break;
154
 
155
                                case UBX_ID_VELNED: // velocity vector in tangent plane
156
                                        ubxP =  (int8_t *)&GpsVelNed; // data start pointer
157
                                        ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer
158
                                        ubxSp = (int8_t *)&GpsVelNed.packetStatus; // status pointer
159
                                        break;
160
 
161
                                default:                        // unsupported identifier
162
                                        ubxstate = UBXSTATE_IDLE;
163
                                        break;
164
                        }
165
                        if (ubxstate != UBXSTATE_IDLE)
166
                        {
167
                                ubxstate = UBXSTATE_LEN1;
168
                                cka = UBX_CLASS_NAV + c;
169
                                ckb = UBX_CLASS_NAV + cka;
170
                        }
171
                        break;
172
 
173
                case UBXSTATE_LEN1: // 1st message length byte
174
                        msglen = c;
175
                        cka += c;
176
                        ckb += cka;
177
                        ubxstate = UBXSTATE_LEN2;
178
                        break;
179
 
180
                case UBXSTATE_LEN2: // 2nd message length byte
181
                        msglen += ((uint16_t)c)<<8;
182
                        cka += c;
183
                        ckb += cka;
184
                        // if the old data are not processed so far then break parsing now
185
                        // to avoid writing new data in ISR during reading by another function
186
                        if ( *ubxSp == VALID ) ubxstate = UBXSTATE_IDLE;
187
                        else // data invalid or allready processd
188
                        {
189
                                *ubxSp = INVALID;
190
                                ubxstate = UBXSTATE_DATA;
191
                        }
192
                        break;
193
 
194
                case UBXSTATE_DATA:
195
                        if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left
196
                        cka += c;
197
                        ckb += cka;
198
                        if (--msglen == 0)      ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
199
                        break;
200
 
201
                case UBXSTATE_CKA:
202
                        if (c == cka) ubxstate = UBXSTATE_CKB;
203
                        else ubxstate = UBXSTATE_IDLE;
204
                        break;
205
 
206
                case UBXSTATE_CKB:
207
                        if (c == ckb)
208
                        {
209
                                *ubxSp = VALID; // new data are availabe
210
                                UpdateGPSInfo(); //update GPS info respectively
211
                        }
212
                        ubxstate = UBXSTATE_IDLE; // ready to parse new data
213
                        break;
214
 
215
                default: // unknown ubx state
216
                        ubxstate = UBXSTATE_IDLE;
217
                        break;
218
        }
219
 
220
}
221