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