WebSVN - FlightCtrl - Blame - Rev 2189 - /branches/dongfang_FC

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/ubx.c - Rev 2189