Subversion Repositories FlightCtrl

Rev

Rev 684 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 684 Rev 702
Line 1... Line 1...
1
#include <inttypes.h>
1
#include <inttypes.h>
2
#include "ubx.h"
2
#include "ubx.h"
-
 
3
#include "main.h"
-
 
4
#include <avr/io.h>
Line 3... Line 5...
3
 
5
 
4
// ubx protocol parser state machine
6
// ubx protocol parser state machine
5
#define UBXSTATE_IDLE   0
7
#define UBXSTATE_IDLE   0
6
#define UBXSTATE_SYNC1  1
8
#define UBXSTATE_SYNC1  1
Line 28... Line 30...
28
        uint8_t                 Flags;          // Navigation Status Flags
30
        uint8_t                 Flags;          // Navigation Status Flags
29
        uint8_t                 DiffS;          // Differential Status
31
        uint8_t                 DiffS;          // Differential Status
30
        uint8_t                 res;            // reserved
32
        uint8_t                 res;            // reserved
31
        uint32_t                TTFF;           // Time to first fix (millisecond time tag)
33
        uint32_t                TTFF;           // Time to first fix (millisecond time tag)
32
        uint32_t                MSSS;           // Milliseconds since Startup / Reset
34
        uint32_t                MSSS;           // Milliseconds since Startup / Reset
33
        uint8_t                 packetStatus;
35
        uint8_t                 Status;
34
} GPS_STATUS_t;
36
} GPS_STATUS_t;
Line 35... Line 37...
35
 
37
 
36
typedef struct {
38
typedef struct {
37
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
39
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
38
        int32_t                 LON;            // 1e-07 deg Longitude
40
        int32_t                 LON;            // 1e-07 deg Longitude
39
        int32_t                 LAT;            // 1e-07 deg Latitude
41
        int32_t                 LAT;            // 1e-07 deg Latitude
40
        int32_t                 HEIGHT;         // mm Height above Ellipsoid
42
        int32_t                 HEIGHT;         // mm Height above Ellipsoid
41
        int32_t                 HMSL;           // mm Height above mean sea level
43
        int32_t                 HMSL;           // mm Height above mean sea level
42
        uint32_t                Hacc;           // mm Horizontal Accuracy Estimate
44
        uint32_t                Hacc;           // mm Horizontal Accuracy Estimate
43
        uint32_t                Vacc;           // mm Vertical Accuracy Estimate
45
        uint32_t                Vacc;           // mm Vertical Accuracy Estimate
44
        uint8_t                 packetStatus;
46
        uint8_t                 Status;
Line 45... Line 47...
45
} GPS_POSLLH_t;
47
} GPS_POSLLH_t;
46
 
48
 
47
typedef struct {
49
typedef struct {
48
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
50
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
49
        int32_t                 EAST;           // cm  UTM Easting
51
        int32_t                 EAST;           // cm  UTM Easting
50
        int32_t                 NORTH;          // cm  UTM Nording
52
        int32_t                 NORTH;          // cm  UTM Nording
51
        int32_t                 ALT;            // cm  altitude
53
        int32_t                 ALT;            // cm  altitude
52
        uint8_t                 ZONE;           // UTM zone number
54
        uint8_t                 ZONE;           // UTM zone number
53
        uint8_t                 HEM;            // Hemisphere Indicator (0=North, 1=South)
55
        uint8_t                 HEM;            // Hemisphere Indicator (0=North, 1=South)
Line 54... Line 56...
54
        uint8_t                 packetStatus;
56
        uint8_t                 Status;
55
} GPS_POSUTM_t;
57
} GPS_POSUTM_t;
56
 
58
 
Line 62... Line 64...
62
        uint32_t                Speed;          // cm/s  Speed (3-D)
64
        uint32_t                Speed;          // cm/s  Speed (3-D)
63
        uint32_t                GSpeed;         // cm/s  Ground Speed (2-D)
65
        uint32_t                GSpeed;         // cm/s  Ground Speed (2-D)
64
        int32_t                 Heading;        // 1e-05 deg  Heading 2-D
66
        int32_t                 Heading;        // 1e-05 deg  Heading 2-D
65
        uint32_t                SAcc;           // cm/s  Speed Accuracy Estimate
67
        uint32_t                SAcc;           // cm/s  Speed Accuracy Estimate
66
        uint32_t                CAcc;           // deg  Course / Heading Accuracy Estimate
68
        uint32_t                CAcc;           // deg  Course / Heading Accuracy Estimate
67
        uint8_t                 packetStatus;
69
        uint8_t                 Status;
68
} GPS_VELNED_t;
70
} GPS_VELNED_t;
Line 69... Line 71...
69
 
71
 
70
 
72
 
71
GPS_STATUS_t    GpsStatus;
73
GPS_STATUS_t    GpsStatus;
72
GPS_POSLLH_t    GpsPosLlh;
74
GPS_POSLLH_t    GpsPosLlh;
Line 73... Line 75...
73
GPS_POSUTM_t    GpsPosUtm;
75
GPS_POSUTM_t    GpsPosUtm;
Line 74... Line 76...
74
GPS_VELNED_t    GpsVelNed;
76
GPS_VELNED_t    GpsVelNed;
75
 
77
 
76
GPS_INFO_t actualPos;
78
GPS_INFO_t GPSInfo;
77
 
79
 
78
void UpdateGPSInfo (void)
80
void UpdateGPSInfo (void)
79
{
81
{
80
        if (GpsStatus.packetStatus == VALID)                    // valid packet
82
        if (GpsStatus.Status == VALID)                  // valid packet
81
        {
83
        {
82
                actualPos.satfix = GpsStatus.GPSfix;
84
                GPSInfo.satfix = GpsStatus.GPSfix;
83
                GpsStatus.packetStatus = PROCESSED;                     // never update old data
85
                GpsStatus.Status = PROCESSED;                   // never update old data
84
        }
86
        }
85
        if (GpsPosLlh.packetStatus == VALID)                    // valid packet
87
        if (GpsPosLlh.Status == VALID)                  // valid packet
86
        {
88
        {
87
                actualPos.longitude = GpsPosLlh.LON;
89
                GPSInfo.longitude = GpsPosLlh.LON;
88
                actualPos.latitude = GpsPosLlh.LAT;
90
                GPSInfo.latitude = GpsPosLlh.LAT;
89
                actualPos.altitude = GpsPosLlh.HEIGHT;
91
                GPSInfo.altitude = GpsPosLlh.HEIGHT;
90
                GpsPosLlh.packetStatus = PROCESSED;                     // never update old data
92
                GpsPosLlh.Status = PROCESSED;                   // never update old data
91
        }
93
        }
92
        if (GpsPosUtm.packetStatus == VALID)                    // valid packet
94
        if (GpsPosUtm.Status == VALID)                  // valid packet
93
        {
95
        {
94
                actualPos.utmeast = GpsPosUtm.EAST;
96
                GPSInfo.utmeast = GpsPosUtm.EAST;
95
                actualPos.utmnorth = GpsPosUtm.NORTH;
97
                GPSInfo.utmnorth = GpsPosUtm.NORTH;
96
                actualPos.utmalt = GpsPosUtm.ALT;
98
                GPSInfo.utmalt = GpsPosUtm.ALT;
97
                GpsPosUtm.packetStatus = PROCESSED;                     // never update old data
99
                GpsPosUtm.Status = PROCESSED;                   // never update old data
98
        }
100
        }
99
        if (GpsVelNed.packetStatus == VALID)                    // valid packet
101
        if (GpsVelNed.Status == VALID)                  // valid packet
100
        {
102
        {
-
 
103
                GPSInfo.veleast = GpsVelNed.VEL_E;
-
 
104
                GPSInfo.velnorth = GpsVelNed.VEL_N;
-
 
105
                GPSInfo.veltop = -GpsVelNed.VEL_D;
-
 
106
                GpsPosUtm.Status = PROCESSED;                   // never update old data
101
                actualPos.veleast = GpsVelNed.VEL_E;
107
        }
102
                actualPos.velnorth = GpsVelNed.VEL_N;
108
        if (GpsStatus.Status | GpsVelNed.Status | GpsPosLlh.Status | GpsPosUtm.Status)
Line 103... Line 109...
103
                actualPos.veltop = -GpsVelNed.VEL_D;
109
        {
104
                GpsPosUtm.packetStatus = PROCESSED;                     // never update old data
110
                GPSInfo.status = VALID;
Line 134... Line 140...
134
                        switch(c)
140
                        switch(c)
135
                        {
141
                        {
136
                                case UBX_ID_POSUTM: // utm position
142
                                case UBX_ID_POSUTM: // utm position
137
                                        ubxP =  (int8_t *)&GpsPosUtm; // data start pointer
143
                                        ubxP =  (int8_t *)&GpsPosUtm; // data start pointer
138
                                        ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer
144
                                        ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer
139
                                        ubxSp = (int8_t *)&GpsPosUtm.packetStatus; // status pointer
145
                                        ubxSp = (int8_t *)&GpsPosUtm.Status; // status pointer
Line 140... Line 146...
140
 
146
 
Line 141... Line 147...
141
                                        break;
147
                                        break;
142
 
148
 
143
                                case UBX_ID_POSLLH: // geodetic position
149
                                case UBX_ID_POSLLH: // geodetic position
144
                                        ubxP =  (int8_t *)&GpsStatus; // data start pointer
150
                                        ubxP =  (int8_t *)&GpsStatus; // data start pointer
145
                                        ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer
151
                                        ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer
Line 146... Line 152...
146
                                        ubxSp = (int8_t *)&GpsStatus.packetStatus; // status pointer
152
                                        ubxSp = (int8_t *)&GpsStatus.Status; // status pointer
147
                                        break;
153
                                        break;
148
 
154
 
149
                                case UBX_ID_STATUS: // receiver status
155
                                case UBX_ID_STATUS: // receiver status
150
                                        ubxP =  (int8_t *)&GpsStatus; // data start pointer
156
                                        ubxP =  (int8_t *)&GpsStatus; // data start pointer
Line 151... Line 157...
151
                                        ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer
157
                                        ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer
152
                                        ubxSp = (int8_t *)&GpsStatus.packetStatus; // status pointer
158
                                        ubxSp = (int8_t *)&GpsStatus.Status; // status pointer
153
                                        break;
159
                                        break;
154
 
160
 
155
                                case UBX_ID_VELNED: // velocity vector in tangent plane
161
                                case UBX_ID_VELNED: // velocity vector in tangent plane
Line 156... Line 162...
156
                                        ubxP =  (int8_t *)&GpsVelNed; // data start pointer
162
                                        ubxP =  (int8_t *)&GpsVelNed; // data start pointer
157
                                        ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer
163
                                        ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer
158
                                        ubxSp = (int8_t *)&GpsVelNed.packetStatus; // status pointer
164
                                        ubxSp = (int8_t *)&GpsVelNed.Status; // status pointer
Line 206... Line 212...
206
                case UBXSTATE_CKB:
212
                case UBXSTATE_CKB:
207
                        if (c == ckb)
213
                        if (c == ckb)
208
                        {
214
                        {
209
                                *ubxSp = VALID; // new data are availabe
215
                                *ubxSp = VALID; // new data are availabe
210
                                UpdateGPSInfo(); //update GPS info respectively
216
                                UpdateGPSInfo(); //update GPS info respectively
-
 
217
                                ROT_FLASH;
-
 
218
 
211
                        }
219
                        }
212
                        ubxstate = UBXSTATE_IDLE; // ready to parse new data
220
                        ubxstate = UBXSTATE_IDLE; // ready to parse new data
213
                        break;
221
                        break;
Line 214... Line 222...
214
 
222