void CSMTPClient::ProcessERRORCommand()
{
m_nStatus = SMTP_ERROR_CMD;
CString returnMsg(_T("502 Unknown command\r\n"));
m_parrent->WriteLog(returnMsg);
Reply(returnMsg);
m_nStatus = SMTP_WAITING_CMD;
}
int headSetup( void )
{
int headFlag = 0;
ROS_INFO("Sonar Head Setup.\n");
sleep(1);
//Read Port
sortPacket();
while( headFlag != 2)
{
if( returnMsg() == mtAlive && headFlag == 0)
{
ROS_INFO("headSetup \t<< mtAlive!\n");
// Range, Left Angle, Right Angle, ADSpam, ADLow, Gain, ADInterval, Number of Bins.
makeHeadPacket( RANGE,
LEFTANGLE,
RIGHTANGLE,
ADSPAN,
ADLOW,
GAIN,
ADINTERVAL,
NUMBEROFBINS);
usleep(100);
ROS_INFO("headSetup \t>> mtHeadCommand\n");
headFlag = 1;
}
else if( returnMsg() == mtAlive && headFlag == 1)
{
ROS_INFO("headSetup \t<< mtAlive!\n");
headFlag = 2;
}
else
{
headFlag = 0;
}
}
return 0;
}
/**********************************************
* Initilise sonar,
* check alive, send version request, check for
* reply
* returns 1 if set up, -1 if failed.
* *******************************************/
int initSonar( void )
{
int initFlag = 0, initAlive = 0;
ROS_INFO("Sonar Initilization\n");
// Check port for mtAlive
while( initFlag != 1 )
{
//Read Port
sortPacket();
//Check for mtAlive command
if( returnMsg() == mtAlive )
{
ROS_INFO("initSonar \t<< mtAlive!\n");
initAlive = 1;
//Send over the mtSendVersion
makePacket(mtSendVersion);
ROS_INFO("initSonar \t>> mtSendVersion\n");
}
// Did we get back mtVersionData and is the alive flag set?
else if( returnMsg() == mtVersionData && initAlive == 1)
{
ROS_INFO("initSonar \t<< mtVersionData!\n");
initFlag = 1;
}
else if( initAlive == 1 )
{
makePacket(mtSendVersion);
ROS_INFO("initSonar \t>> mtSendVersion\n");
}
else
{
initFlag = 0;
}
}
return 0;
}
int requestData( void )
{
int recieveFlag = 0, sendFlag = 0, headPack = 0;
while(recieveFlag != 1)
{
if(sendFlag == 0)
{
//Make the sendData packet and send
makePacket(mtSendData);
ROS_INFO("requestData \t>> mtSendData\n");
sendFlag = 1;
}
//usleep(500);
while( recieveFlag != 1)
{
sortPacket();
//if you get some data back go forth
if(returnMsg() == mtHeadData)
{
ROS_INFO("requestData \t<< mtHeadData!!\n");
headPack = 1;
}
else if(returnMsg() == mtAlive && headPack == 1)
{
recieveFlag = 1;
}
else
{
//Make the sendData packet and send
makePacket(mtSendData);
ROS_INFO("requestData \t>> mtSendData\n");
sendFlag = 1;
}
}
}
return 0;
}
int sendBB( void )
{
int bbFlag = 0;
while( bbFlag != 1 )
{
if( returnMsg() == mtBBUserData )
{
ROS_INFO("sendBB \t<< mtBBUserData!\n");
bbFlag = 1;
}
else
{
makePacket(mtSendBBUser);
ROS_INFO("sendBB \t>> mtSendBBUser\n");
}
}
return 0;
}
int requestData( void )
{
int recieveFlag = 0, sendFlag = 0, headPack = 0;
int count;
while(recieveFlag != 1)
{
if(sendFlag == 0)
{
//Make the sendData packet and send
// tcflush(fd, TCIFLUSH);//remove
// usleep(50);//remove
// tcflush(fd, TCIFLUSH);//remove
// usleep(50);//remove
makePacket(mtSendData);
// ROS_INFO("requestData \t>> mtSendData\n");
//tcflush(fd, TCIFLUSH);//remove
//usleep(50);//remove
sendFlag = 1;
}
//usleep(500);
count = 0;
while( recieveFlag != 1)
{
sortPacket();
//if you get some data back go forth
if(returnMsg() == mtHeadData)
{
// ROS_INFO("requestData \t<< mtHeadData!!\n");
headPack = 1;
return 0;
}
else if(returnMsg() == mtAlive && headPack == 1)
{
recieveFlag = 1;
//tcflush(fd, TCIFLUSH);//remove
}
else
{
if(count > 10)
{
//Make the sendData packet and send
// tcflush(fd, TCIFLUSH);//remove
// usleep(500);//remove
makePacket(mtSendData);
ROS_INFO("requestData \t>> mtSendData\n");
sendFlag = 1;
count = 0;
}
count ++;
//printf("%d\n", count);
}
}
}
return 0;
}