pvt.c

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

/***************************************************************************
               pvt.c  -  Position, Velocity, Time related functions
                             -------------------
    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 <gtk/gtk.h>
#include <math.h>

#include "support.h"
#include "def.h"
#include "llayer.h"
#include "convert.h"
#include "datatypes.h"

/*	get latitude and lonitude in radians	*/
Radian_Type getPosition (int tty)
{
	Radian_Type position;
	
	unsigned int s;
	char *gcommand;

	position.lat=0;
	position.lon=0;

  /* abort current transfer */
  SendCommand(tty, "\x0a\x02\x00\x00", 4);
	gcommand=GetCommand(tty, &s);		//ack 0

	/* get position -> PVT */
	SendCommand(tty, "\x0a\x02\x31\x00", 4);

	gcommand=GetCommand(tty, &s);	//ack x31

	gcommand=GetCommand(tty, &s);	//pvt 

	if( gcommand!=NULL )
	{
	  if ( (gcommand[0]==0x33) && (s==66) && (((unsigned char)gcommand[1]+2)==s) )
  	{
			ACKCommand(tty, gcommand, s);  	  
			position.lat=char2double(&gcommand[28]);
			position.lon=char2double(&gcommand[36]);
		}
		else
		{
			/* invalid packet */
		}
	}
	
	/* stop pvt transfer */
	SendCommand(tty, "\x0a\x02\x32\x00", 4);
	gcommand=GetCommand(tty, &s);

	return position;
}


D600_Date_Time_Type getTime(int tty)
{
	unsigned int s;
	char *gcommand;
	D600_Date_Time_Type back;
	
	back.month=0;
	back.day=0;
	back.year=0;
	back.hour=0;
	back.minute=0;
	back.second=0;
	
	/* get time (A600, D600) */
	SendCommand(tty, "\x0a\x02\x05\x00", 4); // 5 transfer time

	gcommand=GetCommand(tty, &s); //ack

	if (gcommand==NULL)
	{
		return back;
	}

	gcommand=GetCommand(tty, &s); // 14 (\x0e) date_time_data

	if( (gcommand[0]==0x0e) && (gcommand[1]==8) && (s==10) )
	{
		back.month=gcommand[2];
		back.day=gcommand[3];
		back.year=(BYTE)gcommand[4]+(BYTE)gcommand[5]*256;
		back.hour=(BYTE)gcommand[6]+(BYTE)gcommand[7]*256;
		back.minute=gcommand[8];
		back.second=gcommand[9];

		ACKCommand(tty, gcommand, s);

	}

	return back;	
}	


D800_Pvt_Data_Type getAllpvtD800(int tty)
{

	D800_Pvt_Data_Type pvt;

	unsigned int s;
	char *gcommand;

	pvt.alt=0;
	pvt.epe=0;
	pvt.eph=0;
	pvt.epv=0;
	pvt.fix=0;
	pvt.tow=0;
	pvt.posn.lat=0;
	pvt.posn.lon=0;
	pvt.east=0;
	pvt.north=0;
	pvt.up=0;
	pvt.msl_hght=0;
	pvt.leap_scnds=0;
	pvt.wn_days=0;
     
	/* abort current transfer */
	SendCommand(tty, "\x0a\x02\x32\x00", 4);
	gcommand=GetCommand(tty, &s);		//ack10

	/* start transfer */	
	SendCommand(tty, "\x0a\x02\x31\x00", 4);
	gcommand=GetCommand(tty, &s);	//ack10
	
	gcommand=GetCommand(tty, &s);	//pvtdata33

	if (gcommand!=NULL)
	{

		if ( (gcommand[2]=0x33) && (s==66) && (((unsigned char)gcommand[1]+2)==s)  )
		{  

/*	printf("Alt: %f EPE: %f EPEh: %f EPEv %f fix: %d\n", 
			char2float(&gcommand[2]), char2float(&gcommand[6]),
			char2float(&gcommand[10]), char2float(&gcommand[14]),
			(int)((BYTE)(gcommand[18])+((BYTE)(gcommand[19])*256))
			);

	fprintf(stderr, "VelocityE: %f VelocityN: %f VelUP: %f Height %f leap_sec: %d\n", 
			char2float(&gcommand[44]), char2float(&gcommand[48]),
			char2float(&gcommand[52]), char2float(&gcommand[56]),
			(int)(((BYTE)(gcommand[60]))+(((BYTE)(gcommand[61]))*256))
			);

	printf("wn_days: %ld timeofweek: %lf\nlatitude: %lf longitude: %lf\nlat (deg): %3.5lf long(deg): %3.5lf\n", 
			char2long(&gcommand[62]), char2double(&gcommand[20]),
			char2double(&gcommand[28]), char2double(&gcommand[36]),
			rad2deg(char2double(&gcommand[28])), rad2deg(char2double(&gcommand[36]))
			
			);*/

			pvt.alt=char2float(&gcommand[2]);
			pvt.epe=char2float(&gcommand[6]);
			pvt.eph=char2float(&gcommand[10]);
			pvt.epv=char2float(&gcommand[14]);
			pvt.fix=(int)((BYTE)(gcommand[18])+((BYTE)(gcommand[19])*256));
			pvt.tow=char2double(&gcommand[20]);
			pvt.posn=char2radiantype(&gcommand[28]);
			pvt.east=char2float(&gcommand[44]);
			pvt.north=char2float(&gcommand[48]);
			pvt.up=char2float(&gcommand[52]);
			pvt.msl_hght=char2float(&gcommand[56]);
			pvt.leap_scnds=(int)(((BYTE)(gcommand[60]))+(((BYTE)(gcommand[61]))*256));
			pvt.wn_days=char2long(&gcommand[62]);

		}
		else
		{
			/* invalid packet */
		}
	
	}
                            
	SendCommand(tty, "\x0a\x02\x32\x00", 4); //50 (\x32) end pvt
	gcommand=GetCommand(tty, &s);	//ack10


	return pvt;
}