decodesick.cpp

针对激光扫描仪sick的扫描数据进行读取

// DecodeSICK.cpp: implementation of the CDecodeSICK class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "FReadWrite.h"
#include "DecodeSICK.h"


#define CRC16_GEN_POL 0x8005
#define MKSHORT(a,b) ((unsigned short) (a) | ((unsigned short) (b) <<8))



#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////


CDecodeSICK::CDecodeSICK()
{
	//m_fileName=fileName;
m_pMeasurement=NULL;
/*
char* pFileName = "test.dat";
TRY
{
   CFile f( pFileName, CFile::modeCreate | CFile::modeWrite );
}
CATCH( CFileException, e )
{
   #ifdef _DEBUG
      afxDump << "File could not be opened " << e->m_cause << "\n";
   #endif
}*/
}

CDecodeSICK::~CDecodeSICK()
{
	if (!m_pMeasurement)
	{
		delete []m_pMeasurement;
	}
	//m_file.Abort();
	 //delete pFileName;
}

WORD CreateCRC(const BYTE *CommData, WORD uLen)
{
	unsigned short uCrc16;
	unsigned char abDATA[2];
	
	uCrc16=0;
    abDATA[0]=0;
	while (uLen--)
	{
		abDATA[1]=abDATA[0];//初始化abDATA[1]
		abDATA[0]=*CommData++;//填充

		if(uCrc16 & 0x8000)
		{
			uCrc16=(uCrc16 & 0x7fff) << 1;
			uCrc16 ^=CRC16_GEN_POL;
		}
		else
		{
			uCrc16 <<= 1;
		}
		
		uCrc16 ^= MKSHORT(abDATA[0],(abDATA[1]));
		
	}
	return(uCrc16);
}
BOOL CDecodeSICK::Decode(float* data)
{
	if (m_file.m_hFile==CFile::hFileNull)
	{
		return FALSE;
	}
	BYTE buf[1024];
	//BYTE errbuff[1024];
	int i=0;
	//int n;
	if(m_file.Read(buf,1))
	{
		if(buf[0]==0x02)
		{
			if(m_file.Read(&buf[1],1))
			{
				if (m_file.Read(&buf[2],2))
				{
					WORD len=(buf[3]<<8)+buf[2];
					WORD readLen=0;
					if (readLen=m_file.Read(&buf[4],len+2))
					{
						if (readLen==len+2)
						{
							if (buf[4]=0xB0)
							{
								WORD info=(buf[6]<<8)+buf[5];
								WORD num=info&0x3FF;
							                               
								WORD crc=CreateCRC(buf,len+4);//
								WORD crc1=(buf[len+5]<<8)+buf[len+4];//实际校验
								if (crc!=crc1)
								{
									//TRACE("数据校验错误!\n");									
									for(int j=0;j<len+6;j++)
									{
										if(buf[j]==0x02&&buf[j+1]==0x80)//&&buf[j+2]==0x28&&buf[j+3]==0x03)
										{
									    	m_file.Seek(1,CFile::begin);
											//m_file.Read(buf,j);
										    break;
										}
									}									
									return FALSE;
								}		
								else
								{									
									for (i=0;i<num;i++)
									{										
										data[i]=(float)(buf[7+2*i+1]<<8)+buf[7+2*i];
									}	
									data[num]=(float)(0x00<<8)+buf[7+2*num];
								}
							}
							else
							{
								return FALSE;
							}
						}
						else
						{
							return FALSE;
						}
					}
					else
					{
						return FALSE;
					}
				}
				else
				{
					return FALSE;
				}
			}
			else
			{
				return FALSE;
			}
		}
		else
		{
			return FALSE;
		}
	}
	else
	{
		return FALSE;
	}

	return TRUE;
}

BOOL CDecodeSICK::Open(CString& lpszFileName)
{ 
	if (m_file.m_hFile!= CFile::hFileNull)
	{
		m_file.Abort();
	}
	
	if(!m_file.Open(lpszFileName,CFile::modeRead))
	{
		return FALSE;
	}

	m_Length=m_file.GetLength();
	return TRUE;
}