WebSVN - FlightCtrl - Diff - Rev 2059 and 2079 - /branches/dongfang_FC


#include <inttypes.h>
#include "ubx.h"
#include <avr/io.h>
#include "output.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
  Status_t status;
} UBX_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 vAacc; // mm Vertical Accuracy Estimate
  Status_t status;
} UBX_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
  Status_t status;
} UBX_VELNED_t;
 
UBX_SOL_t ubxSol = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
UBX_POSLLH_t ubxPosLlh = { 0, 0, 0, 0, 0, 0, 0, INVALID };
UBX_VELNED_t ubxVelNed = { 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;
volatile uint16_t GPSDatasetCounter = 0;
 
void updateGPSInfo(void) {
  if ((ubxSol.status == NEWDATA) && (ubxPosLlh.status == NEWDATA) && (ubxVelNed.status == NEWDATA)) {
    //RED_FLASH;
    // DebugOut.Digital ....blah...
    if (GPSInfo.status != NEWDATA) {
      GPSDatasetCounter++;
      GPSInfo.status = INVALID;
      // NAV SOL
      GPSInfo.flags =   ubxSol.flags;
      GPSInfo.satfix =  ubxSol.GPSfix;
      GPSInfo.satnum =  ubxSol.numSV;
      GPSInfo.PAcc =    ubxSol.pAcc;
      GPSInfo.VAcc =    ubxSol.sAcc;
      debugOut.analog[31] = GPSInfo.VAcc/1000L;
 
      // NAV POSLLH
      GPSInfo.longitude =   ubxPosLlh.lon;
      GPSInfo.latitude =    ubxPosLlh.lat;
      GPSInfo.altitude =    ubxPosLlh.height;
 
      GPSInfo.veleast =     ubxVelNed.VEL_E;
      GPSInfo.velnorth =    ubxVelNed.VEL_N;
      GPSInfo.veltop =     -ubxVelNed.VEL_D;
      GPSInfo.velground =   ubxVelNed.gSpeed;
 
      GPSInfo.status = NEWDATA;
    }
 
    // set state to collect new data
    ubxSol.status = PROCESSED; // never update old data
    ubxPosLlh.status = PROCESSED; // never update old data
    ubxVelNed.status = PROCESSED; // never update old data
  }
}
 
// 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 *)     &ubxPosLlh; // data start pointer
      ubxEp = (int8_t *)   (&ubxPosLlh + 1); // data end pointer
      ubxSp = (int8_t *)    &ubxPosLlh.status; // status pointer
      break;
 
    case UBX_ID_SOL: // navigation solution
      ubxP = (int8_t *)     &ubxSol; // data start pointer
      ubxEp = (int8_t *)   (&ubxSol + 1); // data end pointer
      ubxSp = (int8_t *)    &ubxSol.status; // status pointer
      break;
 
    case UBX_ID_VELNED: // velocity vector in tangent plane
      ubxP = (int8_t *)     &ubxVelNed; // data start pointer
      ubxEp = (int8_t *)   (&ubxVelNed + 1); // data end pointer
      ubxSp = (int8_t *)    &ubxVelNed.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 == NEWDATA) {
      updateGPSInfo(); //update GPS info respectively
      ubxstate = UBXSTATE_IDLE;
    } else {// data invalid or already 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 = NEWDATA; // new data are valid
      updateGPSInfo(); //update GPS info respectively
      GPSTimeout = 255;
    } else { // if checksum not match then set data invalid
      *ubxSp = INVALID;
    }
    ubxstate = UBXSTATE_IDLE; // ready to parse new data
    break;
 
  default: // unknown ubx state
    ubxstate = UBXSTATE_IDLE;
    break;
  }
}
 

Rev 2059	Rev 2079
1	#include <inttypes.h>	1	#include <inttypes.h>
2	#include "ubx.h"	2	#include "ubx.h"
3	#include <avr/io.h>	3	#include <avr/io.h>
4	#include "output.h"	4	#include "output.h"
5		5
6	// ubx protocol parser state machine	6	// ubx protocol parser state machine
7	#define UBXSTATE_IDLE 0	7	#define UBXSTATE_IDLE 0
8	#define UBXSTATE_SYNC1 1	8	#define UBXSTATE_SYNC1 1
9	#define UBXSTATE_SYNC2 2	9	#define UBXSTATE_SYNC2 2
10	#define UBXSTATE_CLASS 3	10	#define UBXSTATE_CLASS 3
11	#define UBXSTATE_LEN1 4	11	#define UBXSTATE_LEN1 4
12	#define UBXSTATE_LEN2 5	12	#define UBXSTATE_LEN2 5
13	#define UBXSTATE_DATA 6	13	#define UBXSTATE_DATA 6
14	#define UBXSTATE_CKA 7	14	#define UBXSTATE_CKA 7
15	#define UBXSTATE_CKB 8	15	#define UBXSTATE_CKB 8
16		16
17	// ublox protocoll identifier	17	// ublox protocoll identifier
18	#define UBX_CLASS_NAV 0x01	18	#define UBX_CLASS_NAV 0x01
19		19
20	#define UBX_ID_POSLLH 0x02	20	#define UBX_ID_POSLLH 0x02
21	#define UBX_ID_SOL 0x06	21	#define UBX_ID_SOL 0x06
22	#define UBX_ID_VELNED 0x12	22	#define UBX_ID_VELNED 0x12
23		23
24	#define UBX_SYNC1_CHAR 0xB5	24	#define UBX_SYNC1_CHAR 0xB5
25	#define UBX_SYNC2_CHAR 0x62	25	#define UBX_SYNC2_CHAR 0x62
26		26
27	typedef struct {	27	typedef struct {
28	uint32_t ITOW; // ms GPS Millisecond Time of Week	28	uint32_t ITOW; // ms GPS Millisecond Time of Week
29	int32_t frac; // ns remainder of rounded ms above	29	int32_t frac; // ns remainder of rounded ms above
30	int16_t week; // GPS week	30	int16_t week; // GPS week
31	uint8_t GPSfix; // GPSfix Type, range 0..6	31	uint8_t GPSfix; // GPSfix Type, range 0..6
32	uint8_t flags; // Navigation Status Flags	32	uint8_t flags; // Navigation Status Flags
33	int32_t ECEF_X; // cm ECEF X coordinate	33	int32_t ECEF_X; // cm ECEF X coordinate
34	int32_t ECEF_Y; // cm ECEF Y coordinate	34	int32_t ECEF_Y; // cm ECEF Y coordinate
35	int32_t ECEF_Z; // cm ECEF Z coordinate	35	int32_t ECEF_Z; // cm ECEF Z coordinate
36	uint32_t pAcc; // cm 3D Position Accuracy Estimate	36	uint32_t pAcc; // cm 3D Position Accuracy Estimate
37	int32_t ECEFVX; // cm/s ECEF X velocity	37	int32_t ECEFVX; // cm/s ECEF X velocity
38	int32_t ECEFVY; // cm/s ECEF Y velocity	38	int32_t ECEFVY; // cm/s ECEF Y velocity
39	int32_t ECEFVZ; // cm/s ECEF Z velocity	39	int32_t ECEFVZ; // cm/s ECEF Z velocity
40	uint32_t sAcc; // cm/s Speed Accuracy Estimate	40	uint32_t sAcc; // cm/s Speed Accuracy Estimate
41	uint16_t PDOP; // 0.01 Position DOP	41	uint16_t PDOP; // 0.01 Position DOP
42	uint8_t res1; // reserved	42	uint8_t res1; // reserved
43	uint8_t numSV; // Number of SVs used in navigation solution	43	uint8_t numSV; // Number of SVs used in navigation solution
44	uint32_t res2; // reserved	44	uint32_t res2; // reserved
45	Status_t status;	45	Status_t status;
46	} UBX_SOL_t;	46	} UBX_SOL_t;
47		47
48	typedef struct {	48	typedef struct {
49	uint32_t ITOW; // ms GPS Millisecond Time of Week	49	uint32_t ITOW; // ms GPS Millisecond Time of Week
50	int32_t lon; // 1e-07 deg Longitude	50	int32_t lon; // 1e-07 deg Longitude
51	int32_t lat; // 1e-07 deg Latitude	51	int32_t lat; // 1e-07 deg Latitude
52	int32_t height; // mm Height above Ellipsoid	52	int32_t height; // mm Height above Ellipsoid
53	int32_t HMSL; // mm Height above mean sea level	53	int32_t HMSL; // mm Height above mean sea level
54	uint32_t hAcc; // mm Horizontal Accuracy Estimate	54	uint32_t hAcc; // mm Horizontal Accuracy Estimate
55	uint32_t vAacc; // mm Vertical Accuracy Estimate	55	uint32_t vAacc; // mm Vertical Accuracy Estimate
56	Status_t status;	56	Status_t status;
57	} UBX_POSLLH_t;	57	} UBX_POSLLH_t;
58		58
59	typedef struct {	59	typedef struct {
60	uint32_t ITOW; // ms GPS Millisecond Time of Week	60	uint32_t ITOW; // ms GPS Millisecond Time of Week
61	int32_t VEL_N; // cm/s NED north velocity	61	int32_t VEL_N; // cm/s NED north velocity
62	int32_t VEL_E; // cm/s NED east velocity	62	int32_t VEL_E; // cm/s NED east velocity
63	int32_t VEL_D; // cm/s NED down velocity	63	int32_t VEL_D; // cm/s NED down velocity
64	uint32_t speed; // cm/s Speed (3-D)	64	uint32_t speed; // cm/s Speed (3-D)
65	uint32_t gSpeed; // cm/s Ground Speed (2-D)	65	uint32_t gSpeed; // cm/s Ground Speed (2-D)
66	int32_t heading; // 1e-05 deg Heading 2-D	66	int32_t heading; // 1e-05 deg Heading 2-D
67	uint32_t sAcc; // cm/s Speed Accuracy Estimate	67	uint32_t sAcc; // cm/s Speed Accuracy Estimate
68	uint32_t cAcc; // deg Course / Heading Accuracy Estimate	68	uint32_t cAcc; // deg Course / Heading Accuracy Estimate
69	Status_t status;	69	Status_t status;
70	} UBX_VELNED_t;	70	} UBX_VELNED_t;
71		71
72	UBX_SOL_t ubxSol = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };	72	UBX_SOL_t ubxSol = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
73	UBX_POSLLH_t ubxPosLlh = { 0, 0, 0, 0, 0, 0, 0, INVALID };	73	UBX_POSLLH_t ubxPosLlh = { 0, 0, 0, 0, 0, 0, 0, INVALID };
74	UBX_VELNED_t ubxVelNed = { 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };	74	UBX_VELNED_t ubxVelNed = { 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
75	GPS_INFO_t GPSInfo = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };	75	GPS_INFO_t GPSInfo = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
76		76
77	volatile uint8_t GPSTimeout = 0;	77	volatile uint8_t GPSTimeout = 0;
78	volatile uint16_t GPSDatasetCounter = 0;	78	volatile uint16_t GPSDatasetCounter = 0;
79		79
80	void updateGPSInfo(void) {	80	void updateGPSInfo(void) {
81	if ((ubxSol.status == NEWDATA) && (ubxPosLlh.status == NEWDATA) && (ubxVelNed.status == NEWDATA)) {	81	if ((ubxSol.status == NEWDATA) && (ubxPosLlh.status == NEWDATA) && (ubxVelNed.status == NEWDATA)) {
82	//RED_FLASH;	82	//RED_FLASH;
83	// DebugOut.Digital ....blah...	83	// DebugOut.Digital ....blah...
84	if (GPSInfo.status != NEWDATA) {	84	if (GPSInfo.status != NEWDATA) {
85	GPSDatasetCounter++;	85	GPSDatasetCounter++;
86	GPSInfo.status = INVALID;	86	GPSInfo.status = INVALID;
87	// NAV SOL	87	// NAV SOL
88	GPSInfo.flags = ubxSol.flags;	88	GPSInfo.flags = ubxSol.flags;
89	GPSInfo.satfix = ubxSol.GPSfix;	89	GPSInfo.satfix = ubxSol.GPSfix;
90	GPSInfo.satnum = ubxSol.numSV;	90	GPSInfo.satnum = ubxSol.numSV;
91	GPSInfo.PAcc = ubxSol.pAcc;	91	GPSInfo.PAcc = ubxSol.pAcc;
92	GPSInfo.VAcc = ubxSol.sAcc;	92	GPSInfo.VAcc = ubxSol.sAcc;
-		93	debugOut.analog[31] = GPSInfo.VAcc/1000L;
-		94
93	// NAV POSLLH	95	// NAV POSLLH
94	GPSInfo.longitude = ubxPosLlh.lon;	96	GPSInfo.longitude = ubxPosLlh.lon;
95	GPSInfo.latitude = ubxPosLlh.lat;	97	GPSInfo.latitude = ubxPosLlh.lat;
96	GPSInfo.altitude = ubxPosLlh.height;	98	GPSInfo.altitude = ubxPosLlh.height;
97		99
98	GPSInfo.veleast = ubxVelNed.VEL_E;	100	GPSInfo.veleast = ubxVelNed.VEL_E;
99	GPSInfo.velnorth = ubxVelNed.VEL_N;	101	GPSInfo.velnorth = ubxVelNed.VEL_N;
100	GPSInfo.veltop = -ubxVelNed.VEL_D;	102	GPSInfo.veltop = -ubxVelNed.VEL_D;
101	GPSInfo.velground = ubxVelNed.gSpeed;	103	GPSInfo.velground = ubxVelNed.gSpeed;
102		104
103	GPSInfo.status = NEWDATA;	105	GPSInfo.status = NEWDATA;
104	}	106	}
105		107
106	// set state to collect new data	108	// set state to collect new data
107	ubxSol.status = PROCESSED; // never update old data	109	ubxSol.status = PROCESSED; // never update old data
108	ubxPosLlh.status = PROCESSED; // never update old data	110	ubxPosLlh.status = PROCESSED; // never update old data
109	ubxVelNed.status = PROCESSED; // never update old data	111	ubxVelNed.status = PROCESSED; // never update old data
110	}	112	}
111	}	113	}
112		114
113	// this function should be called within the UART RX ISR	115	// this function should be called within the UART RX ISR
114	void ubx_parser(uint8_t c) {	116	void ubx_parser(uint8_t c) {
115	static uint8_t ubxstate = UBXSTATE_IDLE;	117	static uint8_t ubxstate = UBXSTATE_IDLE;
116	static uint8_t cka, ckb;	118	static uint8_t cka, ckb;
117	static uint16_t msglen;	119	static uint16_t msglen;
118	static int8_t ubxP, ubxEp, *ubxSp; // pointers to data currently transfered	120	static int8_t ubxP, ubxEp, *ubxSp; // pointers to data currently transfered
119		121
120	switch (ubxstate) {	122	switch (ubxstate) {
121	case UBXSTATE_IDLE: // check 1st sync byte	123	case UBXSTATE_IDLE: // check 1st sync byte
122	if (c == UBX_SYNC1_CHAR)	124	if (c == UBX_SYNC1_CHAR)
123	ubxstate = UBXSTATE_SYNC1;	125	ubxstate = UBXSTATE_SYNC1;
124	else	126	else
125	ubxstate = UBXSTATE_IDLE; // out of synchronization	127	ubxstate = UBXSTATE_IDLE; // out of synchronization
126	break;	128	break;
127		129
128	case UBXSTATE_SYNC1: // check 2nd sync byte	130	case UBXSTATE_SYNC1: // check 2nd sync byte
129	if (c == UBX_SYNC2_CHAR)	131	if (c == UBX_SYNC2_CHAR)
130	ubxstate = UBXSTATE_SYNC2;	132	ubxstate = UBXSTATE_SYNC2;
131	else	133	else
132	ubxstate = UBXSTATE_IDLE; // out of synchronization	134	ubxstate = UBXSTATE_IDLE; // out of synchronization
133	break;	135	break;
134		136
135	case UBXSTATE_SYNC2: // check msg class to be NAV	137	case UBXSTATE_SYNC2: // check msg class to be NAV
136	if (c == UBX_CLASS_NAV)	138	if (c == UBX_CLASS_NAV)
137	ubxstate = UBXSTATE_CLASS;	139	ubxstate = UBXSTATE_CLASS;
138	else	140	else
139	ubxstate = UBXSTATE_IDLE; // unsupported message class	141	ubxstate = UBXSTATE_IDLE; // unsupported message class
140	break;	142	break;
141		143
142	case UBXSTATE_CLASS: // check message identifier	144	case UBXSTATE_CLASS: // check message identifier
143	switch (c) {	145	switch (c) {
144	case UBX_ID_POSLLH: // geodetic position	146	case UBX_ID_POSLLH: // geodetic position
145	ubxP = (int8_t *) &ubxPosLlh; // data start pointer	147	ubxP = (int8_t *) &ubxPosLlh; // data start pointer
146	ubxEp = (int8_t *) (&ubxPosLlh + 1); // data end pointer	148	ubxEp = (int8_t *) (&ubxPosLlh + 1); // data end pointer
147	ubxSp = (int8_t *) &ubxPosLlh.status; // status pointer	149	ubxSp = (int8_t *) &ubxPosLlh.status; // status pointer
148	break;	150	break;
149		151
150	case UBX_ID_SOL: // navigation solution	152	case UBX_ID_SOL: // navigation solution
151	ubxP = (int8_t *) &ubxSol; // data start pointer	153	ubxP = (int8_t *) &ubxSol; // data start pointer
152	ubxEp = (int8_t *) (&ubxSol + 1); // data end pointer	154	ubxEp = (int8_t *) (&ubxSol + 1); // data end pointer
153	ubxSp = (int8_t *) &ubxSol.status; // status pointer	155	ubxSp = (int8_t *) &ubxSol.status; // status pointer
154	break;	156	break;
155		157
156	case UBX_ID_VELNED: // velocity vector in tangent plane	158	case UBX_ID_VELNED: // velocity vector in tangent plane
157	ubxP = (int8_t *) &ubxVelNed; // data start pointer	159	ubxP = (int8_t *) &ubxVelNed; // data start pointer
158	ubxEp = (int8_t *) (&ubxVelNed + 1); // data end pointer	160	ubxEp = (int8_t *) (&ubxVelNed + 1); // data end pointer
159	ubxSp = (int8_t *) &ubxVelNed.status; // status pointer	161	ubxSp = (int8_t *) &ubxVelNed.status; // status pointer
160	break;	162	break;
161		163
162	default: // unsupported identifier	164	default: // unsupported identifier
163	ubxstate = UBXSTATE_IDLE;	165	ubxstate = UBXSTATE_IDLE;
164	break;	166	break;
165	}	167	}
166	if (ubxstate != UBXSTATE_IDLE) {	168	if (ubxstate != UBXSTATE_IDLE) {
167	ubxstate = UBXSTATE_LEN1;	169	ubxstate = UBXSTATE_LEN1;
168	cka = UBX_CLASS_NAV + c;	170	cka = UBX_CLASS_NAV + c;
169	ckb = UBX_CLASS_NAV + cka;	171	ckb = UBX_CLASS_NAV + cka;
170	}	172	}
171	break;	173	break;
172		174
173	case UBXSTATE_LEN1: // 1st message length byte	175	case UBXSTATE_LEN1: // 1st message length byte
174	msglen = c;	176	msglen = c;
175	cka += c;	177	cka += c;
176	ckb += cka;	178	ckb += cka;
177	ubxstate = UBXSTATE_LEN2;	179	ubxstate = UBXSTATE_LEN2;
178	break;	180	break;
179		181
180	case UBXSTATE_LEN2: // 2nd message length byte	182	case UBXSTATE_LEN2: // 2nd message length byte
181	msglen += ((uint16_t) c) << 8;	183	msglen += ((uint16_t) c) << 8;
182	cka += c;	184	cka += c;
183	ckb += cka;	185	ckb += cka;
184	// if the old data are not processed so far then break parsing now	186	// if the old data are not processed so far then break parsing now
185	// to avoid writing new data in ISR during reading by another function	187	// to avoid writing new data in ISR during reading by another function
186	if (*ubxSp == NEWDATA) {	188	if (*ubxSp == NEWDATA) {
187	updateGPSInfo(); //update GPS info respectively	189	updateGPSInfo(); //update GPS info respectively
188	ubxstate = UBXSTATE_IDLE;	190	ubxstate = UBXSTATE_IDLE;
189	} else {// data invalid or already processd	191	} else {// data invalid or already processd
190	*ubxSp = INVALID;	192	*ubxSp = INVALID;
191	ubxstate = UBXSTATE_DATA;	193	ubxstate = UBXSTATE_DATA;
192	}	194	}
193	break;	195	break;
194		196
195	case UBXSTATE_DATA:	197	case UBXSTATE_DATA:
196	if (ubxP < ubxEp)	198	if (ubxP < ubxEp)
197	*ubxP++ = c; // copy curent data byte if any space is left	199	*ubxP++ = c; // copy curent data byte if any space is left
198	cka += c;	200	cka += c;
199	ckb += cka;	201	ckb += cka;
200	if (--msglen == 0)	202	if (--msglen == 0)
201	ubxstate = UBXSTATE_CKA; // switch to next state if all data was read	203	ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
202	break;	204	break;
203		205
204	case UBXSTATE_CKA:	206	case UBXSTATE_CKA:
205	if (c == cka)	207	if (c == cka)
206	ubxstate = UBXSTATE_CKB;	208	ubxstate = UBXSTATE_CKB;
207	else {	209	else {
208	*ubxSp = INVALID;	210	*ubxSp = INVALID;
209	ubxstate = UBXSTATE_IDLE;	211	ubxstate = UBXSTATE_IDLE;
210	}	212	}
211	break;	213	break;
212		214
213	case UBXSTATE_CKB:	215	case UBXSTATE_CKB:
214	if (c == ckb) {	216	if (c == ckb) {
215	*ubxSp = NEWDATA; // new data are valid	217	*ubxSp = NEWDATA; // new data are valid
216	updateGPSInfo(); //update GPS info respectively	218	updateGPSInfo(); //update GPS info respectively
217	GPSTimeout = 255;	219	GPSTimeout = 255;
218	} else { // if checksum not match then set data invalid	220	} else { // if checksum not match then set data invalid
219	*ubxSp = INVALID;	221	*ubxSp = INVALID;
220	}	222	}
221	ubxstate = UBXSTATE_IDLE; // ready to parse new data	223	ubxstate = UBXSTATE_IDLE; // ready to parse new data
222	break;	224	break;
223		225
224	default: // unknown ubx state	226	default: // unknown ubx state
225	ubxstate = UBXSTATE_IDLE;	227	ubxstate = UBXSTATE_IDLE;
226	break;	228	break;
227	}	229	}
228	}	230	}
229		231

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/ubx.c @ 2690 - Rev 2059 → 2079