AREXPORT void ArLaser::laserConnect(void) { // figure out how many readings we can have and set the current // buffer size to that double degrees; myLastReading.setToNow(); if (canSetDegrees()) { //degrees = fabs(ArMath::subAngle(getStartDegrees(), getEndDegrees())); degrees = fabs(getStartDegrees() - getEndDegrees()); ArLog::log(myInfoLogLevel, "%s: Using degrees settings of %g to %g for %g degrees", getName(), getStartDegrees(), getEndDegrees(), degrees); } else if (canChooseDegrees()) { degrees = getDegreesChoiceDouble(); ArLog::log(myInfoLogLevel, "%s: Using choice of %g degrees", getName(), degrees); } else { degrees = 360; ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 360", getName()); } double increment; if (canSetIncrement()) { increment = getIncrement(); ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees", getName(), increment); } else if (canChooseIncrement()) { increment = getIncrementChoiceDouble(); ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees", getName(), increment); } else { // PS 10/20/11 - This was missing causing buffer size to be very large // set this to the lowest, note both the SZ and S3 are setting the buffer // size but it's being overriden by this procedure - do we want to fix // this or just leave it at the max value 360/.25=1440??? increment = .25; ArLog::log(ArLog::Terse, "%s: Don't have any settings for increment, arbitrarily using .25", getName()); } int size = (int)ceil(degrees / increment) + 1; ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d", getName(), size); setCurrentBufferSize(size); ArLog::log(myInfoLogLevel, "%s: Connected", getName()); myConnectCBList.invoke(); }
AREXPORT void ArLaser::laserConnect(void) { // figure out how many readings we can have and set the current // buffer size to that double degrees; myLastReading.setToNow(); if (canSetDegrees()) { degrees = fabs(getStartDegrees() - getEndDegrees()); ArLog::log(myInfoLogLevel, "%s: Using degrees settings of %g to %g for %g degrees", getName(), getStartDegrees(), getEndDegrees(), degrees); } else if (canChooseDegrees()) { degrees = getDegreesChoiceDouble(); ArLog::log(myInfoLogLevel, "%s: Using choice of %g degrees", getName(), degrees); } else { degrees = 360; ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 360", getName()); } double increment; if (canSetIncrement()) { increment = getIncrement(); ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees", getName(), increment); } else if (canChooseIncrement()) { increment = getIncrementChoiceDouble(); ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees", getName(), increment); } else { ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 1000 for current buffer size", getName()); } int size = (int)ceil(degrees / increment) + 1; ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d", getName(), size); setCurrentBufferSize(size); ArLog::log(myInfoLogLevel, "%s: Connected", getName()); myConnectCBList.invoke(); }
AREXPORT bool ArLMS1XX::blockingConnect(void) { char buf[1024]; if (!getRunning()) runAsync(); myConnMutex.lock(); if (myConn == NULL) { ArLog::log(ArLog::Terse, "%s: Could not connect because there is no connection defined", getName()); myConnMutex.unlock(); failedToConnect(); return false; } if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN && !myConn->openSimple()) { ArLog::log(ArLog::Terse, "%s: Could not connect because the connection was not open and could not open it", getName()); myConnMutex.unlock(); failedToConnect(); return false; } myReceiver.setDeviceConnection(myConn); myConnMutex.unlock(); lockDevice(); myTryingToConnect = true; unlockDevice(); laserPullUnsetParamsFromRobot(); laserCheckParams(); int size = (270 / .25 + 1); ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d", getName(), size); setCurrentBufferSize(size); ArTime timeDone; if (myPowerControlled) timeDone.addMSec(60 * 1000); else timeDone.addMSec(30 * 1000); ArLMS1XXPacket *packet; ArLMS1XXPacket sendPacket; sendPacket.empty(); sendPacket.strToBuf("sMN"); sendPacket.strToBuf("SetAccessMode"); sendPacket.uByteToBuf(0x3); // level sendPacket.strToBuf("F4724744"); // hashed password sendPacket.finalizePacket(); if ((packet = sendAndRecv(timeDone, &sendPacket, "SetAccessMode")) != NULL) { int val; val = packet->bufToUByte(); delete packet; packet = NULL; if (val == 1) { ArLog::log(myLogLevel, "%s: Changed access mode (%d)", getName(), val); } else { ArLog::log(ArLog::Terse, "%s: Could not change access mode (%d)", getName(), val); failedToConnect(); return false; } } else { failedToConnect(); return false; } sendPacket.empty(); sendPacket.strToBuf("sMN"); sendPacket.strToBuf("mLMPsetscancfg"); sendPacket.byte4ToBuf(5000); // scanning freq sendPacket.byte2ToBuf(1); // number segments sendPacket.byte4ToBuf(getIncrementChoiceDouble() * 10000); // angle resolution //sendPacket.byte4ToBuf((getStartDegrees() + 90) * 10000); // starting angle sendPacket.byte4ToBuf(-45 * 10000); // can't change starting angle //sendPacket.byte4ToBuf((getEndDegrees() + 90) * 10000); // ending angle sendPacket.byte4ToBuf(225 * 10000); // can't change ending angle sendPacket.finalizePacket(); ArLog::log(myLogLevel, "%s: setscancfg: %s", getName(), sendPacket.getBuf()); if ((packet = sendAndRecv(timeDone, &sendPacket, "mLMPsetscancfg")) != NULL) { int val; val = packet->bufToUByte(); delete packet; packet = NULL; if (val == 0) { ArLog::log(myLogLevel, "%s: setscancfg succeeded (%d)", getName(), val); } else { ArLog::log(ArLog::Terse, "%s: Setscancfg failed (%d)", getName(), val); failedToConnect(); return false; } } else { failedToConnect(); return false; } sendPacket.empty(); sendPacket.strToBuf("sWN"); sendPacket.strToBuf("LMDscandatacfg"); sendPacket.uByte2ToBuf(0x1); // output channel sendPacket.uByteToBuf(0x0); // remission sendPacket.uByteToBuf(0x0); // remission resolution sendPacket.uByteToBuf(0x0); // unit sendPacket.uByte2ToBuf(0x0); // encoder sendPacket.uByteToBuf(0x0); // position sendPacket.uByteToBuf(0x0); // device name sendPacket.uByteToBuf(0x0); // comment sendPacket.uByteToBuf(0x0); // time sendPacket.byteToBuf(5); // which scan //sendPacket.byteToBuf(1); // which scan sendPacket.finalizePacket(); ArLog::log(myLogLevel, "%s: scandatacfg: %s", getName(), sendPacket.getBuf()); if ((packet = sendAndRecv(timeDone, &sendPacket, "LMDscandatacfg")) != NULL) { ArLog::log(myLogLevel, "%s: scandatacfg succeeded", getName()); delete packet; packet = NULL; } else { failedToConnect(); return false; } sendPacket.empty(); sendPacket.strToBuf("sMN"); sendPacket.strToBuf("Run"); sendPacket.finalizePacket(); if ((packet = sendAndRecv(timeDone, &sendPacket, "Run")) != NULL) { int val; val = packet->bufToUByte(); delete packet; packet = NULL; if (val == 1) { ArLog::log(myLogLevel, "%s: Run succeeded (%d)", getName(), val); } else { ArLog::log(ArLog::Terse, "%s: Could not run (%d)", getName(), val); failedToConnect(); return false; } } else { failedToConnect(); return false; } /* when asking one at a time sendPacket.empty(); sendPacket.strToBuf("sRN"); sendPacket.strToBuf("LMDscandata"); sendPacket.finalizePacket(); if ((packet = sendAndRecv(timeDone, &sendPacket, "LMDscandata")) != NULL) { ArLog::log(myLogLevel, "%s: Got %s scan data %d", getName(), packet->getCommandType(), packet->getLength()); myPacketsMutex.lock(); myPackets.push_back(packet); myPacketsMutex.unlock(); sensorInterp(); ArLog::log(myLogLevel, "%s: Processed scan data", getName()); } else { failedToConnect(); return false; } */ sendPacket.empty(); sendPacket.strToBuf("sEN"); sendPacket.strToBuf("LMDscandata"); sendPacket.uByteToBuf(1); sendPacket.finalizePacket(); //printf("(%s)\n", sendPacket.getBuf()); // just ask for continuous data if (!myConn->write(sendPacket.getBuf(), sendPacket.getLength())) { ArLog::log(ArLog::Terse, "%s: Could not send %s to laser", getName(), "LMDscandata"); failedToConnect(); return false; } while (timeDone.mSecTo()) { packet = myReceiver.receivePacket(1000); if (packet != NULL && strcasecmp(packet->getCommandType(), "sSN") == 0 && strcasecmp(packet->getCommandName(), "LMDscandata") == 0) { delete packet; packet = NULL; lockDevice(); myIsConnected = true; myTryingToConnect = false; unlockDevice(); ArLog::log(ArLog::Normal, "%s: Connected to laser", getName()); laserConnect(); return true; } else if (packet != NULL) { ArLog::log(myLogLevel, "%s: Got %s %s (%d long)", getName(), packet->getCommandType(), packet->getCommandName(), packet->getLength()); delete packet; packet = NULL; } } ArLog::log(ArLog::Terse, "%s: Did not get scandata back from laser", getName()); failedToConnect(); return false; }