garminprotocol.cpp

garmin 协议处理范例： 处理了 0183协议 处理了 gps的专有格式等

//
// GPSMapEdit
// (c) Konstantin Galichsky ([e-mail]), 2002-2004
//
// Garmin protocol implementation.
//

# include "StdAfx.h"
# include "GarminProtocol.h"
# include "ComPort.h"
# include "Positioning.h"
# include "Globals.h"

#ifdef _MSC_VER
#pragma pack (push, 1)
#endif

typedef struct {
	double lat; /* latitude in radians */
	double lon; /* longitude in radians */
} Radian_Type;

struct PacketHdr {
	BYTE btDLE;      // 0x10
	BYTE btPacketID; // Pid_XXX
	BYTE btDataSize; // Size of btData in bytes
	BYTE btData [1]; // The data
};

struct PacketTrailer {
	BYTE btCheckSum; // 2's complement of the sum of all bytes in btPacketID, btDataSize and btData
	BYTE btDLE;      // 0x10
	BYTE btETX;      // 0x03
};

	//
	// Basic packet IDs
	//

enum {
	Pid_Ack_Byte       = 6,
	Pid_Nak_Byte       = 21,
	Pid_Protocol_Array = 253, /* may not be implemented in all products */
	Pid_Product_Rqst   = 254,
	Pid_Product_Data   = 255
};

	//
	// L001 - Link Protocol 1 (majority of GPS products)
	//

enum {
	Pid_Command_Data   = 10,
	Pid_Xfer_Cmplt     = 12,
	Pid_Date_Time_Data = 14,
	Pid_Position_Data  = 17,
	Pid_Prx_Wpt_Data   = 19,
	Pid_Records        = 27,
	Pid_Rte_Hdr        = 29,
	Pid_Rte_Wpt_Data   = 30,
	Pid_Almanac_Data   = 31,
	Pid_Trk_Data       = 34,
	Pid_Wpt_Data       = 35,
//	Pid_UnitID_Data    = 38, /* UNDOCUMENTED: 4-byte reply to Cmnd_Get_UnitID */
	Pid_Pvt_Data       = 51,
	Pid_Rte_Link_Data  = 98,
	Pid_Trk_Hdr        = 99
};

	//
	// A001 - Product Data Protocol
	//

typedef struct {
	WORD product_ID;
	WORD software_version;
	/* char product_description[]; null-terminated string */
	/* ... zero or more additional null-terminated strings */
} Product_Data_Type;

	//
	// A001 - Protocol Capability Protocol
	//

struct Protocol_Data_Type {
	BYTE tag;
	WORD data;
};

enum {
	Tag_Phys_Prot_Id = 'P', /* tag for Physical protocol ID */
	Tag_Link_Prot_Id = 'L', /* tag for Link protocol ID */
	Tag_Appl_Prot_Id = 'A', /* tag for Application protocol ID */
	Tag_Data_Type_Id = 'D'  /* tag for Data Type ID */
};

	//
	// A010 - Device Command Protocol 1 (the majority of GPS products)
	//

typedef WORD Command_Id_Type;

enum {
	Cmnd_Abort_Transfer = 0,  /* abort current transfer */
	Cmnd_Transfer_Alm   = 1,  /* transfer almanac */
	Cmnd_Transfer_Posn  = 2,  /* transfer position */
	Cmnd_Transfer_Prx   = 3,  /* transfer proximity waypoints */
	Cmnd_Transfer_Rte   = 4,  /* transfer routes */
	Cmnd_Transfer_Time  = 5,  /* transfer time */
	Cmnd_Transfer_Trk   = 6,  /* transfer track log */
	Cmnd_Transfer_Wpt   = 7,  /* transfer waypoints */
	Cmnd_Turn_Off_Pwr   = 8,  /* turn off power */
//	Cmnd_Get_UnitID     = 14, /* UNDOCUMENTED: ask UnitID */
	Cmnd_Start_Pvt_Data = 49, /* start transmitting PVT data */
	Cmnd_Stop_Pvt_Data  = 50  /* stop transmitting PVT data */
};

	// A100 - Waypoint Transfer Protocol
	// A200 - Route Transfer Protocol
	// A300 - Track Log Transfer Protocol
	// A400 - Proximity Waypoint Transfer Protocol
	// A500 - Almanac Transfer Protocol
	// A600 - Date and Time Initialization Protocol
	// A700 - Position Initialization Protocol

	//
	// A800 - PVT Data Protocol
	//

struct D800_Pvt_Data_Type {
	float alt;        /* altitude above WGS 84 ellipsoid (meters) */
	float epe;        /* estimated position error, 2 sigma (meters) */
	float eph;        /* epe, but horizontal only (meters) */
	float epv;        /* epe, but vertical only (meters) */
	WORD  fix;        /* type of position fix */
	double tow;       /* time of week (seconds) */
	Radian_Type posn; /* latitude and longitude (radians) */
	float east;       /* velocity east (meters/second) */
	float north;      /* velocity north (meters/second) */
	float up;         /* velocity up (meters/second) */
	float msl_hght;   /* height of WGS 84 ellipsoid above MSL (meters) */
	WORD  leap_scnds; /* difference between GPS and UTC (seconds) */
	DWORD wn_days;    /* week number days */
};

enum {
	fix_unusable = 0, /* failed integrity check */
	fix_invalid  = 1, /* invalid or unavailable */
	fix_2D       = 2, /* two dimensional */
	fix_3D       = 3, /* three dimensional */
	fix_2D_diff  = 4, /* two dimensional differential */
	fix_3D_diff  = 5  /* three dimensional differential */
};

#ifdef _MSC_VER
#pragma pack (pop)
#endif

static BYTE g_pCmdStartPvtData [] = {0x10, Pid_Command_Data, 2, Cmnd_Start_Pvt_Data, 0, -(Pid_Command_Data + 2 + Cmnd_Start_Pvt_Data), 0x10, 0x03};
static BYTE g_pCmdStopPvtData  [] = {0x10, Pid_Command_Data, 2, Cmnd_Stop_Pvt_Data,  0, -(Pid_Command_Data + 2 + Cmnd_Stop_Pvt_Data),  0x10, 0x03};
static BYTE g_pAckPvtData      [] = {0x10, Pid_Ack_Byte,     2, Pid_Pvt_Data,        0, -(Pid_Ack_Byte     + 2 + Pid_Pvt_Data),        0x10, 0x03};
static BYTE g_pNakPvtData      [] = {0x10, Pid_Nak_Byte,     2, Pid_Pvt_Data,        0, -(Pid_Nak_Byte     + 2 + Pid_Pvt_Data),        0x10, 0x03};

static
BYTE GetGarminCheckSum (const BYTE * _pData, size_t _cSize) {
	BYTE btRet = 0;
	for (size_t c = 0; c < _cSize; ++ c) {
		const BYTE bt = _pData [c];
		if (c > 0 && bt == 0x10 && _pData [c - 1] == 0x10)
			continue;
		btRet -= bt;
	}
	return btRet;
}

static
void Send_CmdStartPvt (CComPort * _pPort) {
	DWORD dwWritten = 0;
	::WriteFile (* _pPort, g_pCmdStartPvtData, sizeof (g_pCmdStartPvtData), & dwWritten, NULL);
}

static
void Send_CmdStopPvt (CComPort * _pPort) {
	DWORD dwWritten = 0;
	::WriteFile (* _pPort, g_pCmdStopPvtData,  sizeof (g_pCmdStopPvtData),  & dwWritten, NULL);
}

static
void Send_AckPvt (CComPort * _pPort) {
	DWORD dwWritten = 0;
	::WriteFile (* _pPort, g_pAckPvtData,      sizeof (g_pAckPvtData),      & dwWritten, NULL);
}

static
void Send_NakPvt (CComPort * _pPort) {
	DWORD dwWritten = 0;
	::WriteFile (* _pPort, g_pNakPvtData,      sizeof (g_pNakPvtData),      & dwWritten, NULL);
}

static
void OnPVTData (const D800_Pvt_Data_Type * _pData);

/////////////////////////////////////////////////

void CGarminProtocol::GetName (string_t & _strName) const {
	_strName.append ("Garmin protocol");
}

void CGarminProtocol::Start () {
	// Setup COM port.
	assert (m_pPort);
	m_pPort->SetDCB (CBR_9600, 8, false, ONESTOPBIT);

	COMMTIMEOUTS ct = {100, 10, 10, 10, 10};
	::SetCommTimeouts (* m_pPort, & ct);

	m_pPort->ClearQueues ();

	// Initialize data receiving.
	m_cPacketLen = 0;
	m_btPrev = 0;
	m_dwLastDataTime = ::GetTickCount ();

	// Request PVT data.
	RequestPVT ();
}

void CGarminProtocol::Stop () {
	assert (m_pPort);

	Send_CmdStopPvt (m_pPort);
}

void CGarminProtocol::RequestPVT () {
	m_bPVTRequested = true;
	m_bPVTAcknowledged = false;
	m_dwPVTRequestTime = ::GetTickCount ();
	Send_CmdStartPvt (m_pPort);
}

CGpsProtocol::Status_t CGarminProtocol::OnDataReceived (const BYTE * _pData, size_t _cSize) {
	if (_cSize == 0) {
		// Detect no ACK for PVT request.
		if (m_bPVTRequested && ! m_bPVTAcknowledged && ::GetTickCount () - m_dwPVTRequestTime > 3000)
			return stSureError;
		// Detect NAK for PVT request.
		if (! m_bPVTRequested && ! m_bPVTAcknowledged)
			return stSureError;

		// Detect long silence.
		if (m_bPVTAcknowledged && ::GetTickCount () - m_dwLastDataTime >= 700) {
			// NOTE: some GPS receivers provide PVT in too slow rate.
			// Force GPS reciever to supply PVT each second.
			// NOTE: this also allows to detect disconnection.
			m_cPacketLen = 0;
			m_btPrev = 0;
			m_dwLastDataTime = ::GetTickCount ();
			RequestPVT ();
		}
		return stNotSure;
	}

	m_dwLastDataTime = ::GetTickCount ();

	CGpsProtocol::Status_t ret = stNotSure;
	for (size_t c = 0; c < _cSize; ++ c) {
		const BYTE bt = _pData [c];
		if (m_btPrev == 0x10) {
			if (bt == 0x10)
				OnByteReceived (0x10);
			else if (bt == 0x03) {
				const bool bOK = OnPacketEnd ();
				if (! bOK)
					return stSureError;
				else
					ret = stSureOK;
			} else {
				// Packet begining.
				m_cPacketLen = 0;
				OnByteReceived (bt);
			}
		} else
			OnByteReceived (bt);
		m_btPrev = bt;
	}
	return ret;
}

void CGarminProtocol::OnByteReceived (BYTE _bt) {
	if (m_cPacketLen < sizeof (m_packet))
		m_packet [m_cPacketLen ++] = _bt;
}

bool CGarminProtocol::OnPacketEnd () {
	if (m_cPacketLen < 3)
		return false;

	//
	// Check the data integrity.
	//
	const BYTE btCheckSum = GetGarminCheckSum (m_packet, m_cPacketLen - 2);
	const BYTE btCheckSum2 = m_packet [m_cPacketLen - 2];
	if (btCheckSum != btCheckSum2)
		// NOTE: Under some conditions, the check sum appears invalid too often.
		// So, just ignore such packets to avoid protocol failure.
		return true;

	const BYTE btDataSize = m_packet [1];
	if (m_cPacketLen != size_t (btDataSize + 4))
		return false;

	//
	// Parse the data.
	//
	const BYTE btPacketID = m_packet [0];
	const BYTE * pData = m_packet + 2;
	switch (btPacketID) {
		case Pid_Ack_Byte: {
			const BYTE btPacketIDAcknowleged = pData [0];
			OnACK (btPacketIDAcknowleged);
			break;
		}
		case Pid_Nak_Byte: {
			const BYTE btPacketIDRejected = pData [0];
			OnNAK (btPacketIDRejected);
			break;
		}

		case Pid_Pvt_Data:
			if (btDataSize >= sizeof (D800_Pvt_Data_Type)) {
				OnPVTData (reinterpret_cast<const D800_Pvt_Data_Type *> (pData));
				Send_AckPvt (m_pPort);
			} else
				Send_NakPvt (m_pPort);
			break;
	}

	return true;
}

void CGarminProtocol::OnACK (BYTE _btPacketIDAcknowleged) {
	switch (_btPacketIDAcknowleged) {
		case Pid_Command_Data:
			if (m_bPVTRequested) {
				m_bPVTAcknowledged = true;
				m_bPVTRequested = false;
			}
			break;
	}
}

void CGarminProtocol::OnNAK (BYTE _btPacketIDRejected) {
	switch (_btPacketIDRejected) {
		case Pid_Command_Data:
			if (m_bPVTRequested) {
				m_bPVTAcknowledged = false;
				m_bPVTRequested = false;
			}
			break;
	}
}
/////////////////////////////////////////////////

static
void OnPVTData (const D800_Pvt_Data_Type * _pData) {
	if (_pData->fix == fix_unusable || _pData->fix == fix_invalid) {
		PushNoPositionInfo ();
		return;
	}

	PositionInfo_t pi;

	pi.SetTime (32508 /*1989 31 Dec*/ + _pData->wn_days + (_pData->tow - _pData->leap_scnds)/(24*60*60));

	pi.SetXY (_pData->posn.lon*180/c_PI, _pData->posn.lat*180/c_PI);
	pi.SetXYPrec (_pData->eph/2);

	pi.SetAltitude (_pData->alt + _pData->msl_hght);
	pi.SetAltitudePrec (_pData->epv/2);
	pi.SetGeoidHeight (- _pData->msl_hght);

	pi.SetAzimuth (::atan2 (_pData->east, _pData->north)*180/c_PI);
	pi.SetSpeed (::sqrt (_pData->east*_pData->east + _pData->north*_pData->north + _pData->up*_pData->up)*3.6f);
	pi.SetVertSpeed (_pData->up);

	switch (_pData->fix) {
		case fix_2D:      pi.SetFix (PositionInfo_t::fix2D);      break;
		case fix_3D:      pi.SetFix (PositionInfo_t::fix3D);      break;
		case fix_2D_diff: pi.SetFix (PositionInfo_t::fix2D_diff); break;
		case fix_3D_diff: pi.SetFix (PositionInfo_t::fix3D_diff); break;
		default:          pi.SetFix (PositionInfo_t::fixUnknown); break;
	}

	PushPositionInfo (pi);
}