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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/ubx.c @ 2158 - Rev 2088 → 2189