Subversion Repositories FlightCtrl

Rev

Rev 903 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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