WebSVN - FlightCtrl - Blame - /branches/V0.68d Code Redesign killagreg/ubx.c

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

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/ubx.c @ 775 - Rev