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

Subversion Repositories FlightCtrl

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