完善Tesa star e测试功能。
This commit is contained in:
TAO Cheng
@@ -1,9 +1,7 @@
|
||||
#include "StdAfx.h"
|
||||
#include "TesaStarE.h"
|
||||
|
||||
#define PI 3.1415926535897932384626433
|
||||
#define MAX_BUFF_SIZE 0x200
|
||||
|
||||
const int MAX_RETRY_CNTS=5;
|
||||
//==================================================================
|
||||
CTesaStarE::CTesaStarE(void)
|
||||
{
|
||||
@@ -11,14 +9,19 @@ CTesaStarE::CTesaStarE(void)
|
||||
m_pSerial=NULL;
|
||||
_bReading=FALSE;
|
||||
bEnComm=false;
|
||||
m_TesaStarEAngle[0]=(double)0.0;
|
||||
m_TesaStarEAngle[1]=(double)0.0;
|
||||
memset(m_RecvData,0,sizeof(m_RecvData));
|
||||
|
||||
}
|
||||
//==================================================================
|
||||
CTesaStarE::~CTesaStarE(void)
|
||||
{
|
||||
if (m_pSerial)
|
||||
if (this->m_pSerial)
|
||||
{
|
||||
delete m_pSerial;
|
||||
m_pSerial=NULL;
|
||||
this->m_pSerial->Close();
|
||||
delete this->m_pSerial;
|
||||
this->m_pSerial=NULL;
|
||||
}
|
||||
}
|
||||
//==================================================================
|
||||
@@ -39,8 +42,8 @@ BOOL CTesaStarE::Initialization(bool _bInitConfig)
|
||||
if(m_pSerial->Open())
|
||||
{
|
||||
bEnComm=TRUE;
|
||||
status=TRUE;
|
||||
status=CheckStatus();
|
||||
status=SetPassword();
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -58,164 +61,417 @@ BOOL CTesaStarE::Unload()
|
||||
if (m_pSerial)
|
||||
{
|
||||
m_pSerial->Close();
|
||||
delete m_pSerial;
|
||||
m_pSerial=NULL;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
//==================================================================
|
||||
BOOL CTesaStarE::SetTesaStarEAngle(const int& _AngleNumber,double* _Angle,bool _bReturnPosInfo)
|
||||
{
|
||||
BOOL rStatus(FALSE);
|
||||
if (_AngleNumber>=2)
|
||||
{
|
||||
USES_CONVERSION;
|
||||
CString csSendData("");
|
||||
csSendData.Format(L"A%.1f",_Angle[0]);
|
||||
csSendData+=_T("\r");
|
||||
CStringA csaSendData(csSendData);
|
||||
const char* cSendData=csaSendData.GetString();
|
||||
int iSendDataLength=csSendData.GetLength();
|
||||
_Send_Command(cSendData,iSendDataLength);
|
||||
_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
|
||||
csSendData.Format(L"B%.1f",_Angle[1]);
|
||||
csSendData+=_T("\r");
|
||||
csaSendData=csSendData;
|
||||
const char* cSendData2=csaSendData.GetString();
|
||||
iSendDataLength=csSendData.GetLength();
|
||||
_Send_Command(cSendData2,iSendDataLength);
|
||||
_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
|
||||
char bSendData[2]={'U',0X0D};
|
||||
iSendDataLength=2;
|
||||
_Send_Command((const char*)bSendData,iSendDataLength);
|
||||
rStatus=_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
int iRetryCnts(0);
|
||||
if (_bReturnPosInfo)
|
||||
{
|
||||
int iDataStartCnt(0);
|
||||
if(rStatus)
|
||||
{
|
||||
bool IsTheEnd(false);
|
||||
do
|
||||
{
|
||||
if(m_RecvDataSize>16)
|
||||
{
|
||||
rStatus=FALSE;
|
||||
for (int i=0;i<static_cast<int>(m_RecvDataSize);i++)
|
||||
{
|
||||
if (m_RecvData[i]=='U')
|
||||
{
|
||||
rStatus=TRUE;
|
||||
iDataStartCnt=i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if ((m_RecvDataSize-iDataStartCnt)<16)
|
||||
{
|
||||
rStatus=FALSE;
|
||||
}
|
||||
if (rStatus)
|
||||
{
|
||||
if (CheckRecvData(4,&m_RecvData[iDataStartCnt],1,bSendData,0))
|
||||
{
|
||||
if (CheckRecvData(3,&m_RecvData[m_RecvDataSize-3],0,bSendData,1))
|
||||
{
|
||||
char CData[10]={0};
|
||||
int jj(0);
|
||||
int iADataStartCnt(0);
|
||||
int iBDataStartCnt(0);
|
||||
for (int i=(iDataStartCnt+4);i<static_cast<int>(m_RecvDataSize-3);i++)
|
||||
{
|
||||
if (m_RecvData[i]=='H' && m_RecvData[i+1]=='A')
|
||||
{
|
||||
iADataStartCnt=i+2;
|
||||
break;
|
||||
}
|
||||
}
|
||||
for (int i=(iDataStartCnt+4);i<static_cast<int>(m_RecvDataSize-3);i++)
|
||||
{
|
||||
if (m_RecvData[i]=='B')
|
||||
{
|
||||
iBDataStartCnt=i+1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
for (int i=iADataStartCnt;i<(iBDataStartCnt-1);i++)
|
||||
{
|
||||
if (jj<10)
|
||||
{
|
||||
CData[jj++]=m_RecvData[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
m_TesaStarEAngle[0]=atof(CData);
|
||||
|
||||
memset(CData,0,sizeof(CData));
|
||||
jj=0;
|
||||
for (int i=iBDataStartCnt;i<static_cast<int>(m_RecvDataSize-3);i++)
|
||||
{
|
||||
if (jj<10)
|
||||
{
|
||||
CData[jj++]=m_RecvData[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
m_TesaStarEAngle[1]=atof(CData);
|
||||
IsTheEnd=true;
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
}
|
||||
iRetryCnts++;
|
||||
} while ((!IsTheEnd)&&(iRetryCnts<=MAX_RETRY_CNTS));
|
||||
}
|
||||
if(iRetryCnts>MAX_RETRY_CNTS)
|
||||
{
|
||||
_Angle[0]=0.0;
|
||||
_Angle[1]=0.0;
|
||||
CheckStatus();
|
||||
}
|
||||
else
|
||||
{
|
||||
_Angle[0]=m_TesaStarEAngle[0];
|
||||
_Angle[1]=m_TesaStarEAngle[1];
|
||||
}
|
||||
}
|
||||
}
|
||||
return rStatus;
|
||||
}
|
||||
//==================================================================
|
||||
BOOL CTesaStarE::GetTesaStarEAngle(int& _AngleNumber,double* _Angle)
|
||||
{
|
||||
_AngleNumber=2;
|
||||
_Angle[0]=m_TesaStarEAngle[0];
|
||||
_Angle[1]=m_TesaStarEAngle[1];
|
||||
return TRUE;
|
||||
BOOL rStatus(FALSE);
|
||||
rStatus=ReadTesaStarEAngle();
|
||||
if (rStatus)
|
||||
{
|
||||
_AngleNumber=2;
|
||||
_Angle[0]=m_TesaStarEAngle[0];
|
||||
_Angle[1]=m_TesaStarEAngle[1];
|
||||
}
|
||||
else
|
||||
{
|
||||
_AngleNumber=0;
|
||||
}
|
||||
return rStatus;
|
||||
}
|
||||
//==================================================================
|
||||
BOOL CTesaStarE::ReadTesaStarEAngle()
|
||||
{
|
||||
BOOL rStatus(FALSE);
|
||||
char bSendData[2]={'S',0X0D};
|
||||
int iSendDataLength=2;
|
||||
_Send_Command((const char*)bSendData,iSendDataLength);
|
||||
rStatus=_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
if(rStatus)
|
||||
{
|
||||
bool IsTheEnd(false);
|
||||
do
|
||||
{
|
||||
if(m_RecvDataSize>16)
|
||||
{
|
||||
if (CheckRecvData(4,m_RecvData,1,bSendData,0))
|
||||
{
|
||||
if (CheckRecvData(3,&m_RecvData[m_RecvDataSize-3],0,bSendData,1))
|
||||
{
|
||||
char CData[10]={0};
|
||||
int jj(0);
|
||||
int iADataStartCnt(0);
|
||||
int iBDataStartCnt(0);
|
||||
for (int i=4;i<static_cast<int>(m_RecvDataSize-3);i++)
|
||||
{
|
||||
if (m_RecvData[i]=='H' && m_RecvData[i+1]=='A')
|
||||
{
|
||||
iADataStartCnt=i+2;
|
||||
break;
|
||||
}
|
||||
}
|
||||
for (int i=4;i<static_cast<int>(m_RecvDataSize-3);i++)
|
||||
{
|
||||
if (m_RecvData[i]=='B')
|
||||
{
|
||||
iBDataStartCnt=i+1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
for (int i=iADataStartCnt;i<(iBDataStartCnt-1);i++)
|
||||
{
|
||||
if (jj<10)
|
||||
{
|
||||
CData[jj++]=m_RecvData[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
m_TesaStarEAngle[0]=atof(CData);
|
||||
|
||||
memset(CData,0,sizeof(CData));
|
||||
jj=0;
|
||||
for (int i=iBDataStartCnt;i<static_cast<int>(m_RecvDataSize-3);i++)
|
||||
{
|
||||
if (jj<10)
|
||||
{
|
||||
CData[jj++]=m_RecvData[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
m_TesaStarEAngle[1]=atof(CData);
|
||||
IsTheEnd=true;
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
}
|
||||
if (!rStatus)
|
||||
{
|
||||
IsTheEnd=true;
|
||||
}
|
||||
} while (!IsTheEnd);
|
||||
}
|
||||
if (!rStatus)
|
||||
{
|
||||
CheckStatus();
|
||||
}
|
||||
return rStatus;
|
||||
}
|
||||
//==================================================================
|
||||
BOOL CTesaStarE::CheckStatus()
|
||||
{
|
||||
BOOL rStatus(FALSE);
|
||||
char bSendData[]={0X0D};
|
||||
int iSendDataLength=1;
|
||||
_Send_Command((const char*)bSendData,iSendDataLength);
|
||||
rStatus=_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
if(rStatus)
|
||||
{
|
||||
if (CheckRecvData(3,m_RecvData,0,0,1))
|
||||
{
|
||||
rStatus=TRUE;
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=FALSE;
|
||||
}
|
||||
}
|
||||
return rStatus;
|
||||
}
|
||||
//==================================================================
|
||||
BOOL CTesaStarE::SetPassword()
|
||||
{
|
||||
BOOL rStatus(FALSE);
|
||||
char bSendData[]={'P','2','0','0','4',0X0D};
|
||||
int iSendDataLength=6;
|
||||
_Send_Command((const char*)bSendData,iSendDataLength);
|
||||
rStatus=_Receive_Data(m_RecvData,m_RecvDataSize);
|
||||
if(rStatus)
|
||||
{
|
||||
if (CheckRecvData(4,m_RecvData,0,0,2))
|
||||
{
|
||||
rStatus=TRUE;
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=FALSE;
|
||||
}
|
||||
}
|
||||
return rStatus;
|
||||
}
|
||||
//==================================================================
|
||||
void CTesaStarE::LoadConfig()
|
||||
{
|
||||
}
|
||||
//==================================================================
|
||||
BOOL CTesaStarE::ReadTesaStarEAngle()
|
||||
//===================================================
|
||||
BOOL CTesaStarE::CheckRecvData(const int& _ComparedDataSize,char* _ComparedData,const int& _JudgeDataSize,char* _JudgeVal,int _Mode)
|
||||
{
|
||||
return TRUE;
|
||||
BOOL rStatus(FALSE);
|
||||
UNREFERENCED_PARAMETER(_JudgeDataSize);
|
||||
switch(_Mode)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
if (_ComparedDataSize>=4)
|
||||
{
|
||||
if ((_JudgeVal[0]==_ComparedData[0])&&(_ComparedData[1]==0X0D)&&
|
||||
(_ComparedData[2]==0X0A)&&(_ComparedData[3]==0X0D))
|
||||
{
|
||||
rStatus=TRUE;
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=FALSE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
if (_ComparedDataSize>=3)
|
||||
{
|
||||
if ((_ComparedData[0]==0X0D)&&(_ComparedData[1]==0X0A)&&(_ComparedData[2]==0X0D))
|
||||
{
|
||||
rStatus=TRUE;
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=FALSE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
if (_ComparedDataSize>=4)
|
||||
{
|
||||
if ((_ComparedData[0]=='P')&&(_ComparedData[1]=='*')&&(_ComparedData[2]=='*')&&(_ComparedData[3]=='*'))
|
||||
{
|
||||
rStatus=TRUE;
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=FALSE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
rStatus=FALSE;
|
||||
break;
|
||||
}
|
||||
return rStatus;
|
||||
}
|
||||
//==================================================================
|
||||
BOOL CTesaStarE::CheckStatus()
|
||||
//===================================================
|
||||
BOOL CTesaStarE::_Send_Command(const char* _SendData,DWORD _SendDataLength)
|
||||
{
|
||||
return TRUE;
|
||||
/*
|
||||
BOOL rStatus(FALSE);
|
||||
if (bEnComm)
|
||||
{
|
||||
char sendDataI[12]={'U','P',':','1','1','1','1','1','1','1',13,10};
|
||||
char sendDataII[6]={'S','C',':','1',13,10};
|
||||
char sendDataIII[6]={'R','F',':','1',13,10};
|
||||
CString csRightRecv;
|
||||
csRightRecv.Format(_T("@OK\r\n"));
|
||||
CStringA m_csRecv("");
|
||||
INT iRetrys(0);
|
||||
DWORD iWriteByte(0);
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
|
||||
//Phase I
|
||||
iWriteByte=m_pSerial->Send(sendDataI,12);
|
||||
while(!m_pSerial->m_iRecvState && iRetrys<20 && iWriteByte==12)
|
||||
DWORD iWriteByte(0);
|
||||
iWriteByte=m_pSerial->Send(_SendData,_SendDataLength);
|
||||
}
|
||||
return rStatus;
|
||||
};
|
||||
//===================================================
|
||||
BOOL CTesaStarE::_Receive_Data(char* _SendData,DWORD& _RecvDataLength)
|
||||
{
|
||||
BOOL rStatus(FALSE);
|
||||
if (bEnComm)
|
||||
{
|
||||
iRetrys++;
|
||||
Sleep(50);
|
||||
INT iRetrys(0);
|
||||
CString csTemp;
|
||||
while(!m_pSerial->m_iRecvState && iRetrys<20)
|
||||
{
|
||||
iRetrys++;
|
||||
Sleep(100);
|
||||
}
|
||||
if (m_pSerial->m_iRecvState)
|
||||
{
|
||||
if(m_pSerial->m_RecvData[0]==0)
|
||||
{
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
INT iRetrys2(0);
|
||||
while(!m_pSerial->m_iRecvState && iRetrys2<60)
|
||||
{
|
||||
iRetrys2++;
|
||||
Sleep(100);
|
||||
}
|
||||
}
|
||||
memcpy(_SendData,m_pSerial->m_RecvData,m_pSerial->m_iRecvBytes);
|
||||
_RecvDataLength=m_pSerial->m_iRecvBytes;
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
rStatus=TRUE;
|
||||
}
|
||||
else
|
||||
{
|
||||
rStatus=FALSE;
|
||||
}
|
||||
}
|
||||
|
||||
if (m_pSerial->m_iRecvState)
|
||||
{
|
||||
if(m_pSerial->m_RecvData[0]==0)
|
||||
{
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
iRetrys=0;
|
||||
while(!m_pSerial->m_iRecvState && iRetrys<30)
|
||||
{
|
||||
iRetrys++;
|
||||
Sleep(50);
|
||||
}
|
||||
}
|
||||
|
||||
m_csRecv.Format(("%s"),m_pSerial->m_RecvData);
|
||||
if((CString)m_csRecv==csRightRecv)
|
||||
{
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
iRetrys=0;
|
||||
iWriteByte=0;
|
||||
//Phase II
|
||||
|
||||
iWriteByte=m_pSerial->Send(sendDataII,6);
|
||||
while(!m_pSerial->m_iRecvState && iRetrys<20 && iWriteByte==6)
|
||||
{
|
||||
iRetrys++;
|
||||
Sleep(50);
|
||||
}
|
||||
if(m_pSerial->m_iRecvState)
|
||||
{
|
||||
if(m_pSerial->m_RecvData[0]==0)
|
||||
{
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
iRetrys=0;
|
||||
while(!m_pSerial->m_iRecvState && iRetrys<30)
|
||||
{
|
||||
iRetrys++;
|
||||
Sleep(50);
|
||||
}
|
||||
}
|
||||
|
||||
m_csRecv.Format(("%s"),m_pSerial->m_RecvData);
|
||||
if((CString)m_csRecv==csRightRecv)
|
||||
{
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
iRetrys=0;
|
||||
iWriteByte=0;
|
||||
//Phase III
|
||||
|
||||
iWriteByte=m_pSerial->Send(sendDataIII,6);
|
||||
while(!m_pSerial->m_iRecvState && iRetrys<20 && iWriteByte==6)
|
||||
{
|
||||
iRetrys++;
|
||||
Sleep(50);
|
||||
}
|
||||
if(m_pSerial->m_iRecvState)
|
||||
{
|
||||
if(m_pSerial->m_RecvData[0]==0)
|
||||
{
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
iRetrys=0;
|
||||
while(!m_pSerial->m_iRecvState && iRetrys<30)
|
||||
{
|
||||
iRetrys++;
|
||||
Sleep(50);
|
||||
}
|
||||
}
|
||||
|
||||
m_csRecv.Format(("%s"),m_pSerial->m_RecvData);
|
||||
if((CString)m_csRecv==csRightRecv)
|
||||
{
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
iRetrys=0;
|
||||
iWriteByte=0;
|
||||
return TRUE;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
return FALSE;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return FALSE;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
return FALSE;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
m_pSerial->m_iRecvState=FALSE;
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
return FALSE;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return FALSE;
|
||||
}
|
||||
*/
|
||||
}
|
||||
return rStatus;
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user