nov.c

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

    for(i=0; i<3; i++)
    {
        Solv->vel_n[i] = 0.0;
        
        for(j=0; j<3; j++)
            Solv->vel_n[i] += Solv->Rt2e[i+j*3] * vel_xyz[j];

        Solv->speed += (float)(vel_xyz[i] * vel_xyz[i]);
    }
    
    //// Solution of Heading based on velocity
    Solv->angV = atan2(Solv->vel_n[1], Solv->vel_n[0]);
    Solv->speed = (float)(sqrt(Solv->speed));
}


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

    int j;
    unsigned int 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
**            
***********************************************************************/        
unsigned int CalculateBlockCRC32
    (
    unsigned int ulCount, /*Number of bytes in the data block*/
    unsigned char *buf
    )
{
    unsigned int ulTemp1;
    unsigned int ulTemp2;
    unsigned int ulCRC = 0;

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


/**********************************************************************
**
** Function: Read NOV Measurements
**
**
** Global Input: 
**                None
**
** Global Output: 
**                None
**            
***********************************************************************/
int readNOV
    (
    GNSS_SYS_TYPE *Gnss,                 // Pointer of GNSS System
    int COMPort
    )
{
    unsigned char chr, buff[210]; 
    int cnt, k, i, CRC, CRC1, rcv;
    RAW_EPHEM_TYPE  blks;
    GNSS_MEAS_TYPE *Gmeas;
    GNSS_SOLUTION_TYPE *Solv;  
    GNSS_RCV_TYPE  *rcvr;          
    GNSS_EPHEM_TYPE *Ephem = &Gnss->Ephem; 
#if(RAW_DATA==1)     
    static unsigned char rcv_cnt[2] = {0, 0};    
#endif    

    
    if((COMPort>=0)&&(COMPort<=1))
    	rcv = COMPort;
    else
    	return FALSE;    	

    Gmeas = &Gnss->Meas[rcv];
    Solv = &Gnss->Solv[rcv];
    rcvr = &Gnss->rcv[rcv];
        
#if(PC_SIM==0)
	//// Quote the size of the COM Buffer
    cnt  =  QUARTSize(COMPort);
        
    if( 
        ((GNState[rcv] <= HEADER) && (cnt >= 28)) ||
        ((GNState[rcv] > HEADER) && (cnt>(rcvr->msg_length + 31 - rcvr->recCnt)) ) 
      )
    {
        if(GNState[rcv] <= HEADER)
        {
            cnt = 28;
            // Read header byte from port
            readUART(COMPort, buff, cnt);
        }
        else
        {        
            // Read a byte from port
            cnt = rcvr->msg_length + 32 - rcvr->recCnt;
            
            if(cnt>200)
                cnt = 200;
            // Reading msg bytes with each time <= 200
            readUART(COMPort, buff, cnt);
        }

        for(k=0; k<cnt; k++)
        {
            chr = buff[k];
            
            if(rcvr->recCnt>=800)
            {
                rcvr->recCnt = 0;
                GNState[rcv] = STARTUP;
                return FALSE;
            }
#else
    if(TRUE)
    {
        cnt = 800;
        for(k=0; k<cnt; k++)
        {
			chr = dataBuff[k+4];
#endif           
            //States of CMR reading
            switch (GNState[rcv])
            {
            // Initial reading
            case STARTUP:
                rcvr->rbuff[2] = chr;
            
                if((rcvr->rbuff[0]==NOV_STX) && (rcvr->rbuff[1]==NOV_SYN1) && (rcvr->rbuff[2]==NOV_SYN2))
                {
                    GNState[rcv] = HEADER;
                    rcvr->recCnt = 3;
                }
                else
                {
                    rcvr->rbuff[0] = rcvr->rbuff[1];
                    rcvr->rbuff[1] = rcvr->rbuff[2];
                }
                
                break;

            case HEADER:
                rcvr->rbuff[rcvr->recCnt++] = chr;

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

                    // Set msg_length
                    rcvr->msg_length = rcvr->rbuff[8]+rcvr->rbuff[9]*256; 
                    
                    // Set Week number
                    rcvr->week = rcvr->rbuff[14]+rcvr->rbuff[15]*256;

                    // Set million seconds
                    memcpy(&rcvr->milli_sec, rcvr->rbuff+16, sizeof(unsigned int));
                    
                    // Set receiver status
                    memcpy(&rcvr->rsv_status, rcvr->rbuff+20, sizeof(unsigned int));

                    // Re-judge message length, too big msg can be handled
                    if(rcvr->msg_length>760)
                    {
                        GNState[rcv] = STARTUP;
                        rcvr->recCnt = 0;
                    }
                }
                break;
    
            // Get the data body of the msg
            case DATABODY:    
                rcvr->rbuff[rcvr->recCnt++] = chr;
            
                if(rcvr->recCnt>(rcvr->msg_length+30)) 
                    GNState[rcv] = FINISH;
                break;
    
            // Data reading done
            case FINISH:
                // Reset the state
                GNState[rcv] = STARTUP;
                rcvr->rbuff[rcvr->recCnt] = chr;               
                                                     
                memcpy(&CRC, rcvr->rbuff+rcvr->recCnt-3, sizeof(unsigned int));
                                     
                CRC1 = CalculateBlockCRC32((unsigned int)(rcvr->msg_length+28), rcvr->rbuff);
    
    			rcvr->rlength = rcvr->recCnt+1;
    			
                rcvr->recCnt = 0;
                
                // Calculate the CRC
                if( CRC == CRC1)
                {
#if(RAW_DATA==1)     
					if((rcvr->msg_id == NOV_RAW)||(rcvr->msg_id == NOV_SOLV)||(rcvr->msg_id == NOV_EPHM))
					{           
		                rcvr->rready = TRUE;
		                for(i=rcvr->rlength-1; i>=0; i--)
		                	rcvr->rbuff[i+6] = rcvr->rbuff[i];
		                	
	      				rcvr->rbuff[0] = '$';
		   				if(rcvr->msg_id == NOV_RAW)
	    	  				rcvr->rbuff[1] = READ_GNSS_RAW;      				
	      				else if(rcvr->msg_id == NOV_SOLV)
	    	  				rcvr->rbuff[1] = READ_GNSS_SOLV;      				
	      				else if(rcvr->msg_id == NOV_EPHM)
	    	  				rcvr->rbuff[1] = READ_GNSS_EPH;
	    	  			
		  				rcvr->rbuff[2] = rcvr->rlength+5;
		  				rcvr->rbuff[3] = (rcvr->rlength+5)/256;
		  				rcvr->rbuff[4] = rcv_cnt[rcv]++;
		  				rcvr->rbuff[5] = rcv;		  					    	  				
	    	  			rcvr->rbuff[rcvr->rlength+6] = calcCheckSum(&rcvr->rbuff[1], rcvr->rlength+5);    	    	  				
						rcvr->rready = TRUE;	 
						return TRUE;   	  			  				      				          	          	      				          	          	
	                }
#endif                 
                    if(rcvr->msg_id == NOV_RAW)
                    {
                        // Decode the Epoch Time
                        Gmeas->Time = rcvr->milli_sec/1000.0;
#if(PC_SIM==1)                        
                        fprintf(OUTH3, "\n%d\t%.2lf\t", rcv, Gmeas->Time);
#endif                        
                        readRawMeas(&rcvr->rbuff[28], Gmeas, Ephem, rcv);

                        return TRUE;
                    }
                    else if(rcvr->msg_id == NOV_SOLV)
                    {                        
                        Solv->week = rcvr->week;
                        Solv->seconds = rcvr->milli_sec/1000.0;
#if(PC_SIM==1)                        
                        fprintf(OUTH3, "\n%d\t%.1lf\t", rcv, Solv->seconds);
#endif                         
                        readSolution(&rcvr->rbuff[28], Solv);
                        
                        return (TRUE+1);                    
                    }
                    else if(rcvr->msg_id == NOV_EPHM)
                    {
                        // YY Algrithm 
                        blks.Svs = rcvr->rbuff[28]-1;

                        for(i=0; i<24; i++)
                        {
                            blks.Bytes[i] = rcvr->rbuff[28+i+18];
                            blks.Bytes[24+i] = rcvr->rbuff[28+i+48];
                            blks.Bytes[48+i] = rcvr->rbuff[28+i+78];
                        }
                        readRawEphemMeas( blks, Ephem );

                        return (TRUE+2); 
                    }
                    else
                        return FALSE;
                }                    
            default :
                break;
            }
        } //End of for( ; ;)
    }
        
    return FALSE;
}