TAO Cheng
457 lines
12 KiB
C++
457 lines
12 KiB
C++
#include "StdAfx.h"
|
|
#include "TesaStarE.h"
|
|
|
|
const int MAX_RETRY_CNTS=5;
|
|
//==================================================================
|
|
CTesaStarE::CTesaStarE(void)
|
|
{
|
|
SerialComPort=1;
|
|
_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)
|
|
{
|
|
}
|
|
//==================================================================
|
|
BOOL CTesaStarE::Initialization(bool _bInitConfig)
|
|
{
|
|
BOOL status(FALSE);
|
|
if (_bInitConfig)
|
|
{
|
|
LoadConfig();
|
|
}
|
|
m_pSerial.SetPort(SerialComPort,TESASTARE_BAUD,TESASTARE_PARITY,TESASTARE_BITS,TESASTARE_STOPBITS,TESASTARE_HandShake);
|
|
if(m_pSerial.Open())
|
|
{
|
|
bEnComm=TRUE;
|
|
status=CheckStatus();
|
|
status=SetPassword();
|
|
}
|
|
else
|
|
{
|
|
status=FALSE;
|
|
bEnComm=FALSE;
|
|
}
|
|
return status;
|
|
}
|
|
//==================================================================
|
|
BOOL CTesaStarE::Unload()
|
|
{
|
|
m_pSerial.Close();
|
|
Sleep(500);
|
|
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)
|
|
{
|
|
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::CheckRecvData(const int& _ComparedDataSize,char* _ComparedData,const int& _JudgeDataSize,char* _JudgeVal,int _Mode)
|
|
{
|
|
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::_Send_Command(const char* _SendData,DWORD _SendDataLength)
|
|
{
|
|
BOOL rStatus(FALSE);
|
|
if (bEnComm)
|
|
{
|
|
DWORD iWriteByte(0);
|
|
iWriteByte=m_pSerial.Send(_SendData,_SendDataLength);
|
|
}
|
|
return rStatus;
|
|
};
|
|
//===================================================
|
|
BOOL CTesaStarE::_Receive_Data(char* _SendData,DWORD& _RecvDataLength)
|
|
{
|
|
BOOL rStatus(FALSE);
|
|
if (bEnComm)
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
return rStatus;
|
|
};
|