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