raw.c

GPS_OEM程序用于接收、辨别、发送和校验GPS信息。支持nov协议

/*****************************************************************
*                                                                *
*                  NAV Technology Co., Ltd.                      *
*                      Copyright 2005                            *
*                                                                *
*      Copying is prohibited except by written permission.       *
*                                                                *
******************************************************************/
/*Header file*/

#include "..\src\yhdefs.h"
#include "..\src\yhtype.h"
#include <stdio.h>
#include <stdlib.h>
#define RAW_DEF
	#include "raw.h"
#undef RAW_DEF

#define PDA_BINARY		(1)

/**********************************************************************
**
** Function: Set the output file
**
**
** Global Input: 
**                None
**
** Global Output: 
**                None
**            
***********************************************************************/
void SetOutputFile
    (
    void
    )
{
	static int FID = 0;
	unsigned char chr = '%', buff[20]; 

	if(outH1!=NULL)
		fclose(outH1);
	if(outH2!=NULL)
		fclose(outH2);
	if(outH3!=NULL)
		fclose(outH3);
	if(outH4!=NULL)
		fclose(outH4);

	sprintf(buff, "CMPS_%02d.txt", ++FID);
	if((outH2=fopen(buff, "w"))==NULL)
	{
		printf("\nCan Not open writing file compass.txt!");
		exit(0);
	}

	sprintf(buff, "RLT_%02d.txt", FID);
	if((outH1=fopen(buff, "w"))==NULL)
	{
		printf("\nCan Not open writing file RLT.txt!");
		exit(0);
	}

	sprintf(buff, "GNSS_%02d.txt", FID);
	if((outH3=fopen(buff, "w"))==NULL)
	{
		printf("\nCan Not open writing file GNSS.txt!");
		exit(0);
	}

	sprintf(buff, "IMU_%02d.txt", FID);
	if((outH4=fopen(buff, "w"))==NULL)
	{
		printf("\nCan Not open writing file imu.txt!");
		exit(0);
	}
	else
	{
		fprintf(outH4, "\n%c%cGyro_x\tGyro_y\tGyro_z\tAcc_x\tAcc_y\tAcc_z\tTimer\t", chr, chr);
	}

	sprintf(buff, "KF_%02d.txt", FID);
	if((outH5=fopen(buff, "w"))==NULL)
	{
		printf("\nCan Not open writing file KF.txt!");
		exit(0);
	}

	sprintf(buff, "INS_%02d.txt", FID);
	if((outH6=fopen(buff, "w"))==NULL)
	{
		printf("\nCan Not open writing file INS.txt!");
		exit(0);
	}
}


/**********************************************************************
**
** Function: Install RAW data file
**
**
** Global Input: 
**                None
**
** Global Output: 
**                None
**            
***********************************************************************/
void InstallRaw
    (
    void
    )
{
#if(PDA_BINARY==TRUE)
	if((inH1=fopen("RAW.HEX", "rb"))==NULL)
	{
		printf("\nCan Not open reading file raw.txt!");
		exit(0);
	}
#else
	if((inH1=fopen("raw.txt", "rt"))==NULL)
	{
		printf("\nCan Not open reading file raw.txt!");
		exit(0);
	}
#endif
	SetOutputFile();
}


/**********************************************************************
**
** Function: IMU Data transfer into the angle rate & acceleration 
**
**
** Global Input: 
**               None
**
** Global Output: 
**               None
**            
***********************************************************************/        
void ImuRawDataSet
    (
    ADStruct *AD
    )
{
    unsigned char byte;
    short tmp, i;

    byte = dataBuff[5] & 0x0000000c;
    if(byte==Racc_2g)
        AD->acc_range = 2.0;
    else if(byte==Racc_50g)
        AD->acc_range = 50.0;
    else if(byte==Racc_10g)
        AD->acc_range = 10.0;
    else
        AD->acc_range = 5.0;

    //AD->acc_range = 10.0;

    byte = dataBuff[5] & 0x00000030;
    if(byte==Rgyro_100)
        AD->gyro_range = 100.0;
    else if(byte==Rgyro_300)
        AD->gyro_range = 300.0;
    else if(byte==Rgyro_400)
        AD->gyro_range = 400.0;
    else 
        AD->gyro_range = 200.0;

    AD->CH_data[7] = dataBuff[6] + dataBuff[7]*256; 
    for(i=0; i<7; i++)
    {
        tmp = (short)(dataBuff[8+2*i] + dataBuff[9+2*i]*256);
        AD->CH_data[i] = tmp; 
        AD->flt_data[i] = (float)tmp; 
    }
    /*///////////// -Rotating X&Z axes- /////////////////
    tmp = (short)(AD->flt_data[3]);
    AD->flt_data[3] = AD->flt_data[5];
    AD->flt_data[5] = (float)(-tmp);
    ///////////////////////////////*/
}


/**********************************************************************
**
** Function: Read RAW data from file
**
**
** Global Input: 
**                None
**
** Global Output: 
**                None
**            
***********************************************************************/
int ReadRaw
    (
    void 
    )
{
	static unsigned char checkSum;
	unsigned int i, cnt;

#if(PDA_BINARY==TRUE)
	if(fread(&dataBuff[0], sizeof(unsigned char), 1, inH1)!=1)
		exit(0);

	if(fread(&dataBuff[1], sizeof(unsigned char), 1, inH1)!=1)
		exit(0);

	while(dataBuff[0]!='$'||dataBuff[1]>5)
	{
		dataBuff[0] = dataBuff[1];
		if(fread(&dataBuff[1], sizeof(unsigned char), 1, inH1)!=1)
			exit(0);
	}
	if(fread(&dataBuff[2], sizeof(unsigned char), 1, inH1)!=1)
		exit(0);

	if(dataBuff[1]<=2)
	{
		cnt = dataBuff[2];
		if(cnt<60&&cnt>2)
		{
			if(fread(&dataBuff[3], sizeof(unsigned char), cnt-1, inH1)!=(cnt-1))
				exit(0);
		}
	}
	else
	{
		if(fread(&dataBuff[3], sizeof(unsigned char), 1, inH1)!=1)
			exit(0);
		cnt = dataBuff[2] + 256 * dataBuff[3];
		if(cnt<760&&cnt>2)
		{
			if(fread(&dataBuff[4], sizeof(unsigned char), cnt-2, inH1)!=(cnt-2))
				exit(0);
		}
	}		
#else
	if(fscanf(inH1, "%x ", &dataBuff[0])!=1)
		exit(0);
	if(fscanf(inH1, "%x ", &dataBuff[1])!=1)
		exit(0);

	while(dataBuff[0]!='$'||dataBuff[1]>5)
	{
		dataBuff[0] = dataBuff[1];
		fscanf(inH1, "%x ", &dataBuff[1]);
	}
	fscanf(inH1, "%x ", &dataBuff[2]);
	
	if(dataBuff[1]<=2)
	{
		cnt = dataBuff[2];
		if(cnt<60&&cnt>2)
		{
			for(i=3; i<cnt+2; i++)
				fscanf(inH1, "%x ", &dataBuff[i]);
		}
	}
	else
	{
		fscanf(inH1, "%x ", &dataBuff[3]);
		cnt = dataBuff[2] + 256 * dataBuff[3];
		if(cnt<760&&cnt>2)
		{
			for(i=4; i<cnt+2; i++)
				fscanf(inH1, "%x ", &dataBuff[i]);
		}
	}		
#endif

	checkSum = calcCheckSum(&dataBuff[1], cnt);

	if(dataBuff[cnt+1]==checkSum)
		return TRUE;
	else 
		return FALSE;

}