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