void ByteArray::append(const char *str)
{
int len = str ? (int)strlen(str) : 0;
if (len) {
if (m_stringLength)
{
requestMoreData(m_stringLength + len);
memcpy(m_data + m_stringLength, str, len);
m_stringLength += len;
m_data[m_stringLength] = 0;
}
else
{
setString(str);
}
}
}
void ByteArray::append(const char* str, int index, int count)
{
if (count)
{
if (m_stringLength)
{
requestMoreData(m_stringLength + count);
}
else
{
requestNewData(count);
}
memcpy(m_data + m_stringLength, str + index, count);
m_stringLength += count;
m_data[m_stringLength] = 0;
}
}
bool UHokuyo::receiveData()
{
bool gotData = false;
int n, i = 0;
char * dend = NULL;
UTime t;
bool gotRngData = false;
const int MSL = 500;
char s[MSL];
//
if (modeSimulated)
{
//gotData = receiveSimulatedData(&length);
Wait(0.1);
}
else
{
if (dataCnt > 0)
{ // test for old data
if (dataTime.getTimePassed() > 1.0)
{
printf("Discarded '%s'\n", dataBuf);
for (n = 0; n < dataCnt; n++)
printf("%02x,", dataBuf[n]);
printf("\n");
printf("Discarded %d bytes of old data\n", dataCnt);
statBadCnt++;
dataCnt = 0;
}
}
lock();
if (isPortOpen())
{
// set timeout to ~30 ms
n = receiveFromDevice(&dataBuf[dataCnt], MAX_DATA_LNG - dataCnt - 1, 0.015);
if (/*repeatGetScan and*/ (n > 0))
{ // request more data
requestMoreData();
}
//if (n > 0)
// printf("(%d bytes)", n);
}
else
n = 0;
unlock();
if (n > 0)
{
dataCnt += n;
dataBuf[dataCnt] = '\0';
dend = strstr(dataBuf, "\n\n");
// debug
/* if (verbose and (datalog != NULL))
{ // log the received part of the buffer
dataBuf[n] = '\0';
fprintf(datalog, "%4d (n=%3d) got:'%s'\n", dataCnt, n, dataBuf);
}*/
// debug end
gotData = (dend != NULL);
}
}
//
while (gotData)
{ // message length
badSeries++;
n = dend - dataBuf;
// debug
if (n > dataCnt)
{
printf("UHokuyo::receiveData message end found after buffer end? n=%d dataCnt=%d\n", n, dataCnt);
}
if (verbose)
{
snprintf(s, MSL, "In %d 'gotData' dataCnt=%d n=%d %c%c%c%c%c%c%c%c%c... after %g sec\n", i, dataCnt, n,
dataBuf[0], dataBuf[1], dataBuf[2],dataBuf[3], dataBuf[4], dataBuf[5], dataBuf[6], dataBuf[7], dataBuf[8],
dataTime.getTimePassed());
laslog.toLog("urg:", s);
}
//printf("msg:%s", dataBuf);
i++;
// debug end
switch (dataBuf[0])
{
case 'V':
dend[1] = '\0';
if (dataBuf[1] == '\n')
{ // some garbage may also start with a V
if (versionInfoCnt < 5 or verbose)
printf("Got version (%d) info ((grp %d) %d chars):'%s'\n", versionInfoCnt, i, dataCnt, dataBuf);
decodeName(dataBuf);
versionInfoCnt++;
if (verbose)
laslog.toLog(" - V:", &dataBuf[4]);
}
break;
case 'G':
if (dataBuf[10] != '0')
{ // must be '0 to be valid scandata
// printf("Bad data set laser may be off (bdSeries=%d) - send an on-command (\\nL1\\n)\n", badSeries);
sendToDevice("\nL1\n", 4);
break;
}
badSeries = 0;
if (lasData == NULL)
lasData = new ULaserData();
if (lasData != NULL)
{ // save data into decoded buffer
lasData->lock();
lasData->setDeviceNum(deviceNum);
gotRngData = decodeData(dataBuf, n, lasData);
lasData->setMaxValidRange(maxValidRange);
//if (repeatGetScan and not gotRngData)
//{ // error message - request new data
// // send request for more data right away
// lock();
// //sendToDevice("00038401\n", 9);
// //sendToDevice("00076801\n", 9);
// unlock();
//}
if (gotRngData)
statGoodCnt++;
lasData->setMirror(mirrorData);
lasData->unlock();
//
if (gotRngData)
// do pending push commands.
// No need to set data as locked, as
// a client scanGet may use data at the same time as a scanpush,
// as none of these will modify the 'lasData' structure.
gotNewScan(lasData);
if (verbose)
{ // debug logging
snprintf(s, MSL, "UHokuyo::receiveData: scan=%6s %drngs In %2d msgCnt=%4d/%4d : %c%c%c%c%c%c%c%c%c %c %c%c... after %g sec\n",
bool2str(gotRngData), lasData->getRangeCnt(), i, n, dataCnt,
dataBuf[0], dataBuf[1], dataBuf[2],dataBuf[3], dataBuf[4], dataBuf[5], dataBuf[6], dataBuf[7], dataBuf[8],
dataBuf[10], dataBuf[12], dataBuf[13],
dataTime.getTimePassed());
laslog.toLog(" - G:", s);
// to screen also
printf("%s", s);
}
dataTime.Now();
}
break;
default:
// got error
// discard to first newline
/* char * nl = strchr(dataBuf, '\n');
if (nl < dend and nl > dataBuf)
{ // newline found,
// set new data end (dend) to one earlier, as if it was
// a real "\n\n" command termination.
// this is to avoid a situation where OK scans
// are part of an error-respond.
dend = --nl;
}*/
// close string and restart
dend[1] = '\0';
// debug
if (verbose)
{
snprintf(s, MSL, "UHokuyo::receiveData: garbage msg %4d of %4d chars in buffer:'%c%c%c...'\n", n, dataCnt, dataBuf[0], dataBuf[1], dataBuf[2]);
laslog.toLog(" - ", s);
}
//printf("UHokuyo::receiveData: ignored garbage (%d chars):'%s'\n", dataCnt, dataBuf);
//rp = true; //repeatGetScan;
// no other than G???... should be
// received in repeat mode, well
// sometimes a LFLF is in the error message
//repeatGetScan = false;
/*closePort();
printf("Got unknown garbage:--------Closing port\n");
Wait(0.2);
if (rp)
{ // repeat is enabled - request dummy data
printf("-------------restarting in repeat mode\n");
getNewestData(NULL, 0, false);
//repeatGetScan = true;
}*/
statBadCnt++;
break;
}
// discard the used (or unknown) data
n = dataCnt - (dend - dataBuf) - 2;
memmove(dataBuf, dend + 2, n);
// reduce available data count
dataCnt = n;
dataBuf[dataCnt] = '\0';
// test for more data in buffer
gotData = dataCnt > 3;
if (gotData)
{ // is it a full message
dend = strstr(dataBuf, "\n\n");
gotData = (dend != NULL);
//printf("****Got more messages in one read (OK, but indicate latency)\n");
}
}
return gotRngData;
}
