oem.c

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

/*****************************************************************
*                                                                *
*                     YH Technology                              *
*                  (c) Copyright 2003                            *
*                                                                *
*      Copying is prohibited except by written permission.       *
*                                                                *
******************************************************************/
#include "yhtype.h"
#if(PC_EVAL==0)
    #include "uart.h"
#endif
#define GPSMAIN
   #include "gps.h"
#undef GPSMAIN
#if(PC_EVAL==1)
    #include "..\post\raw.h"
#endif
#define DEBUG_OEM       (0)

READ_STATE GNState = STARTUP;
static unsigned char CH_GPS;


/**********************************************************************
**
** Function: OEM char sending with some delay to make sure OEM
**           get the right commands
**
**
** Global Input: 
**               None
**
** Global Output: 
**               None
**            
***********************************************************************/        
static void OEM_Char_send
    (
    unsigned char *chr, 
    int size 
    )
{
    int i;

    for(i=0;i<size;i++)
    {
#if(PC_EVAL==0)
       Char_send(CH_GPS, (unsigned char *)(&(chr[i])), 1);
       Delay_1ms(); 
#endif
    }
}


/**********************************************************************
**
** Function: GPS Initialization
**
**
** Global Input: 
**               None
**
** Global Output: 
**               None
**            
***********************************************************************/        
void Gps_init
    (
    unsigned char ch
    )
{
    unsigned char setClockEnable[]={"clockadjust enable\r\n"};

    CH_GPS = ch;

#if(PC_EVAL==0)
    init_UART(CH_GPS, 0);

    OEM_Char_send(setClockEnable, 20);
    Delay_1s();

#endif
}

/**********************************************************************
**
** Function: Calculate a CRC value to be used by CRC calculation function.
**
**
** Global Input: 
**               None
**
** Global Output: 
**               None
**            
***********************************************************************/        
#define CRC32_POLYNOMIAL  0xEDB88320L
static unsigned long CRC32Val
    (
    int i
    )
{

    int j;
    unsigned long ulCRC;

    ulCRC = i;
    for(j=8; j>0; j--)
    {
        if(ulCRC & 1)
            ulCRC = (ulCRC>>1)^CRC32_POLYNOMIAL;     
        else
            ulCRC >>= 1;
    }
    return ulCRC; 
}


/**********************************************************************
**
** Function: Calculate a CRC-32 of a block of data all at once.
**
**
** Global Input: 
**               None
**
** Global Output: 
**               None
**            
***********************************************************************/        
static unsigned long CalculateBlockCRC32
    (
    unsigned long ulCount, 
    unsigned char *buf
    )
{
    unsigned long ulTemp1;
    unsigned long ulTemp2;
    unsigned long ulCRC = 0;

    while (ulCount-- != 0)
    {
        ulTemp1 = (ulCRC>>8) & 0x00FFFFFFL;
        ulTemp2 = CRC32Val(((int)ulCRC^(*(buf++)))&0xff);
        ulCRC = ulTemp1^ulTemp2;
    }
    return ulCRC; 
}

/**********************************************************************
**
** Function: Receiving GPS Meas.
**
**
** Global Input: 
**               None
**
** Global Output: 
**               None
**            
***********************************************************************/        
unsigned char ReceiveGPS
    (
    NAV_USER_TYPE *Data,
    float * Rt2e
    )
{
    unsigned long cnt, k, i, CRC32;
    long double ecPos[3];                            //ECEF系统下个三个位置坐标

    static int  msg_length, msg_id, recCnt = 0, week;
    static unsigned char buf[200], chr; 
    
#if(PC_EVAL==0)
    unsigned char buff[150];    

    cnt  =  Q_uart_size(CH_GPS);
    
    if( 
        ((GNState <= HEADER) && (cnt >= 28)) ||
        ((GNState > HEADER) && (cnt>(msg_length + 31 - recCnt)) ) 
      )
    {
        if(GNState <= HEADER)
        {
            cnt = 28;
            // Read header byte from port
            read_uart(CH_GPS, (unsigned char *)buff, cnt);
        }
        else
        {        
            // Read a byte from port
            cnt = msg_length + 32 - recCnt;
            
            if(cnt>100)
                cnt = 100;
            // Reading msg bytes with each time <= 200
            read_uart(CH_GPS, (unsigned char *)buff, cnt);
        }

        for(k=0; k<cnt; k++){
            chr = buff[k];
            
            if(recCnt>=145)
            {
                recCnt = 0;
                GNState = STARTUP;
                return;
            }
#else
    if(TRUE)
    {
        cnt = 160;

        for(k=0; k<cnt; k++)
        {
			chr = dataBuff[k+4];
#endif           
            //States of CMR reading
            switch (GNState)
            {
            // Initial reading
            case STARTUP:
                buf[2] = chr;
            
                if((buf[0]==NOV_STX) && (buf[1]==NOV_SYN1) && (buf[2]==NOV_SYN2))
                {
                    GNState = HEADER;
                    recCnt = 3;
                }
                else
                {
                    buf[0] = buf[1];
                    buf[1] = buf[2];
                }
                
                break;

            case HEADER:
                buf[recCnt++] = chr;

                if(recCnt>=28)
                {
                    // Set the state
                    GNState = DATABODY;
                    // Set msg_id
                    msg_id = buf[4]+buf[5]*256;

                    // Set msg_length
                    msg_length = buf[8]+buf[9]*256; 
                    
                    // Set Week number
                    week = buf[14]+buf[15]*256;

                    // Set million seconds
                    Data->milSec = (unsigned long)((buf[16]&0x000000ff)|
                                                ((buf[17]<<8)&0x0000ff00)|
                                                ((buf[18]<<16)&0x00ff0000)| 
                                                ((buf[19]<<24)&0xff000000));

                    // Seconds & rounding
                    Data->Seconds = (float)(Data->milSec)/1000.0;
        
#if(PC_EVAL==1)
                    fprintf(outH3,"\n%ld\t%.4lf\t", Data->milSec, Data->Seconds);
#endif                    
                    // Set receiver status
                    //memcpy(&milli_sec, buf+16, sizeof(unsigned int));
                    //memcpy(&rsv_status, buf+20, sizeof(unsigned int));

                    // Re-judge message length, too big msg can be handled
                    if(msg_length>100)
                    {
                        GNState = STARTUP;
                        recCnt = 0;
                    }
                }
                break;
    
            // Get the data body of the msg
            case DATABODY:    
                buf[recCnt++] = chr;
            
                if(recCnt>(msg_length+30)) 
                    GNState = FINISH;
                break;
    
            // Data reading done
            case FINISH:
                // Reset the state
                GNState = STARTUP;
                buf[recCnt] = chr;                                                                    

		        /// Decode CRC32
		        CRC32 = ( ((buf[recCnt]<<24) & 0xff000000L) |
		                  ((buf[recCnt-1]<<16) & 0x00ff0000L) |
		                  ((buf[recCnt-2]<<8)  & 0x0000ff00L) |
		                  ( buf[recCnt-3] & 0x000000ffL) );
		
                recCnt = 0;
		        /// Calculate & Compare CRC32
		        if(CalculateBlockCRC32(28+msg_length,buf) != CRC32)
		            return FALSE;
#if(RAW_DATA==1)     
		        if((msg_id==0x2A)||(msg_id==0x63)||(msg_id==0xAE))          //选择读取的数据类型
		        {
                    Data->rlength = msg_length + 32;
		                
	      	        Data->rbuff[0] = '$';
		   	        if(msg_id == 0x2A)
	    	  	        Data->rbuff[1] = READ_GNSS_POS;      				
	      	        else if(msg_id == 0x63)
	    	  	        Data->rbuff[1] = READ_GNSS_VEL;      		
	      	        else if(msg_id == 0xAE)
	    	  	        Data->rbuff[1] = READ_GNSS_DOP;
	    	        
		  	        Data->rbuff[2] = Data->rlength+5;              //数据为8位，寄存单元4位
		  	        Data->rbuff[3] = (Data->rlength+5)/256;        //为什么要除以256?? 把高4位的放入下一个寄存器。
		  	        Data->rbuff[4]++;
		  	        Data->rbuff[5] = 0;		  
            
		            for(i=0; i<Data->rlength; i++)
		                Data->rbuff[i+6] = buf[i];

	    	        Data->rbuff[Data->rlength+6] = calcCheckSum(&Data->rbuff[1], Data->rlength+5);    	    	  		

			        Data->rready = TRUE;
			        //return TRUE;    	          	      				          	          	
	            }
#endif 

#if(DEBUG_OEM==1)
                /// DATA PRINTF
                i = 0;
		        i += intToUChar(msg_id, &buff[i]);
	            buff[i++] = ' ';    
		        i += intToUChar(msg_length, &buff[i]);
	            buff[i++] = ' '; 
		        i += intToUChar(Data->milSec, &buff[i]);
	            buff[i++] = ' '; 
#endif        

                /// Message NAV-POSLLH(0x01 0x02)
                if(msg_id==0x2A)  
                {
                    // Position Decode 
                    Data->LatLong[0] = (long double)(ToDouble(&buf[28+8]))*((long double)(0.01745329252));         //接收机的位置坐标(LatLongHigh)  ToDouble函数?
                    Data->LatLong[1] = (long double)(ToDouble(&buf[28+16]))*((long double)(0.01745329252));
                    Data->LatLong[2] = (long double)(ToDouble(&buf[28+24]));
                    Data->Nb_SV = buf[28+65];                                                                      //卫星的数量

                    /// Calculate the XYZ, Rt2e, S12t from LLH
                    calc_llh2xyz_Rt2e_Sl2t(Data->LatLong, ecPos, Rt2e, Data->Sl2t);

                    Data->posCnt++;
#if(PC_EVAL==1)
                    fprintf(outH3,"\t %.7lf\t %.7lf\t %.3lf\t ", Data->LatLong[0], Data->LatLong[1], Data->LatLong[2]);
                    fprintf(outH3,"\t %.7lf\t %.7lf\t %.3lf\t ", Data->Sl2t[0], Data->Sl2t[1], Data->Sl2t[2]);
#endif     
#if(DEBUG_OEM==1)
                    i += doubleToUChar(Data->LatLong[0], 8, &buff[i]);
	                buff[i++] = ' '; 
                    i += doubleToUChar(Data->LatLong[1], 8, &buff[i]);
	                buff[i++] = ' '; 
                    i += doubleToUChar(Data->LatLong[2], 2, &buff[i]);
	                buff[i++] = ' '; 
                    i += doubleToUChar(Data->Sl2t[0], 2, &buff[i]);
	                buff[i++] = ' '; 
                    i += doubleToUChar(Data->Sl2t[1], 2, &buff[i]);
	                buff[i++] = ' '; 
                    i += doubleToUChar(Data->Sl2t[2], 2, &buff[i]);
	                buff[i++] = ' '; 
	                buff[i++] = '\r';    
	                buff[i++] = '\n';    
                    Char_send(CH_MAG_UART, buff, i);
#endif                            
                    // Special Valid for INS Initialization
                    if((Data->Nb_SV>=4)&&(Data->GDOP<20.0))
                        Data->Valid = TRUE;
                    else
                        Data->Valid = FALSE;

                    return (Data->Valid);
                }
                /// Message NAV_VELNED(0x01 0x12)
                else if(msg_id==0x63)
                {    
                    // Velocity Decode 
                    Data->Speed = (float)(ToDouble(&buf[28+16]));                    //地面速率
                    Data->GND_Angle = ToDouble(&buf[28+24])*0.01745329252;           //地面航向
                    Data->NEV_Vel[0] = Data->Speed * cos(Data->GND_Angle);           //East      //卫星三个方向的速度数据
                    Data->NEV_Vel[1] = Data->Speed * sin(Data->GND_Angle);           //North
                    Data->NEV_Vel[2] = -ToDouble(&buf[28+32]);                       //sky
                    // Special Valid for INS Initialization
                    Data->velCnt++;
#if(PC_EVAL==1)
                    fprintf(outH3,"\t %.3lf\t %.3lf\t %.3lf\t ", Data->NEV_Vel[0], Data->NEV_Vel[1], Data->NEV_Vel[2]);
#endif        
                    
#if(DEBUG_OEM==1)
                    i += doubleToUChar(Data->NEV_Vel[0], 3, &buff[i]);             //doubleToUChar函数结构!  
	                buff[i++] = ' '; 
                    i += doubleToUChar(Data->NEV_Vel[1], 3, &buff[i]);
	                buff[i++] = ' '; 
                    i += doubleToUChar(Data->NEV_Vel[2], 3, &buff[i]);
	                buff[i++] = ' '; 
	                buff[i++] = '\r';    
	                buff[i++] = '\n';    
                    Char_send(CH_MAG_UART, buff, i);
#endif        
                    if((Data->Nb_SV>=4)&&(Data->GDOP<20.0))       //卫星数>=4且GDOP<20.0返回DATA有效
                    {
                        Data->Valid = TRUE+1;
                        Data->velValid = TRUE;
                    }
                    else
                        Data->velValid = FALSE;

                    return (Data->Valid);
                }
                /// Message NAV_DOP(0x01 0x04)
                else if(msg_id==0xAE)
                {
                    Data->GDOP = ToFloat(&buf[28]);           //精度因子数据
                    Data->PDOP = ToFloat(&buf[32]);
#if(PC_EVAL==1)
                    fprintf(outH3,"\t %.3lf\t %.3lf\t ", Data->GDOP, Data->PDOP);
#endif                    
#if(DEBUG_OEM==1)
                    i += doubleToUChar(Data->GDOP, 3, &buff[i]);
	                buff[i++] = ' '; 
                    i += doubleToUChar(Data->PDOP, 3, &buff[i]);
	                buff[i++] = ' '; 
	                buff[i++] = '\r';    
	                buff[i++] = '\n';    
                    Char_send(CH_MAG_UART, buff, i);
#endif        
                    if(Data->Nb_SV>=4)
                        Data->Valid = TRUE+3;
                    else
                        Data->Valid = 0;

                    return (Data->Valid);
                }
                else
                    return FALSE;
                    
            default :
                break;
            }
        } //End of for( ; ;)
    }
        
    return FALSE;
}