// the important function
void KeyPTU::drive(void)
{
// if the PTU isn't initialized, initialize it here... it has to be
// done here instead of above because it needs to be done when the
// robot is connected
if (!myPTUInitRequested && !myPTU.isInitted() && myRobot->isConnected())
{
printf("\nWaiting for Camera to Initialize\n");
myAbsolute = true;
myPTUInitRequested = true;
myPTU.init();
}
// if the camera hasn't initialized yet, then just return
if (myPTUInitRequested && !myPTU.isInitted())
{
return;
}
if (myPTUInitRequested && myPTU.isInitted())
{
myPTUInitRequested = false;
myPanSlew = myPTU.getPanSlew();
myTiltSlew = myPTU.getTiltSlew();
printf("Done.\n");
question();
}
if (myExerciseTime.secSince() > 5 && myExercise)
{
int pan,tilt;
if (ArMath::random()%2)
pan = ArMath::random()%((int)myPTU.getMaxPosPan());
else
pan = -ArMath::random()%((int)myPTU.getMaxNegPan());
if (ArMath::random()%2)
tilt = ArMath::random()%((int)myPTU.getMaxPosTilt());
else
tilt = -ArMath::random()%((int)myPTU.getMaxNegTilt());
myPTU.panTilt(pan, tilt);
//printf("** %d\n", myRobot->getEstop());
//printf("--> %x\n", myRobot->getFlags());
myExerciseTime.setToNow();
}
}
int main(int argc, char** argv)
{
Aria::init();
ArLog::init(ArLog::StdErr, ArLog::Normal);
ArArgumentParser argParser(&argc, argv);
argParser.loadDefaultArguments();
ArSimpleConnector connector(&argParser);
ArGPSConnector gpsConnector(&argParser);
if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
ArLog::log(ArLog::Terse, "gpsExample options:\n -printTable Print data to standard output in regular columns rather than a refreshing terminal display, and print more digits of precision");
return 1;
}
// Try connecting to robot
ArRobot robot;
if(!connector.connectRobot(&robot))
{
ArLog::log(ArLog::Terse, "gpsExample: Warning: Could not connect to robot. Will not be able to switch GPS power on, or load GPS options from this robot's parameter file.");
}
else
{
ArLog::log(ArLog::Normal, "gpsExample: Connected to robot.");
robot.runAsync(true);
}
// check command line arguments for -printTable
bool printTable = argParser.checkArgument("printTable");
// On the Seekur, power to the GPS receiver is switched on by this command.
// (A third argument of 0 would turn it off). On other robots this command is
// ignored.
robot.com2Bytes(116, 6, 1);
// Try connecting to a GPS. We pass the robot pointetr to the connector so it
// can check the robot parameters for this robot type for default values for
// GPS device connection information (receiver type, serial port, etc.)
ArLog::log(ArLog::Normal, "gpsExample: Connecting to GPS, it may take a few seconds...");
ArGPS *gps = gpsConnector.createGPS(&robot);
if(!gps || !gps->connect())
{
ArLog::log(ArLog::Terse, "gpsExample: Error connecting to GPS device. Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments. Use -help for help.");
return -1;
}
ArLog::log(ArLog::Normal, "gpsExample: Reading data...");
ArTime lastReadTime;
if(printTable)
gps->printDataLabelsHeader();
while(true)
{
int r = gps->read();
if(r & ArGPS::ReadError)
{
ArLog::log(ArLog::Terse, "gpsExample: Warning: error reading GPS data.");
ArUtil::sleep(1000);
continue;
}
if(r & ArGPS::ReadUpdated)
{
if(printTable)
{
gps->printData(false);
printf("\n");
}
else
{
gps->printData();
printf("\r");
}
fflush(stdout);
ArUtil::sleep(500);
lastReadTime.setToNow();
continue;
} else {
if(lastReadTime.secSince() >= 5) {
ArLog::log(ArLog::Terse, "gpsExample: Warning: haven't recieved any data from GPS for more than 5 seconds!");
}
ArUtil::sleep(1000);
continue;
}
}
return 0;
}
void *KinectArVideoServer::runThread(void*)
{
// TODO might need to move initialization to separate function
/* Open Kinect */
/*
int ndevs = freenect2.enumerateDevices();
printf("KinectArVideoServer: %d devices\n", ndevs);
std::string serial = freenect2.getDefaultDeviceSerialNumber();
printf("KinectArVideoServer: device serial number is %s\n", serial.c_str());
*/
freenect_dev = freenect2.openDefaultDevice();
if(!freenect_dev)
{
std::cout << "KinectArVideoServer: no kinect2 device connected or failure opening the default one!" << std::endl;
return 0;
}
shutdown = false;
/* Set up libfreenect listeners, start capturing */
//libfreenect2::SyncMultiFrameListener listener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
libfreenect2::SyncMultiFrameListener listener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
libfreenect2::FrameMap frames;
freenect_dev->setColorFrameListener(&listener);
freenect_dev->setIrAndDepthFrameListener(&listener);
assert(freenect_dev);
if(! freenect_dev->start() )
{
std::cout << "KinectArVideoServer: Error starting stream from kinect!" << std::endl;
return NULL;
}
std::cout << "kinect device serial: " << freenect_dev->getSerialNumber() << std::endl;
std::cout << "kinect device firmware: " << freenect_dev->getFirmwareVersion() << std::endl;
// TODO set up registration in libfreenect2
ArVideoOpenCV kinectDepthSource("Kinect_Depth|libfreenect2|OpenCV");
ArVideo::createVideoServer(server, &kinectDepthSource, "Kinect_Depth|libfreenect2|OpenCV", "freenect2|Depth|OpenCV");
ArVideoOpenCV kinectRGBSource("Kinect_RGB|libfreenect2|OpenCV");
ArVideo::createVideoServer(server, &kinectRGBSource, "Kinect_RGB|libfreenect2|OpenCV", "freenect2|RGB|OpenCV");
// ArVideoOpenCV kinectThreshSource("Kinect_Depth|libfreenect2|OpenCV_threshold");
// ArVideo::createVideoServer(&server, &kinectThreshSource, "Kinect_Depth|libfreenect2|OpenCV_threshold", "Kinect depth data with basic threshold applied");
/* Main loop, capture images from kinect, display, copy to ArVideo sources */
bool first = true;
cv::Mat rgbm_small(resize_to_width, resize_to_height, CV_8UC3);
cv::Mat depthm_small(resize_to_width, resize_to_height, CV_8UC3);
cv::Mat rgbm_flip(resize_to_width, resize_to_height, CV_8UC3);
cv::Mat depthm_flip(resize_to_width, resize_to_height, CV_8UC3);
while(!shutdown)
{
// std::cout << "." << std::flush;
ArTime t;
listener.waitForNewFrame(frames); //, 30000);
if(t.secSince() >= 28)
{
std::cout << "KinectArVideoServer: Warning: took more than 30 seconds to receive a frame from Kinect!" << std::endl;
//shutdown = true;
//continue;
}
// printf("%d\n", t.secSince());
libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
// libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
// todo don't recreate Mat objects each time
cv::Mat rgbm(rgb->height, rgb->width, CV_8UC4, rgb->data);
cv::resize (rgbm, rgbm_small, cv::Size(resize_to_width, resize_to_height));
cv::flip(rgbm_small, rgbm_flip, 1);
cv::Mat depthm(depth->height, depth->width, CV_32FC1, depth->data);
cv::resize(depthm, depthm_small, cv::Size(resize_to_width, resize_to_height));
cv::flip(depthm_small, depthm_flip, 1);
// cv::Mat depth_thresh(depth->height, depth->width, CV_32FC1, depth->data);
// cv::threshold(depthm, depth_thresh, 0.4, 1.0, CV_THRESH_BINARY_INV);
// cv::imshow("rgb", rgbm_small);
// cv::imshow("ir", cv::Mat(ir->height, ir->width, CV_32FC1, ir->data) / 20000.0f);
// cv::imshow("depth", depthm / 4500.0f);
if(!kinectRGBSource.updateVideoDataCopy(rgbm_flip, 1, CV_BGR2RGB))
std::cout << "KinectArVideoServer: Warning: error copying rgb data to ArVideo source" << std::endl;
if(!kinectDepthSource.updateVideoDataCopy(depthm_flip/4500.0f, 255,
/*(1/255.0),*/ CV_GRAY2RGB))
std::cout << "KinectArVideoServer: Warning: error copying depth data to ArVideo source" << std::endl;
// if(!kinectThreshSource.updateVideoDataCopy(depth_thresh, 255, CV_GRAY2RGB))
// std::cout << "Warning error copying depth thresholded data to ArVideo source" << std::endl;
listener.release(frames);
//libfreenect2::this_thread::sleep_for(libfreenect2::chrono::milliseconds(100));
// if(first)
// {
// first = false;
// cv::moveWindow("rgb", 90, 85);
// cv::moveWindow("depth", 90, 599);
// }
//ArUtil::sleep(1);
}
close();
// TODO destroy ArVideo servers created
return 0;
}
bool ArUrg::internalConnect(void)
{
bool ret = true;
char buf[1024];
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
bool alreadyAtAutobaud = false;
// empty the buffer...
buf[0] = '\0';
while (readLine(buf, sizeof(buf), 1));
if (!(ret = sendCommandAndRecvStatus(
"V", "version request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "0") != 0)
{
// if we didn't get it, try it at what the autobaud rate is
if (serConn != NULL)
{
alreadyAtAutobaud = true;
serConn->setBaud(atoi(getAutoBaudChoice()));
ArUtil::sleep(100);
if (!(ret = sendCommandAndRecvStatus(
"V", "version request after falling back to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "0") != 0)
{
if (ret && strcasecmp(buf, "0") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after falling back to autobaudchoice",
getName());
return false;
}
}
// if we don't have a serial port, then we can't change the baud,
// so just fail
else
{
if (ret && strcasecmp(buf, "0") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response (%s)",
getName(), buf);
return false;
}
}
if (!alreadyAtAutobaud && serConn != NULL)
{
// empty the buffer from the last version request
while (readLine(buf, sizeof(buf), 100));
// now change the baud...
sprintf(buf, "S%06d7654321", atoi(getAutoBaudChoice()));
if (!writeLine(buf))
return false;
ArUtil::sleep(100);
//serConn->setBaud(115200);
serConn->setBaud(atoi(getAutoBaudChoice()));
// wait a second for the baud to change...
ArUtil::sleep(100);
// empty the buffer from the baud change
while (readLine(buf, sizeof(buf), 100));
if (!(ret = sendCommandAndRecvStatus(
"V", "version request after switching to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "0") != 0)
{
if (ret && strcasecmp(buf, "0") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after switching to autobaudchoice",
getName());
return false;
}
ArLog::log(ArLog::Normal, "%s: Switched to %s baud rate",
getName(), getAutoBaudChoice());
}
while (readLine(buf, sizeof(buf), 10000))
{
/// MPL put this in instead of the following because of the
/// behavior change of readline
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "VEND:", strlen("VEND:")) == 0)
myVendor = &buf[5];
else if (strncasecmp(buf, "PROD:", strlen("PROD:")) == 0)
myProduct = &buf[5];
else if (strncasecmp(buf, "FIRM:", strlen("FIRM:")) == 0)
myFirmwareVersion = &buf[5];
else if (strncasecmp(buf, "PROT:", strlen("PROT:")) == 0)
myProtocolVersion = &buf[5];
else if (strncasecmp(buf, "SERI:", strlen("SERI:")) == 0)
mySerialNumber = &buf[5];
else if (strncasecmp(buf, "STAT:", strlen("STAT:")) == 0)
myStat = &buf[5];
}
if (myVendor.empty() || myProduct.empty() || myFirmwareVersion.empty() ||
myProtocolVersion.empty() || mySerialNumber.empty())
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in version response",
getName());
return false;
}
log();
myLogMore = true;
// myLogMore = false;
ArUtil::sleep(100);
printf("myRequestString %s\n", myRequestString);
if (!(ret = sendCommandAndRecvStatus(
myRequestString, "request distance reading",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "0") != 0)
{
if (ret && strcasecmp(buf, "0") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on distance reading response (%c)",
getName(), buf[0]);
return false;
}
myLogMore = false;
ArTime started;
started.setToNow();
while (started.secSince() < 10 && readLine(buf, sizeof(buf), 10000))
{
if (strlen(buf) == 0)
return true;
}
ArLog::log(ArLog::Normal, "%s::blockingConnect: Did not get distance reading back",
getName(), buf[0]);
return false;
}
bool ArUrg_2_0::internalConnect(void)
{
bool ret = true;
char buf[1024];
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
bool alreadyAtAutobaud = false;
// empty the buffer...
/*
sendCommandAndRecvStatus(
"RS", "reset",
buf, sizeof(buf), 1000);
readLine(buf, sizeof(buf), 1, true, false);
sendCommandAndRecvStatus(
"SCIP2.0", "SCIP 2.0 request",
buf, sizeof(buf), 1000);
*/
writeLine("RS");
ArUtil::sleep(100);
writeLine("SCIP2.0");
ArUtil::sleep(100);
ArTime startedFlushing;
while (readLine(buf, sizeof(buf), 1, true, false) ||
startedFlushing.mSecSince() < 1000);
buf[0] = '\0';
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
// if we didn't get it and have an autobaud, try it at what the autobaud rate is
if (serConn != NULL && atoi(getAutoBaudChoice()) > 0)
{
alreadyAtAutobaud = true;
serConn->setBaud(atoi(getAutoBaudChoice()));
ArUtil::sleep(100);
writeLine("RS");
ArUtil::sleep(100);
writeLine("SCIP2.0");
ArUtil::sleep(100);
startedFlushing.setToNow();
while (readLine(buf, sizeof(buf), 1, true, false) ||
startedFlushing.mSecSince() < 1000);
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request after falling back to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after falling back to autobaudchoice",
getName());
return false;
}
}
// if we don't have a serial port or no autobaud then we can't
// change the baud, so just fail
else
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response (%s)",
getName(), buf);
return false;
}
}
// if we want to autobaud, then give it a whirl
if (!alreadyAtAutobaud && serConn != NULL && atoi(getAutoBaudChoice()) > 0)
{
// empty the buffer from the last version request
while (readLine(buf, sizeof(buf), 100, true, false));
// now change the baud...
sprintf(buf, "SS%06d", atoi(getAutoBaudChoice()));
if (!writeLine(buf))
return false;
ArUtil::sleep(100);
//serConn->setBaud(115200);
serConn->setBaud(atoi(getAutoBaudChoice()));
// wait a second for the baud to change...
ArUtil::sleep(100);
// empty the buffer from the baud change
while (readLine(buf, sizeof(buf), 100, true, false));
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request after switching to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after switching to autobaudchoice",
getName());
return false;
}
ArLog::log(ArLog::Verbose, "%s: Switched to %s baud rate",
getName(), getAutoBaudChoice());
}
while (readLine(buf, sizeof(buf), 10000, false, true))
{
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "VEND:", strlen("VEND:")) == 0)
myVendor = &buf[5];
else if (strncasecmp(buf, "PROD:", strlen("PROD:")) == 0)
myProduct = &buf[5];
else if (strncasecmp(buf, "FIRM:", strlen("FIRM:")) == 0)
myFirmwareVersion = &buf[5];
else if (strncasecmp(buf, "PROT:", strlen("PROT:")) == 0)
myProtocolVersion = &buf[5];
else if (strncasecmp(buf, "SERI:", strlen("SERI:")) == 0)
mySerialNumber = &buf[5];
else if (strncasecmp(buf, "STAT:", strlen("STAT:")) == 0)
myStat = &buf[5];
}
if (myVendor.empty() || myProduct.empty() || myFirmwareVersion.empty() ||
myProtocolVersion.empty() || mySerialNumber.empty())
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in version response",
getName());
return false;
}
if (!(ret = sendCommandAndRecvStatus(
"PP", "parameter info request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad response to parameter info request",
getName());
return false;
}
while (readLine(buf, sizeof(buf), 10000, false, true))
{
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "MODL:", strlen("MODL:")) == 0)
myModel = &buf[5];
else if (strncasecmp(buf, "DMIN:", strlen("DMIN:")) == 0)
myDMin = atoi(&buf[5]);
else if (strncasecmp(buf, "DMAX:", strlen("DMAX:")) == 0)
myDMax = atoi(&buf[5]);
else if (strncasecmp(buf, "ARES:", strlen("ARES:")) == 0)
myARes = atoi(&buf[5]);
else if (strncasecmp(buf, "AMIN:", strlen("AMIN:")) == 0)
myAMin = atoi(&buf[5]);
else if (strncasecmp(buf, "AMAX:", strlen("AMAX:")) == 0)
myAMax = atoi(&buf[5]);
else if (strncasecmp(buf, "AFRT:", strlen("AFRT:")) == 0)
myAFront = atoi(&buf[5]);
else if (strncasecmp(buf, "SCAN:", strlen("SCAN:")) == 0)
myScan = atoi(&buf[5]);
}
if (myModel.empty() || myDMin == 0 || myDMax == 0 || myARes == 0 ||
myAMin == 0 || myAMax == 0 || myAFront == 0 || myScan == 0)
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in parameter info response",
getName());
return false;
}
myStepSize = 360.0 / myARes;
myStepFirst = myAFront * myStepSize;
if (myMaxRange > myDMax)
setMaxRange(myDMax);
//log();
setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());
//myLogMore = true;
// myLogMore = false;
ArUtil::sleep(100);
//printf("myRequestString %s\n", myRequestString);
if (!(ret = sendCommandAndRecvStatus(
myRequestString, "request distance reading",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on distance reading response (%s)",
getName(), buf);
return false;
}
//myLogMore = false;
ArTime started;
started.setToNow();
while (started.secSince() < 10 &&
readLine(buf, sizeof(buf), 10000, true, false))
{
if (strlen(buf) == 0)
return true;
}
ArLog::log(ArLog::Normal, "%s::blockingConnect: Did not get distance reading back",
getName());
return false;
}
int main(int argc, char **argv)
{
int ret;
std::string str;
ArSerialConnection con;
ArSickPacket sick;
ArSickPacket *packet;
ArSickPacketReceiver receiver(&con);
ArTime start;
unsigned int value;
int numReadings;
ArTime lastReading;
ArTime packetTime;
start.setToNow();
// open the connection, if it fails, exit
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
start.setToNow();
printf("Waiting for laser to power on\n");
sick.empty();
sick.uByteToBuf(0x10);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
while (start.secSince() < 70 &&
((packet = receiver.receivePacket(100)) == NULL
|| (packet->getID() != 0x90)));
if (packet != NULL)
printf("Laser powered on\n");
else
exit(1);
printf("Changing baud\n");
sick.empty();
sick.byteToBuf(0x20);
sick.byteToBuf(0x40);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
ArUtil::sleep(10);
if (!con.setBaud(38400))
{
printf("Could not set baud, exiting\n");
}
/*packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
*/
sick.empty();
sick.uByteToBuf(0x3B);
sick.uByte2ToBuf(180);
sick.uByte2ToBuf(100);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
sick.empty();
sick.byteToBuf(0x20);
sick.byteToBuf(0x24);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
printf("Starting to report back from port, it took %ld ms to get here:\n",
start.mSecSince());
start.setToNow();
while (start.secSince() < 6)
{
packetTime.setToNow();
packet = receiver.receivePacket();
if (packet != NULL)
printf("####### %ld ms was how long the packet took\n", packetTime.mSecSince());
if (packet != NULL)
{
if (packet->getLength() < 10)
packet->log();
else if (packet->getID() == 0x90)
{
char strBuf[512];
packet->log();
//printf("%x\n", packet->bufToUByte());
packet->bufToStr(strBuf, 512);
printf("0x%x %s\n", packet->getID(), strBuf);
sick.empty();
sick.uByteToBuf(0x3B);
sick.uByte2ToBuf(180);
sick.uByte2ToBuf(100);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
sick.empty();
sick.uByteToBuf(0x20);
sick.uByteToBuf(0x24);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
}
else
{
value = packet->bufToUByte2();
numReadings = value & 0x3ff;
printf("%ld ms after last reading.\n", lastReading.mSecSince());
/*
printf("Reading number %d, complete %d, unit: %d %d:\n", value & 0x3ff, !(bool)(value & ArUtil::BIT13), (bool)(value & ArUtil::BIT14), (bool)(value & ArUtil::BIT15));
for (i = 0; i < numReadings; i++)
{
value = packet->bufToUByte2();
if (value & ArUtil::BIT13)
printf("D");
printf("%d ", value & 0x1fff);
}
printf("\n");
*/
lastReading.setToNow();
}
}
else
{
//printf("No packet\n");
}
}
}
bool tryport(const char *port, bool is422, int baud, char **argv, int argc, int maxread, int timeout)
{
ArSerialConnection ser1(is422);
ser1.setPort(port);
ser1.setBaud(baud);
printf("Trying %s port %s at baud rate %d:\n", is422?"RS-422":"RS-232", port, baud);
if (!ser1.openSimple())
{
ArLog::logErrorFromOS(ArLog::Terse, "Error opening %s.", port);
return false;
}
if (argc > 0)
{
printf("-> %3d bytes: ", argc);
fflush(stdout);
}
for (int i = 0; i < argc; ++i)
{
int d = strtol(argv[i], NULL, 0);
if (d == 0 && errno != 0)
{
ArLog::logErrorFromOS(ArLog::Terse, "error parsing command argument %d (\"%s\"). Must be decimal, hexidecimal, or octal number.", i, argv[i]);
Aria::exit(3);
}
unsigned char c = (unsigned char)d;
printf("0x%.2X %c ", (unsigned char) c, (c >= ' ' && c <= '~') ? c : ' ');
fflush(stdout);
ser1.write((char*)&c, 1);
}
if (argc > 0)
puts("");
char buf[256];
int n = 0;
ArTime t;
while (1)
{
ArUtil::sleep(1);
int r = 0;
if ((r = ser1.read(buf, sizeof(buf))) < 0)
{
puts("Error reading data from serial port.");
return false;
}
else if (r > 0)
{
printf("<- %3d bytes: ", r);
for (int i = 0; i < r; ++i)
{
//printf("[%d] ", i); fflush(stdout);
char c = buf[i];
printf("0x%.2X %c ", (unsigned char)c, (c >= ' ' && c <= '~') ? c : ' ');
if ((i+1) % 8 == 0)
printf("\n ");
}
printf("\n");
}
if ((n += r) >= maxread)
{
puts("max");
return true;
}
if (t.secSince() > timeout)
{
puts("timeout");
return false;
}
}
return true;
}
