Subversion Repositories FlightCtrl

Rev

Rev 722 | Go to most recent revision | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

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