WebSVN - FlightCtrl - Diff - Rev 2084 and 2088 - /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, 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.position3DAcc =  ubxSol.pAcc;
      GPSInfo.verticalAcc =    ubxPosLlh.vAacc;
      GPSInfo.velocityAcc =    ubxSol.sAcc;
 
      debugOut.analog[30] = ubxPosLlh.HMSL/1000L;
      debugOut.analog[31] = GPSInfo.verticalAcc/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 2084	Rev 2088
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, 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
91	GPSInfo.PAcc = ubxSol.pAcc;	92	GPSInfo.position3DAcc = ubxSol.pAcc;
-		93	GPSInfo.verticalAcc = ubxPosLlh.vAacc;
92	GPSInfo.VAcc = ubxSol.sAcc;	94	GPSInfo.velocityAcc = ubxSol.sAcc;
-		95
-		96	debugOut.analog[30] = ubxPosLlh.HMSL/1000L;
93	debugOut.analog[31] = GPSInfo.VAcc;	97	debugOut.analog[31] = GPSInfo.verticalAcc/1000L;
94		98
95	// NAV POSLLH	99	// NAV POSLLH
96	GPSInfo.longitude = ubxPosLlh.lon;	100	GPSInfo.longitude = ubxPosLlh.lon;
97	GPSInfo.latitude = ubxPosLlh.lat;	101	GPSInfo.latitude = ubxPosLlh.lat;
98	GPSInfo.altitude = ubxPosLlh.height;	102	GPSInfo.altitude = ubxPosLlh.height;
99		103
100	GPSInfo.veleast = ubxVelNed.VEL_E;	104	GPSInfo.veleast = ubxVelNed.VEL_E;
101	GPSInfo.velnorth = ubxVelNed.VEL_N;	105	GPSInfo.velnorth = ubxVelNed.VEL_N;
102	GPSInfo.veltop = -ubxVelNed.VEL_D;	106	GPSInfo.veltop = -ubxVelNed.VEL_D;
103	GPSInfo.velground = ubxVelNed.gSpeed;	107	GPSInfo.velground = ubxVelNed.gSpeed;
104		108
105	GPSInfo.status = NEWDATA;	109	GPSInfo.status = NEWDATA;
106	}	110	}
107		111
108	// set state to collect new data	112	// set state to collect new data
109	ubxSol.status = PROCESSED; // never update old data	113	ubxSol.status = PROCESSED; // never update old data
110	ubxPosLlh.status = PROCESSED; // never update old data	114	ubxPosLlh.status = PROCESSED; // never update old data
111	ubxVelNed.status = PROCESSED; // never update old data	115	ubxVelNed.status = PROCESSED; // never update old data
112	}	116	}
113	}	117	}
114		118
115	// this function should be called within the UART RX ISR	119	// this function should be called within the UART RX ISR
116	void ubx_parser(uint8_t c) {	120	void ubx_parser(uint8_t c) {
117	static uint8_t ubxstate = UBXSTATE_IDLE;	121	static uint8_t ubxstate = UBXSTATE_IDLE;
118	static uint8_t cka, ckb;	122	static uint8_t cka, ckb;
119	static uint16_t msglen;	123	static uint16_t msglen;
120	static int8_t ubxP, ubxEp, *ubxSp; // pointers to data currently transfered	124	static int8_t ubxP, ubxEp, *ubxSp; // pointers to data currently transfered
121		125
122	switch (ubxstate) {	126	switch (ubxstate) {
123	case UBXSTATE_IDLE: // check 1st sync byte	127	case UBXSTATE_IDLE: // check 1st sync byte
124	if (c == UBX_SYNC1_CHAR)	128	if (c == UBX_SYNC1_CHAR)
125	ubxstate = UBXSTATE_SYNC1;	129	ubxstate = UBXSTATE_SYNC1;
126	else	130	else
127	ubxstate = UBXSTATE_IDLE; // out of synchronization	131	ubxstate = UBXSTATE_IDLE; // out of synchronization
128	break;	132	break;
129		133
130	case UBXSTATE_SYNC1: // check 2nd sync byte	134	case UBXSTATE_SYNC1: // check 2nd sync byte
131	if (c == UBX_SYNC2_CHAR)	135	if (c == UBX_SYNC2_CHAR)
132	ubxstate = UBXSTATE_SYNC2;	136	ubxstate = UBXSTATE_SYNC2;
133	else	137	else
134	ubxstate = UBXSTATE_IDLE; // out of synchronization	138	ubxstate = UBXSTATE_IDLE; // out of synchronization
135	break;	139	break;
136		140
137	case UBXSTATE_SYNC2: // check msg class to be NAV	141	case UBXSTATE_SYNC2: // check msg class to be NAV
138	if (c == UBX_CLASS_NAV)	142	if (c == UBX_CLASS_NAV)
139	ubxstate = UBXSTATE_CLASS;	143	ubxstate = UBXSTATE_CLASS;
140	else	144	else
141	ubxstate = UBXSTATE_IDLE; // unsupported message class	145	ubxstate = UBXSTATE_IDLE; // unsupported message class
142	break;	146	break;
143		147
144	case UBXSTATE_CLASS: // check message identifier	148	case UBXSTATE_CLASS: // check message identifier
145	switch (c) {	149	switch (c) {
146	case UBX_ID_POSLLH: // geodetic position	150	case UBX_ID_POSLLH: // geodetic position
147	ubxP = (int8_t *) &ubxPosLlh; // data start pointer	151	ubxP = (int8_t *) &ubxPosLlh; // data start pointer
148	ubxEp = (int8_t *) (&ubxPosLlh + 1); // data end pointer	152	ubxEp = (int8_t *) (&ubxPosLlh + 1); // data end pointer
149	ubxSp = (int8_t *) &ubxPosLlh.status; // status pointer	153	ubxSp = (int8_t *) &ubxPosLlh.status; // status pointer
150	break;	154	break;
151		155
152	case UBX_ID_SOL: // navigation solution	156	case UBX_ID_SOL: // navigation solution
153	ubxP = (int8_t *) &ubxSol; // data start pointer	157	ubxP = (int8_t *) &ubxSol; // data start pointer
154	ubxEp = (int8_t *) (&ubxSol + 1); // data end pointer	158	ubxEp = (int8_t *) (&ubxSol + 1); // data end pointer
155	ubxSp = (int8_t *) &ubxSol.status; // status pointer	159	ubxSp = (int8_t *) &ubxSol.status; // status pointer
156	break;	160	break;
157		161
158	case UBX_ID_VELNED: // velocity vector in tangent plane	162	case UBX_ID_VELNED: // velocity vector in tangent plane
159	ubxP = (int8_t *) &ubxVelNed; // data start pointer	163	ubxP = (int8_t *) &ubxVelNed; // data start pointer
160	ubxEp = (int8_t *) (&ubxVelNed + 1); // data end pointer	164	ubxEp = (int8_t *) (&ubxVelNed + 1); // data end pointer
161	ubxSp = (int8_t *) &ubxVelNed.status; // status pointer	165	ubxSp = (int8_t *) &ubxVelNed.status; // status pointer
162	break;	166	break;
163		167
164	default: // unsupported identifier	168	default: // unsupported identifier
165	ubxstate = UBXSTATE_IDLE;	169	ubxstate = UBXSTATE_IDLE;
166	break;	170	break;
167	}	171	}
168	if (ubxstate != UBXSTATE_IDLE) {	172	if (ubxstate != UBXSTATE_IDLE) {
169	ubxstate = UBXSTATE_LEN1;	173	ubxstate = UBXSTATE_LEN1;
170	cka = UBX_CLASS_NAV + c;	174	cka = UBX_CLASS_NAV + c;
171	ckb = UBX_CLASS_NAV + cka;	175	ckb = UBX_CLASS_NAV + cka;
172	}	176	}
173	break;	177	break;
174		178
175	case UBXSTATE_LEN1: // 1st message length byte	179	case UBXSTATE_LEN1: // 1st message length byte
176	msglen = c;	180	msglen = c;
177	cka += c;	181	cka += c;
178	ckb += cka;	182	ckb += cka;
179	ubxstate = UBXSTATE_LEN2;	183	ubxstate = UBXSTATE_LEN2;
180	break;	184	break;
181		185
182	case UBXSTATE_LEN2: // 2nd message length byte	186	case UBXSTATE_LEN2: // 2nd message length byte
183	msglen += ((uint16_t) c) << 8;	187	msglen += ((uint16_t) c) << 8;
184	cka += c;	188	cka += c;
185	ckb += cka;	189	ckb += cka;
186	// if the old data are not processed so far then break parsing now	190	// if the old data are not processed so far then break parsing now
187	// to avoid writing new data in ISR during reading by another function	191	// to avoid writing new data in ISR during reading by another function
188	if (*ubxSp == NEWDATA) {	192	if (*ubxSp == NEWDATA) {
189	updateGPSInfo(); //update GPS info respectively	193	updateGPSInfo(); //update GPS info respectively
190	ubxstate = UBXSTATE_IDLE;	194	ubxstate = UBXSTATE_IDLE;
191	} else {// data invalid or already processd	195	} else {// data invalid or already processd
192	*ubxSp = INVALID;	196	*ubxSp = INVALID;
193	ubxstate = UBXSTATE_DATA;	197	ubxstate = UBXSTATE_DATA;
194	}	198	}
195	break;	199	break;
196		200
197	case UBXSTATE_DATA:	201	case UBXSTATE_DATA:
198	if (ubxP < ubxEp)	202	if (ubxP < ubxEp)
199	*ubxP++ = c; // copy curent data byte if any space is left	203	*ubxP++ = c; // copy curent data byte if any space is left
200	cka += c;	204	cka += c;
201	ckb += cka;	205	ckb += cka;
202	if (--msglen == 0)	206	if (--msglen == 0)
203	ubxstate = UBXSTATE_CKA; // switch to next state if all data was read	207	ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
204	break;	208	break;
205		209
206	case UBXSTATE_CKA:	210	case UBXSTATE_CKA:
207	if (c == cka)	211	if (c == cka)
208	ubxstate = UBXSTATE_CKB;	212	ubxstate = UBXSTATE_CKB;
209	else {	213	else {
210	*ubxSp = INVALID;	214	*ubxSp = INVALID;
211	ubxstate = UBXSTATE_IDLE;	215	ubxstate = UBXSTATE_IDLE;
212	}	216	}
213	break;	217	break;
214		218
215	case UBXSTATE_CKB:	219	case UBXSTATE_CKB:
216	if (c == ckb) {	220	if (c == ckb) {
217	*ubxSp = NEWDATA; // new data are valid	221	*ubxSp = NEWDATA; // new data are valid
218	updateGPSInfo(); //update GPS info respectively	222	updateGPSInfo(); //update GPS info respectively
219	GPSTimeout = 255;	223	GPSTimeout = 255;
220	} else { // if checksum not match then set data invalid	224	} else { // if checksum not match then set data invalid
221	*ubxSp = INVALID;	225	*ubxSp = INVALID;
222	}	226	}
223	ubxstate = UBXSTATE_IDLE; // ready to parse new data	227	ubxstate = UBXSTATE_IDLE; // ready to parse new data
224	break;	228	break;
225		229
226	default: // unknown ubx state	230	default: // unknown ubx state
227	ubxstate = UBXSTATE_IDLE;	231	ubxstate = UBXSTATE_IDLE;
228	break;	232	break;
229	}	233	}
230	}	234	}
231		235

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/ubx.c @ 2218 - Rev 2084 → 2088