convert.c

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

/***************************************************************************
               convert.c  -  various format converting 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 <math.h>
#include "def.h"

/*	convert first 4 bytes to float, lsb first	*/ 
float char2float(BYTE *ch)
{
	BYTE *start;
	float back;
	int i;
	
	start=(BYTE *)&back;

	for( i=0; i<=3; i++ )
	{
		start[i]=ch[i];
	}

	return back;	
}


/*	convert first 4 bytes to long, lsb first	*/ 
long char2long(BYTE *ch)
{
	long ff;
	
	ff=(long)(((((BYTE)ch[3]*256*256*256) + (BYTE)ch[2]*256*256) 
				+ (BYTE)ch[1]*256) + (BYTE)ch[0]);

	return ff;
}


/* convert first 8 bytes to double	*/
double char2double(BYTE *ch)
{
	BYTE *start;
	double back;
	int i;
	
	start=(BYTE *)&back;

	for( i=0; i<=7; i++ )
	{
		start[i]=ch[i];
	}

	return back;
}

/* convert firsz 16 bytes to Radian_Type  */
Radian_Type char2radiantype(BYTE *ch)
{
	Radian_Type back;
      
	back.lat=char2double(&ch[0]);
	back.lon=char2double(&ch[8]);
                  
	return back;
}


/*	convert degrees to radian	*/
double deg2rad(double deg)
{
	double radian;

	radian=deg*(pi/180);

	return radian;	
}


/*	convert radian to degrees	*/
double rad2deg(double rad)
{
	double degree;
	
	degree=rad*(180/pi);
	
	return degree;
}


double directionrad(double vn, double ve)
{
	double dd=0;
	double r;

	if ( (vn==0)&&(ve==0) )
		return 0;
		
	/* normalize */
	r=sqrt((vn*vn)+(ve*ve));

	ve=ve/r;
	vn=vn/r;

	if ( (vn>0) && (ve>=0) )
	{
		// 1. Quadrant
		dd=atan(ve/vn);
	}
	else if ( (vn<=0 ) && ( ve>0 ) )	
	{
		// 4. Quadrant	
		dd=atan(-vn/ve)+pi/2;
	}
	else if ( (vn<0) && (ve<=0) )		
	{
		// 3. Quadrant	
		dd=pi+atan(ve/vn);
	}
	else if ( (vn>=0) && (ve<0) )	
	{
		// 2. Quadrant
		dd=(3*pi/2)+atan(-vn/ve);
	}

	return dd;
}