bool UHokuyo::requestMoreData()
{
const int MSL = 30;
char s[MSL];
// make data request string
makeGetDataString(s, MSL);
//printf("mode=%d, res=%g, sending:%s", modeAngleScan, angleResolution, s);
// request data
return sendToDevice(s, 11);
}
void ULaserDevice::threadRunLoop()
{
int STAT_INTERVAL = 1000;
//struct timeval t1, t2;
double dt;
bool gotData;
UTime t;
//
threadRunning = true;
loopCnt = 0;
t.Now();
while (not threadStop)
{
// send new data to device
if (sendNewData)
{ // debug
printf("ULaserDevice::threadRunLoop sending to device '%s'\n", sendStr);
// debug end
sendToDevice(sendStr, sendStrCnt);
sendNewData = false;
}
// receive data from device
// may be measurements or not
gotData = receiveData();
// do other things as appropriate
if (not isPortOpen())
Wait(0.1);
if (not gotData)
Wait(0.001); // changed to 1 ms
if(int(statGoodCnt + statBadCnt) == STAT_INTERVAL )
{
dt = t.getTimePassed();
t.Now();
if (verbose)
{
printf("Got %d Good and %d Bad packets (total %d) %3.1f%% good\n",
statGoodCnt, statBadCnt, statGoodCnt + statBadCnt,
(float)(statGoodCnt) /
(float)(statGoodCnt + statBadCnt) * 100.0);
printf("in %7.3f seconds or %6.2f scan/sec\n",
dt, (float)(statGoodCnt)/dt);
}
//
statMsgRate = double(statGoodCnt)/dt;
statGoodCnt = 0;
statBadCnt = 0;
}
loopCnt++;
}
threadRunning = false;
}
const char * UHokuyo::getNameFromDevice()
{
int i;
//
if (isPortOpen())
{
closePort();
}
// open portand ensure that receive thread is running
start();
if (isPortOpen())
{ // wait for port to open ...
lock();
Wait(0.1);
printf("Name request [V\\n]...\n");
// request forst message - vill probably be lost
sendToDevice("V\n", 2);
printf("Waiting for reply ...\n");
Wait(0.1);
// request again
sendToDevice("V\n", 2);
// wait for reply
i = 0;
unlock();
while (strlen(getName()) < 10)
{ // wait for reply
Wait(0.1);
i++;
if (i > 20)
break;
}
printf("Finished after <= %g secs\n", 0.1 * i);
closePort();
}
return getName();
}
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;
}
bool UHokuyo::getNewestData(ULaserData * dest,
unsigned long lastSerial,
int fake)
{
bool result = false;
UTime t;
bool isOK;
//
result = (dest != NULL);
if (result)
{
if (fake > 0)
getFakeScan( dest, lastSerial, fake);
else
{ // live data
if (not isPortOpen())
{ // open port (in get scan mode)
// and start read thread
start();
if (isPortOpen())
{ // wait for port to open
printf("Opening port for scan ...\n");
Wait(0.1);
lock();
sendToDevice("\nV\n", 3);
//isOK = sendToDevice("\nG04572501\n", 11);
unlock();
//printf("Waiting for reply 1 to [G38438501\\n] ...\n");
Wait(0.1);
lock();
isOK = sendToDevice("\nG04572501\n", 11);
unlock();
//printf("Waiting for reply 2 to [G38438501\\n] ...\n");
Wait(0.3);
if (isOK)
printf("Opening port for scan ... is OK\n");
badSeries = 0;
versionInfoCnt = 0;
}
}
if (isPortOpen())
{ // get a full scan of data
t = dataRxTime;
}
result = (lasData != NULL) and (dest != NULL);
if (result)
{
lasData->lock();
//printf("Last serial = %lu, got data fromserial %lu, valid=%s\n",
// lastSerial, lasData->getSerial(), bool2str(lasData->isValid()));
result = lasData->isValid() and // and ((lasData->getScanTime() - t) > 0.001);
(lasData->getSerial() > lastSerial);
if (result)
{
dest->copy(lasData);
// save to logfile (if open)
if (datalogUsedScans)
logThisScan(lasData);
}
else
dest->setValid(false);
lasData->unlock();
}
else if (dest != NULL)
dest->setValid(false);
}
if (result)
dest->setDeviceNum( deviceNum);
}
return result;
}
bool RFduinoGZLLClass::sendToDevice(device_t device, String &data)
{
char buf[32];
data.toCharArray(buf, sizeof(buf));
return sendToDevice(device, buf);
}
bool RFduinoGZLLClass::sendToDevice(device_t device, const char *data)
{
return sendToDevice(device, data, strlen(data));
}
