Subversion Repositories FlightCtrl

Rev

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

Rev 752 Rev 781
Line 19... Line 19...
19 
// ublox protocoll identifier
 19 
// ublox protocoll identifier
20 
#define UBX_CLASS_NAV   0x01
 20 
#define UBX_CLASS_NAV   0x01
Line 21... Line 21...
21 
 
 21 
 
22 
#define UBX_ID_POSLLH   0x02
 22 
#define UBX_ID_POSLLH   0x02
23 
#define UBX_ID_SOL              0x06
 - 
 
24 
#define UBX_ID_POSUTM   0x08
 23 
#define UBX_ID_SOL              0x06
Line 25... Line 24...
25 
#define UBX_ID_VELNED   0x12
 24 
#define UBX_ID_VELNED   0x12
26 
 
 25 
 
Line 58... Line 57...
58 
        uint32_t                Vacc;           // mm Vertical Accuracy Estimate
 57 
        uint32_t                Vacc;           // mm Vertical Accuracy Estimate
59 
        uint8_t                 Status;
 58 
        uint8_t                 Status;
60 
} GPS_POSLLH_t;
 59 
} GPS_POSLLH_t;
Line 61... Line 60...
61 
 
 60 
 
62 
typedef struct {
 - 
 
63 
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
 - 
 
64 
        int32_t                 EAST;           // cm  UTM Easting
 - 
 
65 
        int32_t                 NORTH;          // cm  UTM Nording
 - 
 
66 
        int32_t                 ALT;            // cm  altitude
 - 
 
67 
        uint8_t                 ZONE;           // UTM zone number
 - 
 
68 
        uint8_t                 HEM;            // Hemisphere Indicator (0=North, 1=South)
 - 
 
69 
        uint8_t                 Status;
 - 
 
70 
} GPS_POSUTM_t;
 - 
 
71 
 
 - 
 
72 
typedef struct {
 61 
typedef struct {
73 
        uint32_t                ITOW;           // ms  GPS Millisecond Time of Week
 62 
        uint32_t                ITOW;           // ms  GPS Millisecond Time of Week
74 
        int32_t                 VEL_N;          // cm/s  NED north velocity
 63 
        int32_t                 VEL_N;          // cm/s  NED north velocity
75 
        int32_t                 VEL_E;          // cm/s  NED east velocity
 64 
        int32_t                 VEL_E;          // cm/s  NED east velocity
76 
        int32_t                 VEL_D;          // cm/s  NED down velocity
 65 
        int32_t                 VEL_D;          // cm/s  NED down velocity
Line 82... Line 71...
82 
        uint8_t                 Status;
 71 
        uint8_t                 Status;
83 
} GPS_VELNED_t;
 72 
} GPS_VELNED_t;
Line 84... Line 73...
84 
 
 73 
 
85 
GPS_SOL_t               GpsSol    = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
 74 
GPS_SOL_t               GpsSol    = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
86 
GPS_POSLLH_t    GpsPosLlh = {0,0,0,0,0,0,0, INVALID};
 - 
 
87 
GPS_POSUTM_t    GpsPosUtm = {0,0,0,0,0,0, INVALID};
 75 
GPS_POSLLH_t    GpsPosLlh = {0,0,0,0,0,0,0, INVALID};
88 
GPS_VELNED_t    GpsVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
 76 
GPS_VELNED_t    GpsVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
Line 89... Line 77...
89 
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 105... Line 93...
105 
                GPSInfo.longitude = GpsPosLlh.LON;
 93 
                GPSInfo.longitude = GpsPosLlh.LON;
106 
                GPSInfo.latitude = GpsPosLlh.LAT;
 94 
                GPSInfo.latitude = GpsPosLlh.LAT;
107 
                GPSInfo.altitude = GpsPosLlh.HEIGHT;
 95 
                GPSInfo.altitude = GpsPosLlh.HEIGHT;
108 
                GpsPosLlh.Status = PROCESSED;           // never update old data
 96 
                GpsPosLlh.Status = PROCESSED;           // never update old data
109 
        }
 97 
        }
110 
        if (GpsPosUtm.Status == VALID)                  // valid packet
 - 
 
111 
        {
 - 
 
112 
                GPSInfo.utmeast = GpsPosUtm.EAST;
 - 
 
113 
                GPSInfo.utmnorth = GpsPosUtm.NORTH;
 - 
 
114 
                GPSInfo.utmalt = GpsPosUtm.ALT;
 - 
 
115 
                GpsPosUtm.Status = PROCESSED;           // never update old data
 - 
 
116 
        }
 - 
 
117 
        if (GpsVelNed.Status == VALID)                  // valid packet
 98 
        if (GpsVelNed.Status == VALID)                  // valid packet
118 
        {
 99 
        {
119 
                GPSInfo.veleast = GpsVelNed.VEL_E;
 100 
                GPSInfo.veleast = GpsVelNed.VEL_E;
120 
                GPSInfo.velnorth = GpsVelNed.VEL_N;
 101 
                GPSInfo.velnorth = GpsVelNed.VEL_N;
121 
                GPSInfo.veltop = -GpsVelNed.VEL_D;
 102 
                GPSInfo.veltop = -GpsVelNed.VEL_D;
122 
                GpsVelNed.Status = PROCESSED;           // never update old data
 103 
                GpsVelNed.Status = PROCESSED;           // never update old data
123 
        }
 104 
        }
124 
        if ((GpsSol.Status != INVALID) && (GpsPosLlh.Status != INVALID) && (GpsPosUtm.Status != INVALID) && (GpsVelNed.Status != INVALID))
 105 
        if ((GpsSol.Status != INVALID) && (GpsPosLlh.Status != INVALID) && (GpsVelNed.Status != INVALID))
125 
        {
 106 
        {
126 
                GPSInfo.status = VALID;             // set valid if data are updated
 107 
                GPSInfo.status = VALID;             // set valid if data are updated
127 
        }
 108 
        }
128 
}
 109 
}
Line 154... Line 135...
154 
                        break;
 135 
                        break;
Line 155... Line 136...
155 
 
 136 
 
156 
                case UBXSTATE_CLASS: // check message identifier
 137 
                case UBXSTATE_CLASS: // check message identifier
157 
                        switch(c)
 138 
                        switch(c)
158 
                        {
 - 
 
159 
                                case UBX_ID_POSUTM: // utm position
 - 
 
160 
                                        ubxP =  (int8_t *)&GpsPosUtm; // data start pointer
 - 
 
161 
                                        ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer
 - 
 
162 
                                        ubxSp = (int8_t *)&GpsPosUtm.Status; // status pointer
 - 
 
163 
                                        break;
 - 
 
164 
 
 139 
                        {
165 
                                case UBX_ID_POSLLH: // geodetic position
 140 
                                case UBX_ID_POSLLH: // geodetic position
166 
                                        ubxP =  (int8_t *)&GpsPosLlh; // data start pointer
 141 
                                        ubxP =  (int8_t *)&GpsPosLlh; // data start pointer
167 
                                        ubxEp = (int8_t *)(&GpsPosLlh + sizeof(GPS_POSLLH_t)); // data end pointer
 142 
                                        ubxEp = (int8_t *)(&GpsPosLlh + sizeof(GPS_POSLLH_t)); // data end pointer
168 
                                        ubxSp = (int8_t *)&GpsPosLlh.Status; // status pointer
 143 
                                        ubxSp = (int8_t *)&GpsPosLlh.Status; // status pointer