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