AREXPORT bool ArUrg::blockingConnect(void)
{
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;
}
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
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;
}
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());
ArUtil::sleep(100);
bool connected = false;
if (internalConnect())
connected = true;
if (connected)
{
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else
{
failedToConnect();
return false;
}
}
AREXPORT bool ArSZSeries::blockingConnect(void) {
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;
}
// myPrevSensorIntTime = myConn->getTimeRead(0);
// PS 9/9/11 - moved this here to fix issue with setting baud in mt400.p
laserPullUnsetParamsFromRobot();
laserCheckParams();
// PS 9/9/11 - add setting baud
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
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;
}
// PS - set logging level and laser type in packet receiver class
myReceiver.setmyInfoLogLevel(myInfoLogLevel);
myReceiver.setmyName(getName());
myReceiver.setDeviceConnection(myConn);
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
// PS 9/9/11 - moved up top
//laserPullUnsetParamsFromRobot();
//laserCheckParams();
int size = ArMath::roundInt((270/.3) + 1);
ArLog::log(myInfoLogLevel,
"%s::blockingConnect() Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArTime timeDone;
if (myPowerControlled)
{
if (!timeDone.addMSec(60 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (60 * 1000)",
getName());
}
}
else
{
if (!timeDone.addMSec(30 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (30 * 1000)",
getName());
}
}
ArSZSeriesPacket *packet;
ArSZSeriesPacket sendPacket;
#if 0
sendPacket.empty();
sendPacket.uByteToBuf(0xA0); // stop continous sending
sendPacket.uByteToBuf(0x00);
sendPacket.uByteToBuf(0x1D);
sendPacket.uByteToBuf(0x7E);
sendPacket.finalizePacket();
if ((myConn->write(sendPacket.getBuf(), sendPacket.getLength())) == -1)
{
ArLog::log(ArLog::Terse,
"%s::blockingConnect() Could not send Stop Continuous mode to laser", getName());
failedToConnect();
return false;
}
#endif
// Build the Start Continuous sending packet and set it
// once we get a response, then we are connected, note
// the response needs to be a real reading
sendPacket.empty();
// command id = 0x91
//sendPacket.uByteToBuf(145);
sendPacket.uByteToBuf(0x91);
// note communication ID default is 0
// this value is set via the SZ Configurator
// ???? not sure what to do if it fails
// and put in CRC - from manual, this is
// specific to the communication ID = 0
sendPacket.uByteToBuf(0);
sendPacket.uByteToBuf(43);
sendPacket.uByteToBuf(218);
unsigned short crc = myReceiver.CRC16((unsigned char *)sendPacket.getBuf(), 2);
#if 0
// other communications IDs and CRC
// communication ID =1
sendPacket.uByteToBuf(1);
sendPacket.uByteToBuf(59);
sendPacket.uByteToBuf(251);
// communication ID =2
sendPacket.uByteToBuf(2);
sendPacket.uByteToBuf(11);
sendPacket.uByteToBuf(152);
// communication ID =3
sendPacket.uByteToBuf(3);
sendPacket.uByteToBuf(27);
sendPacket.uByteToBuf(185);
#endif
sendPacket.finalizePacket();
IFDEBUG(
int i;
char x[100000];
printf("buffer with len = %d: ",sendPacket.getLength());
for (i = 0;i < sendPacket.getLength();i++)
{
printf("0x%x ",sendPacket.getBuf()[i] & 0xff);
//sprintf(&x[i], "%2x", (char *)sendPacket.getBuf()[i]);
}
printf("\n");
//ArLog::log(ArLog::Terse,
// "%s::blockingConnect() write Buffer = %s", getName(), x);
); // end IFDEBUG
AREXPORT bool ArUrg_2_0::blockingConnect(void)
{
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;
}
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
// if we have a starting baud and are a serial port, then change the
// baud rate... not by default this will set it to 0 baud which'll
// cause the serial stuff not to touch it
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
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;
}
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
ArUtil::sleep(100);
bool connected = false;
if (internalConnect())
connected = true;
if (connected)
{
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else
{
failedToConnect();
return false;
}
}
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;
}
