WebSVN - FlightCtrl - Diff - Rev 752 and 781 - /branches/V0.68d Code Redesign killagreg/ubx.c


#include <inttypes.h>
#include "ubx.h"
#include "main.h"
#include <avr/io.h>
 
#include "uart.h"
 
// ubx protocol parser state machine
#define UBXSTATE_IDLE   0
#define UBXSTATE_SYNC1  1
#define UBXSTATE_SYNC2  2
#define UBXSTATE_CLASS  3
#define UBXSTATE_LEN1   4
#define UBXSTATE_LEN2   5
#define UBXSTATE_DATA   6
#define UBXSTATE_CKA    7
#define UBXSTATE_CKB    8
 
// ublox protocoll identifier
#define UBX_CLASS_NAV   0x01
 
#define UBX_ID_POSLLH   0x02
#define UBX_ID_SOL              0x06
 
#define UBX_ID_VELNED   0x12
 
#define UBX_SYNC1_CHAR  0xB5
#define UBX_SYNC2_CHAR  0x62
 
typedef struct {
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
        int32_t                 Frac;           // ns remainder of rounded ms above
        int16_t                 week;           // GPS week
        uint8_t                 GPSfix;         // GPSfix Type, range 0..6
        uint8_t                 Flags;          // Navigation Status Flags
        int32_t                 ECEF_X;         // cm ECEF X coordinate
        int32_t                 ECEF_Y;         // cm ECEF Y coordinate
        int32_t                 ECEF_Z;         // cm ECEF Z coordinate
        uint32_t                PAcc;           // cm 3D Position Accuracy Estimate
        int32_t                 ECEFVX;         // cm/s ECEF X velocity
        int32_t                 ECEFVY;         // cm/s ECEF Y velocity
        int32_t                 ECEFVZ;         // cm/s ECEF Z velocity
        uint32_t                SAcc;           // cm/s Speed Accuracy Estimate
        uint16_t                PDOP;           // 0.01 Position DOP
        uint8_t                 res1;           // reserved
        uint8_t                 numSV;          // Number of SVs used in navigation solution
        uint32_t                res2;           // reserved
        uint8_t                 Status;
} GPS_SOL_t;
 
typedef struct {
        uint32_t                ITOW;           // ms GPS Millisecond Time of Week
        int32_t                 LON;            // 1e-07 deg Longitude
        int32_t                 LAT;            // 1e-07 deg Latitude
        int32_t                 HEIGHT;         // mm Height above Ellipsoid
        int32_t                 HMSL;           // mm Height above mean sea level
        uint32_t                Hacc;           // mm Horizontal Accuracy Estimate
        uint32_t                Vacc;           // mm Vertical Accuracy Estimate
        uint8_t                 Status;
} GPS_POSLLH_t;
 
typedef struct {
 
 
 
 
 
 
 
 
 
 
        uint32_t                ITOW;           // ms  GPS Millisecond Time of Week
        int32_t                 VEL_N;          // cm/s  NED north velocity
        int32_t                 VEL_E;          // cm/s  NED east velocity
        int32_t                 VEL_D;          // cm/s  NED down velocity
        uint32_t                Speed;          // cm/s  Speed (3-D)
        uint32_t                GSpeed;         // cm/s  Ground Speed (2-D)
        int32_t                 Heading;        // 1e-05 deg  Heading 2-D
        uint32_t                SAcc;           // cm/s  Speed Accuracy Estimate
        uint32_t                CAcc;           // deg  Course / Heading Accuracy Estimate
        uint8_t                 Status;
} GPS_VELNED_t;
 
GPS_SOL_t               GpsSol    = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
GPS_POSLLH_t    GpsPosLlh = {0,0,0,0,0,0,0, INVALID};
 
GPS_VELNED_t    GpsVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
GPS_INFO_t      GPSInfo   = {0,0,0,0,0,0,0,0,0,0, INVALID};
 
volatile uint8_t GPSTimeout = 0;
 
void UpdateGPSInfo (void)
{
        if (GpsSol.Status == VALID)                             // valid packet
        {
                GPSInfo.satfix = GpsSol.GPSfix;
                GPSInfo.satnum = GpsSol.numSV;
                GPSInfo.PAcc = GpsSol.PAcc;
                GPSInfo.VAcc = GpsSol.SAcc;
                GpsSol.Status = PROCESSED;                      // never update old data
        }
        if (GpsPosLlh.Status == VALID)                  // valid packet
        {
                GPSInfo.longitude = GpsPosLlh.LON;
                GPSInfo.latitude = GpsPosLlh.LAT;
                GPSInfo.altitude = GpsPosLlh.HEIGHT;
                GpsPosLlh.Status = PROCESSED;           // never update old data
        }
 
 
 
 
 
 
 
        if (GpsVelNed.Status == VALID)                  // valid packet
        {
                GPSInfo.veleast = GpsVelNed.VEL_E;
                GPSInfo.velnorth = GpsVelNed.VEL_N;
                GPSInfo.veltop = -GpsVelNed.VEL_D;
                GpsVelNed.Status = PROCESSED;           // never update old data
        }
        if ((GpsSol.Status != INVALID) && (GpsPosLlh.Status != INVALID) && (GpsVelNed.Status != INVALID))
        {
                GPSInfo.status = VALID;             // set valid if data are updated
        }
}
 
 
// this function should be called within the UART RX ISR
void ubx_parser(uint8_t c)
{
        static uint8_t ubxstate = UBXSTATE_IDLE;
        static uint8_t cka, ckb;
        static uint16_t msglen;
        static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered
 
        switch(ubxstate)
        {
                case UBXSTATE_IDLE: // check 1st sync byte
                        if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1;
                        else ubxstate = UBXSTATE_IDLE; // out of synchronization
                        break;
 
                case UBXSTATE_SYNC1: // check 2nd sync byte
                        if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2;
                        else ubxstate = UBXSTATE_IDLE; // out of synchronization
                        break;
 
                case UBXSTATE_SYNC2: // check msg class to be NAV
                        if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS;
                        else ubxstate = UBXSTATE_IDLE; // unsupported message class
                        break;
 
                case UBXSTATE_CLASS: // check message identifier
                        switch(c)
                        {
 
 
 
 
 
 
                                case UBX_ID_POSLLH: // geodetic position
                                        ubxP =  (int8_t *)&GpsPosLlh; // data start pointer
                                        ubxEp = (int8_t *)(&GpsPosLlh + sizeof(GPS_POSLLH_t)); // data end pointer
                                        ubxSp = (int8_t *)&GpsPosLlh.Status; // status pointer
                                        break;
 
                                case UBX_ID_SOL: // navigation solution
                                        ubxP =  (int8_t *)&GpsSol; // data start pointer
                                        ubxEp = (int8_t *)(&GpsSol + sizeof(GPS_SOL_t)); // data end pointer
                                        ubxSp = (int8_t *)&GpsSol.Status; // status pointer
                                        break;
 
                                case UBX_ID_VELNED: // velocity vector in tangent plane
                                        ubxP =  (int8_t *)&GpsVelNed; // data start pointer
                                        ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer
                                        ubxSp = (int8_t *)&GpsVelNed.Status; // status pointer
                                        break;
 
                                default:                        // unsupported identifier
                                        ubxstate = UBXSTATE_IDLE;
                                        break;
                        }
                        if (ubxstate != UBXSTATE_IDLE)
                        {
                                ubxstate = UBXSTATE_LEN1;
                                cka = UBX_CLASS_NAV + c;
                                ckb = UBX_CLASS_NAV + cka;
                        }
                        break;
 
                case UBXSTATE_LEN1: // 1st message length byte
                        msglen = c;
                        cka += c;
                        ckb += cka;
                        ubxstate = UBXSTATE_LEN2;
                        break;
 
                case UBXSTATE_LEN2: // 2nd message length byte
                        msglen += ((uint16_t)c)<<8;
                        cka += c;
                        ckb += cka;
                        // if the old data are not processed so far then break parsing now
                        // to avoid writing new data in ISR during reading by another function
                        if ( *ubxSp == VALID ) ubxstate = UBXSTATE_IDLE;
                        else // data invalid or allready processd
                        {
                                *ubxSp = INVALID;
                                ubxstate = UBXSTATE_DATA;
                        }
                        break;
 
                case UBXSTATE_DATA:
                        if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left
                        cka += c;
                        ckb += cka;
                        if (--msglen == 0)      ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
                        break;
 
                case UBXSTATE_CKA:
                        if (c == cka) ubxstate = UBXSTATE_CKB;
                        else
                        {
                                *ubxSp = INVALID;
                                ubxstate = UBXSTATE_IDLE;
                        }
                        break;
 
                case UBXSTATE_CKB:
                        if (c == ckb)
                        {
                                *ubxSp = VALID; // new data are valid
                                ROT_FLASH;
                                UpdateGPSInfo(); //update GPS info respectively
                                GPSTimeout = 255;
                        }
                        else
                        {       // if checksum not fit then set data invalid
                                *ubxSp = INVALID;
                        }
                        ubxstate = UBXSTATE_IDLE; // ready to parse new data
                        break;
 
                default: // unknown ubx state
                        ubxstate = UBXSTATE_IDLE;
                        break;
        }
 
}
 
 
 

Rev 752	Rev 781
1	#include <inttypes.h>	1	#include <inttypes.h>
2	#include "ubx.h"	2	#include "ubx.h"
3	#include "main.h"	3	#include "main.h"
4	#include <avr/io.h>	4	#include <avr/io.h>
5		5
6	#include "uart.h"	6	#include "uart.h"
7		7
8	// ubx protocol parser state machine	8	// ubx protocol parser state machine
9	#define UBXSTATE_IDLE 0	9	#define UBXSTATE_IDLE 0
10	#define UBXSTATE_SYNC1 1	10	#define UBXSTATE_SYNC1 1
11	#define UBXSTATE_SYNC2 2	11	#define UBXSTATE_SYNC2 2
12	#define UBXSTATE_CLASS 3	12	#define UBXSTATE_CLASS 3
13	#define UBXSTATE_LEN1 4	13	#define UBXSTATE_LEN1 4
14	#define UBXSTATE_LEN2 5	14	#define UBXSTATE_LEN2 5
15	#define UBXSTATE_DATA 6	15	#define UBXSTATE_DATA 6
16	#define UBXSTATE_CKA 7	16	#define UBXSTATE_CKA 7
17	#define UBXSTATE_CKB 8	17	#define UBXSTATE_CKB 8
18		18
19	// ublox protocoll identifier	19	// ublox protocoll identifier
20	#define UBX_CLASS_NAV 0x01	20	#define UBX_CLASS_NAV 0x01
21		21
22	#define UBX_ID_POSLLH 0x02	22	#define UBX_ID_POSLLH 0x02
23	#define UBX_ID_SOL 0x06	23	#define UBX_ID_SOL 0x06
24	#define UBX_ID_POSUTM 0x08	-
25	#define UBX_ID_VELNED 0x12	24	#define UBX_ID_VELNED 0x12
26		25
27	#define UBX_SYNC1_CHAR 0xB5	26	#define UBX_SYNC1_CHAR 0xB5
28	#define UBX_SYNC2_CHAR 0x62	27	#define UBX_SYNC2_CHAR 0x62
29		28
30	typedef struct {	29	typedef struct {
31	uint32_t ITOW; // ms GPS Millisecond Time of Week	30	uint32_t ITOW; // ms GPS Millisecond Time of Week
32	int32_t Frac; // ns remainder of rounded ms above	31	int32_t Frac; // ns remainder of rounded ms above
33	int16_t week; // GPS week	32	int16_t week; // GPS week
34	uint8_t GPSfix; // GPSfix Type, range 0..6	33	uint8_t GPSfix; // GPSfix Type, range 0..6
35	uint8_t Flags; // Navigation Status Flags	34	uint8_t Flags; // Navigation Status Flags
36	int32_t ECEF_X; // cm ECEF X coordinate	35	int32_t ECEF_X; // cm ECEF X coordinate
37	int32_t ECEF_Y; // cm ECEF Y coordinate	36	int32_t ECEF_Y; // cm ECEF Y coordinate
38	int32_t ECEF_Z; // cm ECEF Z coordinate	37	int32_t ECEF_Z; // cm ECEF Z coordinate
39	uint32_t PAcc; // cm 3D Position Accuracy Estimate	38	uint32_t PAcc; // cm 3D Position Accuracy Estimate
40	int32_t ECEFVX; // cm/s ECEF X velocity	39	int32_t ECEFVX; // cm/s ECEF X velocity
41	int32_t ECEFVY; // cm/s ECEF Y velocity	40	int32_t ECEFVY; // cm/s ECEF Y velocity
42	int32_t ECEFVZ; // cm/s ECEF Z velocity	41	int32_t ECEFVZ; // cm/s ECEF Z velocity
43	uint32_t SAcc; // cm/s Speed Accuracy Estimate	42	uint32_t SAcc; // cm/s Speed Accuracy Estimate
44	uint16_t PDOP; // 0.01 Position DOP	43	uint16_t PDOP; // 0.01 Position DOP
45	uint8_t res1; // reserved	44	uint8_t res1; // reserved
46	uint8_t numSV; // Number of SVs used in navigation solution	45	uint8_t numSV; // Number of SVs used in navigation solution
47	uint32_t res2; // reserved	46	uint32_t res2; // reserved
48	uint8_t Status;	47	uint8_t Status;
49	} GPS_SOL_t;	48	} GPS_SOL_t;
50		49
51	typedef struct {	50	typedef struct {
52	uint32_t ITOW; // ms GPS Millisecond Time of Week	51	uint32_t ITOW; // ms GPS Millisecond Time of Week
53	int32_t LON; // 1e-07 deg Longitude	52	int32_t LON; // 1e-07 deg Longitude
54	int32_t LAT; // 1e-07 deg Latitude	53	int32_t LAT; // 1e-07 deg Latitude
55	int32_t HEIGHT; // mm Height above Ellipsoid	54	int32_t HEIGHT; // mm Height above Ellipsoid
56	int32_t HMSL; // mm Height above mean sea level	55	int32_t HMSL; // mm Height above mean sea level
57	uint32_t Hacc; // mm Horizontal Accuracy Estimate	56	uint32_t Hacc; // mm Horizontal Accuracy Estimate
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;
61		60
62	typedef struct {	61	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 {	-
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
77	uint32_t Speed; // cm/s Speed (3-D)	66	uint32_t Speed; // cm/s Speed (3-D)
78	uint32_t GSpeed; // cm/s Ground Speed (2-D)	67	uint32_t GSpeed; // cm/s Ground Speed (2-D)
79	int32_t Heading; // 1e-05 deg Heading 2-D	68	int32_t Heading; // 1e-05 deg Heading 2-D
80	uint32_t SAcc; // cm/s Speed Accuracy Estimate	69	uint32_t SAcc; // cm/s Speed Accuracy Estimate
81	uint32_t CAcc; // deg Course / Heading Accuracy Estimate	70	uint32_t CAcc; // deg Course / Heading Accuracy Estimate
82	uint8_t Status;	71	uint8_t Status;
83	} GPS_VELNED_t;	72	} GPS_VELNED_t;
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};	75	GPS_POSLLH_t GpsPosLlh = {0,0,0,0,0,0,0, INVALID};
87	GPS_POSUTM_t GpsPosUtm = {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};
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};
90		78
91	volatile uint8_t GPSTimeout = 0;	79	volatile uint8_t GPSTimeout = 0;
92		80
93	void UpdateGPSInfo (void)	81	void UpdateGPSInfo (void)
94	{	82	{
95	if (GpsSol.Status == VALID) // valid packet	83	if (GpsSol.Status == VALID) // valid packet
96	{	84	{
97	GPSInfo.satfix = GpsSol.GPSfix;	85	GPSInfo.satfix = GpsSol.GPSfix;
98	GPSInfo.satnum = GpsSol.numSV;	86	GPSInfo.satnum = GpsSol.numSV;
99	GPSInfo.PAcc = GpsSol.PAcc;	87	GPSInfo.PAcc = GpsSol.PAcc;
100	GPSInfo.VAcc = GpsSol.SAcc;	88	GPSInfo.VAcc = GpsSol.SAcc;
101	GpsSol.Status = PROCESSED; // never update old data	89	GpsSol.Status = PROCESSED; // never update old data
102	}	90	}
103	if (GpsPosLlh.Status == VALID) // valid packet	91	if (GpsPosLlh.Status == VALID) // valid packet
104	{	92	{
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	}
129		110
130		111
131	// this function should be called within the UART RX ISR	112	// this function should be called within the UART RX ISR
132	void ubx_parser(uint8_t c)	113	void ubx_parser(uint8_t c)
133	{	114	{
134	static uint8_t ubxstate = UBXSTATE_IDLE;	115	static uint8_t ubxstate = UBXSTATE_IDLE;
135	static uint8_t cka, ckb;	116	static uint8_t cka, ckb;
136	static uint16_t msglen;	117	static uint16_t msglen;
137	static int8_t ubxP, ubxEp, *ubxSp; // pointers to data currently transfered	118	static int8_t ubxP, ubxEp, *ubxSp; // pointers to data currently transfered
138		119
139	switch(ubxstate)	120	switch(ubxstate)
140	{	121	{
141	case UBXSTATE_IDLE: // check 1st sync byte	122	case UBXSTATE_IDLE: // check 1st sync byte
142	if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1;	123	if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1;
143	else ubxstate = UBXSTATE_IDLE; // out of synchronization	124	else ubxstate = UBXSTATE_IDLE; // out of synchronization
144	break;	125	break;
145		126
146	case UBXSTATE_SYNC1: // check 2nd sync byte	127	case UBXSTATE_SYNC1: // check 2nd sync byte
147	if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2;	128	if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2;
148	else ubxstate = UBXSTATE_IDLE; // out of synchronization	129	else ubxstate = UBXSTATE_IDLE; // out of synchronization
149	break;	130	break;
150		131
151	case UBXSTATE_SYNC2: // check msg class to be NAV	132	case UBXSTATE_SYNC2: // check msg class to be NAV
152	if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS;	133	if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS;
153	else ubxstate = UBXSTATE_IDLE; // unsupported message class	134	else ubxstate = UBXSTATE_IDLE; // unsupported message class
154	break;	135	break;
155		136
156	case UBXSTATE_CLASS: // check message identifier	137	case UBXSTATE_CLASS: // check message identifier
157	switch(c)	138	switch(c)
158	{	139	{
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		-
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
169	break;	144	break;
170		145
171	case UBX_ID_SOL: // navigation solution	146	case UBX_ID_SOL: // navigation solution
172	ubxP = (int8_t *)&GpsSol; // data start pointer	147	ubxP = (int8_t *)&GpsSol; // data start pointer
173	ubxEp = (int8_t *)(&GpsSol + sizeof(GPS_SOL_t)); // data end pointer	148	ubxEp = (int8_t *)(&GpsSol + sizeof(GPS_SOL_t)); // data end pointer
174	ubxSp = (int8_t *)&GpsSol.Status; // status pointer	149	ubxSp = (int8_t *)&GpsSol.Status; // status pointer
175	break;	150	break;
176		151
177	case UBX_ID_VELNED: // velocity vector in tangent plane	152	case UBX_ID_VELNED: // velocity vector in tangent plane
178	ubxP = (int8_t *)&GpsVelNed; // data start pointer	153	ubxP = (int8_t *)&GpsVelNed; // data start pointer
179	ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer	154	ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer
180	ubxSp = (int8_t *)&GpsVelNed.Status; // status pointer	155	ubxSp = (int8_t *)&GpsVelNed.Status; // status pointer
181	break;	156	break;
182		157
183	default: // unsupported identifier	158	default: // unsupported identifier
184	ubxstate = UBXSTATE_IDLE;	159	ubxstate = UBXSTATE_IDLE;
185	break;	160	break;
186	}	161	}
187	if (ubxstate != UBXSTATE_IDLE)	162	if (ubxstate != UBXSTATE_IDLE)
188	{	163	{
189	ubxstate = UBXSTATE_LEN1;	164	ubxstate = UBXSTATE_LEN1;
190	cka = UBX_CLASS_NAV + c;	165	cka = UBX_CLASS_NAV + c;
191	ckb = UBX_CLASS_NAV + cka;	166	ckb = UBX_CLASS_NAV + cka;
192	}	167	}
193	break;	168	break;
194		169
195	case UBXSTATE_LEN1: // 1st message length byte	170	case UBXSTATE_LEN1: // 1st message length byte
196	msglen = c;	171	msglen = c;
197	cka += c;	172	cka += c;
198	ckb += cka;	173	ckb += cka;
199	ubxstate = UBXSTATE_LEN2;	174	ubxstate = UBXSTATE_LEN2;
200	break;	175	break;
201		176
202	case UBXSTATE_LEN2: // 2nd message length byte	177	case UBXSTATE_LEN2: // 2nd message length byte
203	msglen += ((uint16_t)c)<<8;	178	msglen += ((uint16_t)c)<<8;
204	cka += c;	179	cka += c;
205	ckb += cka;	180	ckb += cka;
206	// if the old data are not processed so far then break parsing now	181	// if the old data are not processed so far then break parsing now
207	// to avoid writing new data in ISR during reading by another function	182	// to avoid writing new data in ISR during reading by another function
208	if ( *ubxSp == VALID ) ubxstate = UBXSTATE_IDLE;	183	if ( *ubxSp == VALID ) ubxstate = UBXSTATE_IDLE;
209	else // data invalid or allready processd	184	else // data invalid or allready processd
210	{	185	{
211	*ubxSp = INVALID;	186	*ubxSp = INVALID;
212	ubxstate = UBXSTATE_DATA;	187	ubxstate = UBXSTATE_DATA;
213	}	188	}
214	break;	189	break;
215		190
216	case UBXSTATE_DATA:	191	case UBXSTATE_DATA:
217	if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left	192	if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left
218	cka += c;	193	cka += c;
219	ckb += cka;	194	ckb += cka;
220	if (--msglen == 0) ubxstate = UBXSTATE_CKA; // switch to next state if all data was read	195	if (--msglen == 0) ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
221	break;	196	break;
222		197
223	case UBXSTATE_CKA:	198	case UBXSTATE_CKA:
224	if (c == cka) ubxstate = UBXSTATE_CKB;	199	if (c == cka) ubxstate = UBXSTATE_CKB;
225	else	200	else
226	{	201	{
227	*ubxSp = INVALID;	202	*ubxSp = INVALID;
228	ubxstate = UBXSTATE_IDLE;	203	ubxstate = UBXSTATE_IDLE;
229	}	204	}
230	break;	205	break;
231		206
232	case UBXSTATE_CKB:	207	case UBXSTATE_CKB:
233	if (c == ckb)	208	if (c == ckb)
234	{	209	{
235	*ubxSp = VALID; // new data are valid	210	*ubxSp = VALID; // new data are valid
236	ROT_FLASH;	211	ROT_FLASH;
237	UpdateGPSInfo(); //update GPS info respectively	212	UpdateGPSInfo(); //update GPS info respectively
238	GPSTimeout = 255;	213	GPSTimeout = 255;
239	}	214	}
240	else	215	else
241	{ // if checksum not fit then set data invalid	216	{ // if checksum not fit then set data invalid
242	*ubxSp = INVALID;	217	*ubxSp = INVALID;
243	}	218	}
244	ubxstate = UBXSTATE_IDLE; // ready to parse new data	219	ubxstate = UBXSTATE_IDLE; // ready to parse new data
245	break;	220	break;
246		221
247	default: // unknown ubx state	222	default: // unknown ubx state
248	ubxstate = UBXSTATE_IDLE;	223	ubxstate = UBXSTATE_IDLE;
249	break;	224	break;
250	}	225	}
251		226
252	}	227	}
253		228
254		229
255		230

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/ubx.c - Rev 752 → 781