bool CMOOSSerialPort::GetTelegramOrAccumulate(std::string &sTelegram,double dfTimeOut,double *pTime)
{
if(IsStreaming())
{
MOOSTrace("don't call GetTelegram on a streaming device!\n");
return false;
}
static char telegramBuffer[TELEGRAM_LEN];
static int nTelegramBufferRead = 0; //total number of chars read
double dfTimeWaited = 0.0; //haven't waited any time yet
double dfInterval = 0.01; //10ms
while ((dfTimeWaited<dfTimeOut) && nTelegramBufferRead<TELEGRAM_LEN)
{
int nGrabbed = 0;
//try the read
nGrabbed = GrabN(telegramBuffer+nTelegramBufferRead,1);
if (nGrabbed == 0)
{
//OK wait a while...maybe it is on its way!
dfTimeWaited+=dfInterval;
MOOSPause((int)(dfInterval*1000.0));
}
else
{
if(nTelegramBufferRead==0 && pTime!=NULL)
{
//grab the time..
*pTime = MOOSTime();
}
nTelegramBufferRead+=nGrabbed;
//have we reached the end of the message?
if(IsCompleteReply(telegramBuffer,TELEGRAM_LEN,nTelegramBufferRead))
{
telegramBuffer[nTelegramBufferRead]='\0';
nTelegramBufferRead = 0;
sTelegram = telegramBuffer;
MOOSRemoveChars(sTelegram,"\r\n");
if(IsVerbose())
{
MOOSTrace("Telegram = %s\n",sTelegram.c_str());
}
//MOOSTrace("Required %d retries and %d accumulates\n",nRetries,nAccumulates);
return true;
}
}
}
return false;
}
bool CMOOSNavEngine::SetUpSensorChannels(STRING_LIST sParams,string sToken)
{
//ok we need to load up rejection settings
STRING_LIST::iterator p;
for(p = sParams.begin();p!=sParams.end();p++)
{
string sLine = *p;
if(sLine.find(sToken)!=string::npos)
{
MOOSRemoveChars(sLine," \t");
MOOSChomp(sLine,"=");
//LSQ_REJECTION = TheAvtrak : Reject = 3, History = 5,Fail = 0.001
CMOOSSensorChannel NewChannel;
string sSensor = MOOSChomp(sLine,":");
string sHistory = MOOSChomp(sLine,",");
string sFail = MOOSChomp(sLine,",");
MOOSChomp(sHistory,"=");
MOOSChomp(sFail,"=");
if(sFail.empty() ||sHistory.empty())
{
MOOSTrace("error in %s line!\n",sToken.c_str());
return false;
}
int nDepth = atoi(sHistory.c_str());
if(nDepth>0)
{
NewChannel.SetHistoryDepth(nDepth);
}
double dfFail = atof(sFail.c_str());
if(dfFail>0)
{
NewChannel.SetNoiseLimit(dfFail);
}
NewChannel.SetName(sSensor);
m_SensorChannelMap[sSensor]=NewChannel;
}
}
return true;
}
bool CThirdPartyTask::OnNewInstruction(string sInstruction,double dfTimeNow)
{
MOOSToUpper(sInstruction);
MOOSRemoveChars(sInstruction," \t");
if(sInstruction.find("ACTUATION:")!=string::npos)
{
return OnActuationInstruction(sInstruction,dfTimeNow);
}
else if(sInstruction.find("STATUS:")!=string::npos)
{
//eg "STATUS:ENABLE=TRUE"
return OnStatusInstruction(sInstruction,dfTimeNow);
}
else
{
MOOSTrace("Unknown thirdparty command! : %s",sInstruction.c_str());
return false;
}
}
bool CProcessConfigReader::GetConfiguration(std::string sAppName, STRING_LIST &Params)
{
int nBrackets = 0;
Params.clear();
Reset();
std::string sKey = "PROCESSCONFIG="+sAppName;
if(GoTo(sKey))
{
std::string sBracket = GetNextValidLine();
if(sBracket.find("{")==0)
{
nBrackets++;
while(!GetFile()->eof())
{
std::string sLine = GetNextValidLine();
MOOSRemoveChars(sLine," \t\r");
if(sLine.find("}")!=0)
{
#if(1)
// jckerken 8-12-2004
// ignore if param = <empty string>
std::string sTmp(sLine);
std::string sTok = MOOSChomp(sTmp, "=");
MOOSTrimWhiteSpace(sTok); // Handle potential whitespaces.
MOOSTrimWhiteSpace(sTmp);
if (sTok.size() > 0)
{
MOOSTrimWhiteSpace(sTmp);
if (!sTmp.empty())
{
Params.push_front(sTok+std::string("=")+sTmp); // Was: sLine
}
else if(sLine.find("[")!=std::string::npos || sLine.find("]")!=std::string::npos)
{
Params.push_front(sTok+std::string("=")+sTmp); // Was: sLine
}
}
else
{
Params.push_front(sTok+std::string("=")+sTmp); // Was: sLine
}
#else
Params.push_front(sLine);
#endif
}
else
{
return true;
}
//quick error check - we don't allow nested { on single lines
if(sLine.find("{")==0)
{
MOOSTrace("CProcessConfigReader::GetConfiguration() missing \"}\" syntax error in mission file\n");
}
}
}
}
return false;
}
bool CMOOSSerialPort::CommsLoop()
{
char pTmp[DEFAULT_COMMS_SPACE];
char pAccumulator[2*DEFAULT_COMMS_SPACE];
int nInStore = 0;
double dfTimeOut = 0.1;
while(!m_bQuit)
{
int nRead = ReadNWithTimeOut(pTmp,DEFAULT_COMMS_SPACE,dfTimeOut);
if(nRead!=-1)
{
if((nInStore+nRead)>int(sizeof(pAccumulator)))
{
//oops...
MOOSTrace("Comms Loop Accumulator Overflow, reseting\n");
nInStore = 0;
continue;
}
//append to the accumulator
memcpy(pAccumulator+nInStore,pTmp,nRead);
//we have more in the cupboard!
nInStore+=nRead;
//lock
m_InBoxLock.Lock();
char * pTelegramEnd = NULL;
do
{
//A device will by default use the termination character <CR>
//But we now allow for setting of a termination character
//to allow for flexibility in a reply.
pTelegramEnd = (char*)memchr(pAccumulator,GetTermCharacter(),nInStore);
if(pTelegramEnd!=NULL)
{
//how long is the telegram?
int nTelegramLength = pTelegramEnd - pAccumulator + 1;
//make a buffer that is one larger to preserve the last character
//in case we are interested in it.
char bBiggerBuff[2*DEFAULT_COMMS_SPACE+1];
//copy the telegram to a buffer size that is 1 larger
memcpy(bBiggerBuff, pAccumulator, nTelegramLength);
//terminate it as a string
//*pTelegramEnd = '\0';
bBiggerBuff[nTelegramLength] = '\0';
//copy it
//std::string sTelegram((char*)pAccumulator);
std::string sTelegram((char*)bBiggerBuff);
MOOSRemoveChars(sTelegram,"\r\n");
if(IsVerbose())
{
MOOSTrace("Telegram = %s\n",sTelegram.c_str());
}
//shuffle accumulator
memmove(pAccumulator,
pAccumulator+nTelegramLength,
sizeof(pAccumulator)-nTelegramLength);
//we have less in our store now!
nInStore-=nTelegramLength;
//make a telegram
CMOOSSerialTelegram Tg(sTelegram,MOOSTime());
//stuff it
m_InBox.push_front(Tg);
}
}while(pTelegramEnd !=NULL);
//whatch we don't exhibit disgraceful behaviour!
while(m_InBox.size()>MOOS_SERIAL_INBOX_MAX_SIZE)
{
m_InBox.pop_back();
}
//and unlock
m_InBoxLock.UnLock();
}
else
{
}
}
return true;
}
