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


#include <inttypes.h>
#include "ubx.h"
#include "main.h"
#include <avr/io.h>
 
#include "uart.h"
#include "timer0.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_POSUTM   0x08
#define UBX_ID_STATUS   0x03
#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
        uint8_t                 GPSfix;         // GPSfix Type, range 0..6
        uint8_t                 Flags;          // Navigation Status Flags
        uint8_t                 DiffS;          // Differential Status
        uint8_t                 res;            // reserved
        uint32_t                TTFF;           // Time to first fix (millisecond time tag)
        uint32_t                MSSS;           // Milliseconds since Startup / Reset
        uint8_t                 Status;
} GPS_STATUS_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                 EAST;           // cm  UTM Easting
        int32_t                 NORTH;          // cm  UTM Nording
        int32_t                 ALT;            // cm  altitude
        uint8_t                 ZONE;           // UTM zone number
        uint8_t                 HEM;            // Hemisphere Indicator (0=North, 1=South)
        uint8_t                 Status;
} GPS_POSUTM_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_STATUS_t    GpsStatus;
GPS_POSLLH_t    GpsPosLlh;
GPS_POSUTM_t    GpsPosUtm;
GPS_VELNED_t    GpsVelNed;
 
GPS_INFO_t GPSInfo;
 
void UpdateGPSInfo (void)
{
        if (GpsStatus.Status == VALID)                  // valid packet
        {
                GPSInfo.satfix = GpsStatus.GPSfix;
                GpsStatus.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 (GpsPosUtm.Status == VALID)                  // valid packet
        {
                GPSInfo.utmeast = GpsPosUtm.EAST;
                GPSInfo.utmnorth = GpsPosUtm.NORTH;
                GPSInfo.utmalt = GpsPosUtm.ALT;
                GpsPosUtm.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;
                GpsPosUtm.Status = PROCESSED;                   // never update old data
        }
        if (GpsStatus.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_POSUTM: // utm position
                                        ubxP =  (int8_t *)&GpsPosUtm; // data start pointer
                                        ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer
                                        ubxSp = (int8_t *)&GpsPosUtm.Status; // status pointer
 
                                        break;
 
                                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_STATUS: // receiver status
                                        ubxP =  (int8_t *)&GpsStatus; // data start pointer
                                        ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer
                                        ubxSp = (int8_t *)&GpsStatus.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
                        }
                        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 702	Rev 722
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
-		6	#include "uart.h"
-		7	#include "timer0.h"
5		8
6	// ubx protocol parser state machine	9	// ubx protocol parser state machine
7	#define UBXSTATE_IDLE 0	10	#define UBXSTATE_IDLE 0
8	#define UBXSTATE_SYNC1 1	11	#define UBXSTATE_SYNC1 1
9	#define UBXSTATE_SYNC2 2	12	#define UBXSTATE_SYNC2 2
10	#define UBXSTATE_CLASS 3	13	#define UBXSTATE_CLASS 3
11	#define UBXSTATE_LEN1 4	14	#define UBXSTATE_LEN1 4
12	#define UBXSTATE_LEN2 5	15	#define UBXSTATE_LEN2 5
13	#define UBXSTATE_DATA 6	16	#define UBXSTATE_DATA 6
14	#define UBXSTATE_CKA 7	17	#define UBXSTATE_CKA 7
15	#define UBXSTATE_CKB 8	18	#define UBXSTATE_CKB 8
16		19
17	// ublox protocoll identifier	20	// ublox protocoll identifier
18	#define UBX_CLASS_NAV 0x01	21	#define UBX_CLASS_NAV 0x01
19	#define UBX_ID_POSLLH 0x02	22	#define UBX_ID_POSLLH 0x02
20	#define UBX_ID_POSUTM 0x08	23	#define UBX_ID_POSUTM 0x08
21	#define UBX_ID_STATUS 0x03	24	#define UBX_ID_STATUS 0x03
22	#define UBX_ID_VELNED 0x12	25	#define UBX_ID_VELNED 0x12
23	#define UBX_SYNC1_CHAR 0xB5	26	#define UBX_SYNC1_CHAR 0xB5
24	#define UBX_SYNC2_CHAR 0x62	27	#define UBX_SYNC2_CHAR 0x62
25		28
26		29
27	typedef struct {	30	typedef struct {
28	uint32_t ITOW; // ms GPS Millisecond Time of Week	31	uint32_t ITOW; // ms GPS Millisecond Time of Week
29	uint8_t GPSfix; // GPSfix Type, range 0..6	32	uint8_t GPSfix; // GPSfix Type, range 0..6
30	uint8_t Flags; // Navigation Status Flags	33	uint8_t Flags; // Navigation Status Flags
31	uint8_t DiffS; // Differential Status	34	uint8_t DiffS; // Differential Status
32	uint8_t res; // reserved	35	uint8_t res; // reserved
33	uint32_t TTFF; // Time to first fix (millisecond time tag)	36	uint32_t TTFF; // Time to first fix (millisecond time tag)
34	uint32_t MSSS; // Milliseconds since Startup / Reset	37	uint32_t MSSS; // Milliseconds since Startup / Reset
35	uint8_t Status;	38	uint8_t Status;
36	} GPS_STATUS_t;	39	} GPS_STATUS_t;
37		40
38	typedef struct {	41	typedef struct {
39	uint32_t ITOW; // ms GPS Millisecond Time of Week	42	uint32_t ITOW; // ms GPS Millisecond Time of Week
40	int32_t LON; // 1e-07 deg Longitude	43	int32_t LON; // 1e-07 deg Longitude
41	int32_t LAT; // 1e-07 deg Latitude	44	int32_t LAT; // 1e-07 deg Latitude
42	int32_t HEIGHT; // mm Height above Ellipsoid	45	int32_t HEIGHT; // mm Height above Ellipsoid
43	int32_t HMSL; // mm Height above mean sea level	46	int32_t HMSL; // mm Height above mean sea level
44	uint32_t Hacc; // mm Horizontal Accuracy Estimate	47	uint32_t Hacc; // mm Horizontal Accuracy Estimate
45	uint32_t Vacc; // mm Vertical Accuracy Estimate	48	uint32_t Vacc; // mm Vertical Accuracy Estimate
46	uint8_t Status;	49	uint8_t Status;
47	} GPS_POSLLH_t;	50	} GPS_POSLLH_t;
48		51
49	typedef struct {	52	typedef struct {
50	uint32_t ITOW; // ms GPS Millisecond Time of Week	53	uint32_t ITOW; // ms GPS Millisecond Time of Week
51	int32_t EAST; // cm UTM Easting	54	int32_t EAST; // cm UTM Easting
52	int32_t NORTH; // cm UTM Nording	55	int32_t NORTH; // cm UTM Nording
53	int32_t ALT; // cm altitude	56	int32_t ALT; // cm altitude
54	uint8_t ZONE; // UTM zone number	57	uint8_t ZONE; // UTM zone number
55	uint8_t HEM; // Hemisphere Indicator (0=North, 1=South)	58	uint8_t HEM; // Hemisphere Indicator (0=North, 1=South)
56	uint8_t Status;	59	uint8_t Status;
57	} GPS_POSUTM_t;	60	} GPS_POSUTM_t;
58		61
59	typedef struct {	62	typedef struct {
60	uint32_t ITOW; // ms GPS Millisecond Time of Week	63	uint32_t ITOW; // ms GPS Millisecond Time of Week
61	int32_t VEL_N; // cm/s NED north velocity	64	int32_t VEL_N; // cm/s NED north velocity
62	int32_t VEL_E; // cm/s NED east velocity	65	int32_t VEL_E; // cm/s NED east velocity
63	int32_t VEL_D; // cm/s NED down velocity	66	int32_t VEL_D; // cm/s NED down velocity
64	uint32_t Speed; // cm/s Speed (3-D)	67	uint32_t Speed; // cm/s Speed (3-D)
65	uint32_t GSpeed; // cm/s Ground Speed (2-D)	68	uint32_t GSpeed; // cm/s Ground Speed (2-D)
66	int32_t Heading; // 1e-05 deg Heading 2-D	69	int32_t Heading; // 1e-05 deg Heading 2-D
67	uint32_t SAcc; // cm/s Speed Accuracy Estimate	70	uint32_t SAcc; // cm/s Speed Accuracy Estimate
68	uint32_t CAcc; // deg Course / Heading Accuracy Estimate	71	uint32_t CAcc; // deg Course / Heading Accuracy Estimate
69	uint8_t Status;	72	uint8_t Status;
70	} GPS_VELNED_t;	73	} GPS_VELNED_t;
71		74
72		75
73	GPS_STATUS_t GpsStatus;	76	GPS_STATUS_t GpsStatus;
74	GPS_POSLLH_t GpsPosLlh;	77	GPS_POSLLH_t GpsPosLlh;
75	GPS_POSUTM_t GpsPosUtm;	78	GPS_POSUTM_t GpsPosUtm;
76	GPS_VELNED_t GpsVelNed;	79	GPS_VELNED_t GpsVelNed;
77		80
78	GPS_INFO_t GPSInfo;	81	GPS_INFO_t GPSInfo;
79		82
80	void UpdateGPSInfo (void)	83	void UpdateGPSInfo (void)
81	{	84	{
82	if (GpsStatus.Status == VALID) // valid packet	85	if (GpsStatus.Status == VALID) // valid packet
83	{	86	{
84	GPSInfo.satfix = GpsStatus.GPSfix;	87	GPSInfo.satfix = GpsStatus.GPSfix;
85	GpsStatus.Status = PROCESSED; // never update old data	88	GpsStatus.Status = PROCESSED; // never update old data
86	}	89	}
87	if (GpsPosLlh.Status == VALID) // valid packet	90	if (GpsPosLlh.Status == VALID) // valid packet
88	{	91	{
89	GPSInfo.longitude = GpsPosLlh.LON;	92	GPSInfo.longitude = GpsPosLlh.LON;
90	GPSInfo.latitude = GpsPosLlh.LAT;	93	GPSInfo.latitude = GpsPosLlh.LAT;
91	GPSInfo.altitude = GpsPosLlh.HEIGHT;	94	GPSInfo.altitude = GpsPosLlh.HEIGHT;
92	GpsPosLlh.Status = PROCESSED; // never update old data	95	GpsPosLlh.Status = PROCESSED; // never update old data
93	}	96	}
94	if (GpsPosUtm.Status == VALID) // valid packet	97	if (GpsPosUtm.Status == VALID) // valid packet
95	{	98	{
96	GPSInfo.utmeast = GpsPosUtm.EAST;	99	GPSInfo.utmeast = GpsPosUtm.EAST;
97	GPSInfo.utmnorth = GpsPosUtm.NORTH;	100	GPSInfo.utmnorth = GpsPosUtm.NORTH;
98	GPSInfo.utmalt = GpsPosUtm.ALT;	101	GPSInfo.utmalt = GpsPosUtm.ALT;
99	GpsPosUtm.Status = PROCESSED; // never update old data	102	GpsPosUtm.Status = PROCESSED; // never update old data
100	}	103	}
101	if (GpsVelNed.Status == VALID) // valid packet	104	if (GpsVelNed.Status == VALID) // valid packet
102	{	105	{
103	GPSInfo.veleast = GpsVelNed.VEL_E;	106	GPSInfo.veleast = GpsVelNed.VEL_E;
104	GPSInfo.velnorth = GpsVelNed.VEL_N;	107	GPSInfo.velnorth = GpsVelNed.VEL_N;
105	GPSInfo.veltop = -GpsVelNed.VEL_D;	108	GPSInfo.veltop = -GpsVelNed.VEL_D;
106	GpsPosUtm.Status = PROCESSED; // never update old data	109	GpsPosUtm.Status = PROCESSED; // never update old data
107	}	110	}
108	if (GpsStatus.Status \| GpsVelNed.Status \| GpsPosLlh.Status \| GpsPosUtm.Status)	111	if (GpsStatus.Status != INVALID)
109	{	112	{
110	GPSInfo.status = VALID;	113	GPSInfo.status = VALID; // set valid if data are updated
111	}	114	}
112	}	115	}
113		116
114	// this function should be called within the UART RX ISR	117	// this function should be called within the UART RX ISR
115	void ubx_parser(uint8_t c)	118	void ubx_parser(uint8_t c)
116	{	119	{
117	static uint8_t ubxstate = UBXSTATE_IDLE;	120	static uint8_t ubxstate = UBXSTATE_IDLE;
118	static uint8_t cka, ckb;	121	static uint8_t cka, ckb;
119	static uint16_t msglen;	122	static uint16_t msglen;
120	static int8_t ubxP, ubxEp, *ubxSp; // pointers to data currently transfered	123	static int8_t ubxP, ubxEp, *ubxSp; // pointers to data currently transfered
121		124
122	switch(ubxstate)	125	switch(ubxstate)
123	{	126	{
124	case UBXSTATE_IDLE: // check 1st sync byte	127	case UBXSTATE_IDLE: // check 1st sync byte
125	if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1;	128	if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1;
126	else ubxstate = UBXSTATE_IDLE; // out of synchronization	129	else ubxstate = UBXSTATE_IDLE; // out of synchronization
127	break;	130	break;
128		131
129	case UBXSTATE_SYNC1: // check 2nd sync byte	132	case UBXSTATE_SYNC1: // check 2nd sync byte
130	if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2;	133	if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2;
131	else ubxstate = UBXSTATE_IDLE; // out of synchronization	134	else ubxstate = UBXSTATE_IDLE; // out of synchronization
132	break;	135	break;
133		136
134	case UBXSTATE_SYNC2: // check msg class to be NAV	137	case UBXSTATE_SYNC2: // check msg class to be NAV
135	if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS;	138	if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS;
136	else ubxstate = UBXSTATE_IDLE; // unsupported message class	139	else ubxstate = UBXSTATE_IDLE; // unsupported message class
137	break;	140	break;
138		141
139	case UBXSTATE_CLASS: // check message identifier	142	case UBXSTATE_CLASS: // check message identifier
140	switch(c)	143	switch(c)
141	{	144	{
142	case UBX_ID_POSUTM: // utm position	145	case UBX_ID_POSUTM: // utm position
143	ubxP = (int8_t *)&GpsPosUtm; // data start pointer	146	ubxP = (int8_t *)&GpsPosUtm; // data start pointer
144	ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer	147	ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer
145	ubxSp = (int8_t *)&GpsPosUtm.Status; // status pointer	148	ubxSp = (int8_t *)&GpsPosUtm.Status; // status pointer
146		-
147	break;	149	break;
148		150
149	case UBX_ID_POSLLH: // geodetic position	151	case UBX_ID_POSLLH: // geodetic position
150	ubxP = (int8_t *)&GpsStatus; // data start pointer	152	ubxP = (int8_t *)&GpsPosLlh; // data start pointer
151	ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer	153	ubxEp = (int8_t *)(&GpsPosLlh + sizeof(GPS_POSLLH_t)); // data end pointer
152	ubxSp = (int8_t *)&GpsStatus.Status; // status pointer	154	ubxSp = (int8_t *)&GpsPosLlh.Status; // status pointer
153	break;	155	break;
154		156
155	case UBX_ID_STATUS: // receiver status	157	case UBX_ID_STATUS: // receiver status
156	ubxP = (int8_t *)&GpsStatus; // data start pointer	158	ubxP = (int8_t *)&GpsStatus; // data start pointer
157	ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer	159	ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer
158	ubxSp = (int8_t *)&GpsStatus.Status; // status pointer	160	ubxSp = (int8_t *)&GpsStatus.Status; // status pointer
159	break;	161	break;
160		162
161	case UBX_ID_VELNED: // velocity vector in tangent plane	163	case UBX_ID_VELNED: // velocity vector in tangent plane
162	ubxP = (int8_t *)&GpsVelNed; // data start pointer	164	ubxP = (int8_t *)&GpsVelNed; // data start pointer
163	ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer	165	ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer
164	ubxSp = (int8_t *)&GpsVelNed.Status; // status pointer	166	ubxSp = (int8_t *)&GpsVelNed.Status; // status pointer
165	break;	167	break;
166		168
167	default: // unsupported identifier	169	default: // unsupported identifier
168	ubxstate = UBXSTATE_IDLE;	170	ubxstate = UBXSTATE_IDLE;
169	break;	171	break;
170	}	172	}
171	if (ubxstate != UBXSTATE_IDLE)	173	if (ubxstate != UBXSTATE_IDLE)
172	{	174	{
173	ubxstate = UBXSTATE_LEN1;	175	ubxstate = UBXSTATE_LEN1;
174	cka = UBX_CLASS_NAV + c;	176	cka = UBX_CLASS_NAV + c;
175	ckb = UBX_CLASS_NAV + cka;	177	ckb = UBX_CLASS_NAV + cka;
176	}	178	}
177	break;	179	break;
178		180
179	case UBXSTATE_LEN1: // 1st message length byte	181	case UBXSTATE_LEN1: // 1st message length byte
180	msglen = c;	182	msglen = c;
181	cka += c;	183	cka += c;
182	ckb += cka;	184	ckb += cka;
183	ubxstate = UBXSTATE_LEN2;	185	ubxstate = UBXSTATE_LEN2;
184	break;	186	break;
185		187
186	case UBXSTATE_LEN2: // 2nd message length byte	188	case UBXSTATE_LEN2: // 2nd message length byte
187	msglen += ((uint16_t)c)<<8;	189	msglen += ((uint16_t)c)<<8;
188	cka += c;	190	cka += c;
189	ckb += cka;	191	ckb += cka;
190	// if the old data are not processed so far then break parsing now	192	// if the old data are not processed so far then break parsing now
191	// to avoid writing new data in ISR during reading by another function	193	// to avoid writing new data in ISR during reading by another function
192	if ( *ubxSp == VALID ) ubxstate = UBXSTATE_IDLE;	194	if ( *ubxSp == VALID ) ubxstate = UBXSTATE_IDLE;
193	else // data invalid or allready processd	195	else // data invalid or allready processd
194	{	196	{
195	*ubxSp = INVALID;	197	*ubxSp = INVALID;
196	ubxstate = UBXSTATE_DATA;	198	ubxstate = UBXSTATE_DATA;
197	}	199	}
198	break;	200	break;
199		201
200	case UBXSTATE_DATA:	202	case UBXSTATE_DATA:
201	if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left	203	if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left
202	cka += c;	204	cka += c;
203	ckb += cka;	205	ckb += cka;
204	if (--msglen == 0) ubxstate = UBXSTATE_CKA; // switch to next state if all data was read	206	if (--msglen == 0) ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
205	break;	207	break;
206		208
207	case UBXSTATE_CKA:	209	case UBXSTATE_CKA:
208	if (c == cka) ubxstate = UBXSTATE_CKB;	210	if (c == cka) ubxstate = UBXSTATE_CKB;
-		211	else
-		212	{
-		213	*ubxSp = INVALID;
209	else ubxstate = UBXSTATE_IDLE;	214	ubxstate = UBXSTATE_IDLE;
-		215	}
210	break;	216	break;
211		217
212	case UBXSTATE_CKB:	218	case UBXSTATE_CKB:
213	if (c == ckb)	219	if (c == ckb)
214	{	220	{
215	*ubxSp = VALID; // new data are availabe	221	*ubxSp = VALID; // new data are valid
216	UpdateGPSInfo(); //update GPS info respectively	-
217	ROT_FLASH;	222	ROT_FLASH;
-		223	UpdateGPSInfo(); //update GPS info respectively
218		224	}
-		225	else
-		226	{ // if checksum not fit then set data invalid
-		227	*ubxSp = INVALID;
219	}	228	}
220	ubxstate = UBXSTATE_IDLE; // ready to parse new data	229	ubxstate = UBXSTATE_IDLE; // ready to parse new data
221	break;	230	break;
222		231
223	default: // unknown ubx state	232	default: // unknown ubx state
224	ubxstate = UBXSTATE_IDLE;	233	ubxstate = UBXSTATE_IDLE;
225	break;	234	break;
226	}	235	}
227		236
228	}	237	}
229		238
230		239

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/ubx.c @ 1358 - Rev 702 → 722