Subversion Repositories FlightCtrl

Rev

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

Rev 935 Rev 936
Line 42... Line 42...
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
        Status_t                Status;
48
} GPS_SOL_t;
48
} UBX_SOL_t;
Line 49... Line 49...
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
        Status_t                Status;
Line 59... Line 59...
59
} GPS_POSLLH_t;
59
} UBX_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
Line 66... Line 66...
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
        Status_t                Status;
72
} GPS_VELNED_t;
72
} UBX_VELNED_t;
Line 73... Line 73...
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
UBX_SOL_t               UbxSol    = {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
UBX_POSLLH_t    UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
76
GPS_VELNED_t    GpsVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
76
UBX_VELNED_t    UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
Line 77... Line 77...
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};
Line 78... Line 78...
78
 
78
 
79
volatile uint8_t GPSTimeout = 0;
79
volatile uint8_t GPSTimeout = 0;
80
 
-
 
81
void UpdateGPSInfo (void)
-
 
82
{
80
 
83
        static uint32_t lasttime;
-
 
84
        if (GpsSol.Status == VALID)                             // valid packet
-
 
85
        {
-
 
86
                GPSInfo.satfix = GpsSol.GPSfix;
-
 
87
                GPSInfo.satnum = GpsSol.numSV;
-
 
88
                GPSInfo.PAcc = GpsSol.PAcc;
-
 
89
                GPSInfo.VAcc = GpsSol.SAcc;
-
 
90
                GpsSol.Status = PROCESSED;                      // never update old data
-
 
91
        }
-
 
92
        if (GpsPosLlh.Status == VALID)                  // valid packet
-
 
93
        {
-
 
94
                GPSInfo.updatetime = GpsPosLlh.ITOW - lasttime;
-
 
95
                lasttime = GpsPosLlh.ITOW;
-
 
96
                GPSInfo.longitude = GpsPosLlh.LON;
-
 
97
                GPSInfo.latitude = GpsPosLlh.LAT;
-
 
98
                GPSInfo.altitude = GpsPosLlh.HEIGHT;
-
 
99
                GpsPosLlh.Status = PROCESSED;           // never update old data
-
 
100
        }
-
 
101
        if (GpsVelNed.Status == VALID)                  // valid packet
-
 
102
        {
-
 
103
                GPSInfo.veleast = GpsVelNed.VEL_E;
-
 
104
                GPSInfo.velnorth = GpsVelNed.VEL_N;
-
 
105
                GPSInfo.veltop = -GpsVelNed.VEL_D;
-
 
106
                GPSInfo.velground = GpsVelNed.GSpeed;
81
void UpdateGPSInfo (void)
107
                GpsVelNed.Status = PROCESSED;           // never update old data
82
{
-
 
83
 
-
 
84
        if ((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))
-
 
85
        {
-
 
86
                RED_FLASH;
-
 
87
                if(GPSInfo.status != NEWDATA)
-
 
88
                {
-
 
89
                        GPSInfo.status = INVALID;
-
 
90
                        // NAV SOL
-
 
91
                        GPSInfo.flags = UbxSol.Flags;
-
 
92
                        GPSInfo.satfix = UbxSol.GPSfix;
-
 
93
                        GPSInfo.satnum = UbxSol.numSV;
-
 
94
                        GPSInfo.PAcc = UbxSol.PAcc;
-
 
95
                        GPSInfo.VAcc = UbxSol.SAcc;
-
 
96
                        // NAV POSLLH
-
 
97
                        GPSInfo.longitude = UbxPosLlh.LON;
-
 
98
                        GPSInfo.latitude = UbxPosLlh.LAT;
-
 
99
                        GPSInfo.altitude = UbxPosLlh.HEIGHT;
-
 
100
 
-
 
101
                        GPSInfo.veleast = UbxVelNed.VEL_E;
-
 
102
                        GPSInfo.velnorth = UbxVelNed.VEL_N;
-
 
103
                        GPSInfo.veltop = -UbxVelNed.VEL_D;
-
 
104
                        GPSInfo.velground = UbxVelNed.GSpeed;
-
 
105
 
-
 
106
                        GPSInfo.status = NEWDATA;
108
        }
107
 
-
 
108
                }
-
 
109
                // set state to collect new data
109
        if ((GpsSol.Status != INVALID) && (GpsPosLlh.Status != INVALID) && (GpsVelNed.Status != INVALID))
110
                UbxSol.Status = PROCESSED;                      // never update old data
-
 
111
                UbxPosLlh.Status = PROCESSED;           // never update old data
-
 
112
                UbxVelNed.Status = PROCESSED;           // never update old data
110
        {
113
        }
Line 111... Line 114...
111
                GPSInfo.status = VALID;             // set valid if data are updated
114
 
112
        }
115
 
Line 140... Line 143...
140
 
143
 
141
                case UBXSTATE_CLASS: // check message identifier
144
                case UBXSTATE_CLASS: // check message identifier
142
                        switch(c)
145
                        switch(c)
143
                        {
146
                        {
144
                                case UBX_ID_POSLLH: // geodetic position
147
                                case UBX_ID_POSLLH: // geodetic position
145
                                        ubxP =  (int8_t *)&GpsPosLlh; // data start pointer
148
                                        ubxP =  (int8_t *)&UbxPosLlh; // data start pointer
146
                                        ubxEp = (int8_t *)(&GpsPosLlh + 1); // data end pointer
149
                                        ubxEp = (int8_t *)(&UbxPosLlh + 1); // data end pointer
147
                                        ubxSp = (int8_t *)&GpsPosLlh.Status; // status pointer
150
                                        ubxSp = (int8_t *)&UbxPosLlh.Status; // status pointer
Line 148... Line 151...
148
                                        break;
151
                                        break;
149
 
152
 
150
                                case UBX_ID_SOL: // navigation solution
153
                                case UBX_ID_SOL: // navigation solution
151
                                        ubxP =  (int8_t *)&GpsSol; // data start pointer
154
                                        ubxP =  (int8_t *)&UbxSol; // data start pointer
152
                                        ubxEp = (int8_t *)(&GpsSol + 1); // data end pointer
155
                                        ubxEp = (int8_t *)(&UbxSol + 1); // data end pointer
Line 153... Line 156...
153
                                        ubxSp = (int8_t *)&GpsSol.Status; // status pointer
156
                                        ubxSp = (int8_t *)&UbxSol.Status; // status pointer
154
                                        break;
157
                                        break;
155
 
158
 
156
                                case UBX_ID_VELNED: // velocity vector in tangent plane
159
                                case UBX_ID_VELNED: // velocity vector in tangent plane
157
                                        ubxP =  (int8_t *)&GpsVelNed; // data start pointer
160
                                        ubxP =  (int8_t *)&UbxVelNed; // data start pointer
Line 158... Line 161...
158
                                        ubxEp = (int8_t *)(&GpsVelNed + 1); // data end pointer
161
                                        ubxEp = (int8_t *)(&UbxVelNed + 1); // data end pointer
159
                                        ubxSp = (int8_t *)&GpsVelNed.Status; // status pointer
162
                                        ubxSp = (int8_t *)&UbxVelNed.Status; // status pointer
160
                                        break;
163
                                        break;
Line 182... Line 185...
182
                        msglen += ((uint16_t)c)<<8;
185
                        msglen += ((uint16_t)c)<<8;
183
                        cka += c;
186
                        cka += c;
184
                        ckb += cka;
187
                        ckb += cka;
185
                        // if the old data are not processed so far then break parsing now
188
                        // 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
189
                        // to avoid writing new data in ISR during reading by another function
187
                        if ( *ubxSp == VALID )
190
                        if ( *ubxSp == NEWDATA )
188
                        {
191
                        {
189
                                UpdateGPSInfo(); //update GPS info respectively
192
                                UpdateGPSInfo(); //update GPS info respectively
190
                                ubxstate = UBXSTATE_IDLE;
193
                                ubxstate = UBXSTATE_IDLE;
191
                        }
194
                        }
192
                        else // data invalid or allready processd
195
                        else // data invalid or allready processd
Line 213... Line 216...
213
                        break;
216
                        break;
Line 214... Line 217...
214
 
217
 
215
                case UBXSTATE_CKB:
218
                case UBXSTATE_CKB:
216
                        if (c == ckb)
219
                        if (c == ckb)
217
                        {
220
                        {
218
                                *ubxSp = VALID; // new data are valid
-
 
219
                                ROT_FLASH;
221
                                *ubxSp = NEWDATA; // new data are valid
220
                                UpdateGPSInfo(); //update GPS info respectively
222
                                UpdateGPSInfo(); //update GPS info respectively
221
                                GPSTimeout = 255;
223
                                GPSTimeout = 255;
222
                        }
224
                        }
223
                        else
225
                        else