llayer.c

非常好用的gps数据读写接口

/***************************************************************************
                          llayer.c  -  link layer
                             -------------------
    begin                : So M鋜 21 17:50:31 CET 2004
    copyright            : (C) 2004 by Dennis Real
    email                : [e-mail]
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/

#include <stdio.h>
#include "phlayer.h"
#include "def.h"

/*	read garmin packet, verify checksum, remove "escaped" DLEs
		output:
		returns null if checksum fails otherwise packetid+size+data	
		*size contains number of bytes of packetid+size+data	*/
char *GetCommand (int iohandler, unsigned int *size)
{
	unsigned int rawsize, i, j;
	char *gpacket;
	static char gcommand[READBUFFER]; 
	unsigned char checksum=0;
		
	gpacket=GetPacket(iohandler, &rawsize);

	if( gpacket==NULL )
	{
		/* no packet received */
		fprintf(stderr, "Warning: No packet received. GPS on?\n");
		return NULL;
	}

	/* forget 0x10 @^ & trailing 0x10 0x03, deescape DLEs */
	if ( (gpacket[0]!=0x10) || (gpacket[rawsize-2]!=0x10) || (gpacket[rawsize-1]!=0x03) )
	{
		/* not a valid packet */
		fprintf(stderr, "Warning: Incomplete packet received.\n");
		fflush(stderr);
		*size=0;
		return NULL;
	}

	j=0;
	
	/* deescape DLEs, verify checksum */
	for ( i=1; i<rawsize-3; i++ )
	{

		if ( (gpacket[i]==0x10) && (gpacket[i+1]==0x10) )
		{
			/* do nothing */
		}
		else
		{	
			gcommand[j]=gpacket[i];
			checksum-=gcommand[j];
			j++;
		}

 		*size=j;		
	}

	/* verify checksum */	
	if ( checksum!=(unsigned char)gpacket[rawsize-3] )
	{
		fprintf(stderr, "Warning: Checksum of received packet is invalid.\n");
		*size=0;
		return NULL;
	}

	/* verify packet length	*/
	if ( (unsigned char)gcommand[1]!=(j-2) )
	{
		fprintf(stderr, "Warning: length of received packet is invalid.\n");
		*size=0;
		return NULL;		
	}

#ifdef DEBUG
	fprintf(stderr, "Received %i byte command: \"", *size);
/*	for ( j=0; j<*size; j++)
	{
		fprintf(stderr, "%c", (unsigned char)gcommand[j]);
	}
	fprintf(stderr, "\"\n");*/
	              
	for ( j=0; j<*size; j++)
	{
		fprintf(stderr, "%02x ", (unsigned char)gcommand[j]);
	}
	fprintf(stderr, "\n\n");

#endif

	return gcommand;

}


/*	Build garmin packet for command and send in to the gps
		adds all DLEs and ETX, inserts checksum, "escapes" DLE if necessary
		input: PacketID, size of data , data	*/ 
void SendCommand(int iohandler, char *command, BYTE size)
{
	BYTE i, j;
	unsigned checksum=0;
	char gcommand[READBUFFER];

	/* build garmin packet */
	gcommand[0]=0x10;

	j=1;
	for ( i=1; i<=size; i++ )
	{
		checksum-=command[i-1];
		if( command[i]=='\x10' )	/* escape DLE in size, data... */
		{
			gcommand[j++]=command[i-1];	
			gcommand[j++]='\x10';
		}
		else
		{
			gcommand[j++]=command[i-1];	
		}
	}

	gcommand[j++]=checksum;
	if ( checksum==0x10 )			/* ... and checksum */
	{
		gcommand[j++]=0x10;
	}
	gcommand[j++]=0x10;
	gcommand[j++]=0x03;

	/* send ready garmin packet to gps */
	SendPacket (iohandler, gcommand, j);
	
}


/*	Send ACK packet for command 
		command is packetid, size, data		*/
void ACKCommand (int iohandler, char *command, BYTE size)
{
	BYTE gsize=0;
	char gcommand[READBUFFER];
	
	gcommand[gsize++]=6;
	gcommand[gsize++]=2;
	gcommand[gsize++]=command[0];
	gcommand[gsize++]=0;

	SendCommand(iohandler, gcommand, gsize);
}


/*	Send NAK packet for command 
		command is packetid, size, data		*/
void NAKCommand (int iohandler, char *command, BYTE size)
{
	BYTE gsize=0;
	char gcommand[READBUFFER];
	
	gcommand[gsize++]=21;
	gcommand[gsize++]=2;
	gcommand[gsize++]=command[0];
	gcommand[gsize++]=0;
	
	SendCommand(iohandler, gcommand, gsize);
}