void Opa::connect(int port)
{
disconnect();
int result = comOpen(port, OPA_COM_BAUDRATE);
if (!result) {
state = OPA_DISCONNECTED;
return;
}
this->port = port;
memset(rxBuffer, 0, OPA_RXBUFFER_LEN);
rxLen = 0;
globalsReturn = NULL;
programReturn = NULL;
kitReturn = NULL;
programReturnIndex = 0;
kitReturnIndex = 0;
programParamReturn = NULL;
programParamReturnIndex = 0;
kitParamReturn = NULL;
kitParamReturnIndex = 0;
state = OPA_CONNECTED;
}
double Motor::motorMove ()
{
comOpen ();
comSet (19200, 8, 'N', 1); //set the port's parameters
char buf[30];
int n_pnum = 0;
n_pnum = getPulseNum ();
sprintf(buf, "M%c%06d%06d", m_direction_, m_frequence_, n_pnum); //get the motor control command with paraters that we set.
comWrite (buf);
double time_motor_move = ros::Time::now ().toSec ();
ROS_INFO ("The command is %s", buf);
comClose ();
return time_motor_move;
}
static void
#if defined(_WIN32)
WINAPI
#endif
service_loop(int argc, char** argv)
{
int argn;
char* arg;
char str[128];
char compiler[128];
for(argn=1; argn<(int)argc; argn++) {
arg=argv[argn];
if(*arg!='-') { /* .ini file specified */
if(!fexist(arg)) {
lprintf(LOG_ERR,"Initialization file does not exist: %s", arg);
exit(usage(argv[0]));
}
parse_ini_file(arg);
continue;
}
while(*arg=='-')
arg++;
if(stricmp(arg,"null")==0)
mdm_null=TRUE;
else if(stricmp(arg,"com")==0 && argc > argn+1)
SAFECOPY(com_dev, argv[++argn]);
else if(stricmp(arg,"baud")==0 && argc > argn+1)
com_baudrate = (ulong)strtol(argv[++argn],NULL,0);
else if(stricmp(arg,"host")==0 && argc > argn+1)
SAFECOPY(host, argv[++argn]);
else if(stricmp(arg,"port")==0 && argc > argn+1)
port = (ushort)strtol(argv[++argn], NULL, 0);
else if(stricmp(arg,"live")==0) {
if(argc > argn+1 &&
(com_handle = (COM_HANDLE)strtol(argv[argn+1], NULL, 0)) != 0) {
argn++;
com_handle_passed=TRUE;
}
com_alreadyconnected=TRUE;
terminate_after_one_call=TRUE;
mdm_null=TRUE;
}
else if(stricmp(arg,"nohangup")==0) {
com_hangup=FALSE;
}
else if(stricmp(arg,"debug")==0) {
log_level=LOG_DEBUG;
}
else if(stricmp(arg,"help")==0 || *arg=='?')
exit(usage(argv[0]));
else {
fprintf(stderr,"Invalid option: %s\n", arg);
exit(usage(argv[0]));
}
}
#if defined(_WIN32)
/* Convert "1" to "COM1" for Windows */
{
int i;
if((i=atoi(com_dev)) != 0)
SAFEPRINTF(com_dev, "COM%d", i);
}
if(daemonize) {
if((svc_status_handle = RegisterServiceCtrlHandler(NAME, ServiceControlHandler))==0) {
lprintf(LOG_ERR,"!ERROR %d registering service control handler",GetLastError());
return;
}
svc_status.dwServiceType=SERVICE_WIN32_OWN_PROCESS;
svc_status.dwControlsAccepted=SERVICE_ACCEPT_SHUTDOWN;
svc_status.dwWaitHint=NTSVC_TIMEOUT_STARTUP;
svc_status.dwCurrentState=SERVICE_START_PENDING;
SetServiceStatus(svc_status_handle, &svc_status);
}
#endif
lprintf(LOG_INFO,"%s", comVersion(str,sizeof(str)));
DESCRIBE_COMPILER(compiler);
lprintf(LOG_INFO,"Build %s %s %s", __DATE__, __TIME__, compiler);
/************************************/
/* Inititalize WinSock and COM Port */
/************************************/
if(!winsock_startup())
exit(1);
/* Install clean-up callback */
atexit(cleanup);
lprintf(LOG_INFO,"TCP Host: %s", host);
lprintf(LOG_INFO,"TCP Port: %u", port);
if(!com_handle_passed) {
lprintf(LOG_INFO,"Opening Communications Device (COM Port): %s", com_dev);
if((com_handle=comOpen(com_dev)) == COM_HANDLE_INVALID) {
lprintf(LOG_ERR,"ERROR %u opening communications device/port: '%s'", COM_ERROR_VALUE, com_dev);
exit(1);
}
}
lprintf(LOG_INFO,"COM Port device handle: %u", com_handle);
if(com_baudrate!=0) {
if(!comSetBaudRate(com_handle,com_baudrate))
lprintf(LOG_ERR,"ERROR %u setting DTE rate to %lu bps"
,COM_ERROR_VALUE, com_baudrate);
}
lprintf(LOG_INFO,"COM Port DTE rate: %ld bps", comGetBaudRate(com_handle));
if(ident)
_beginthread(ident_server_thread, 0, NULL);
#if defined(_WIN32)
if(daemonize) {
svc_status.dwCurrentState=SERVICE_RUNNING;
svc_status.dwControlsAccepted|=SERVICE_ACCEPT_STOP;
SetServiceStatus(svc_status_handle, &svc_status);
}
#endif
/***************************/
/* Initialization Complete */
/***************************/
/* Main service loop: */
while(!terminated && wait_for_call(com_handle)) {
if(!carrier_detect(com_handle)) /* re-initialization timer time-out? */
continue;
comWriteByte(com_handle,'\r');
comWriteString(com_handle, banner);
comWriteString(com_handle, "\r\n");
if((sock=connect_socket(host, port)) == INVALID_SOCKET) {
comWriteString(com_handle,"\7\r\n!ERROR connecting to TCP port\r\n");
} else {
handle_call();
close_socket(&sock);
total_calls++;
lprintf(LOG_INFO,"Call completed (%lu total)", total_calls);
}
if(com_hangup && !hangup_call(com_handle))
break;
if(terminate_after_one_call)
break;
}
exit(0);
}
int serial_init(int port, int baudrate) {
comInit();
return comOpen(port, baudrate, DATABITS_8, STOPBITS_1, PARITY_NONE, FLOWCONTROL_NONE, COM_READWRITE, __SERIAL_BUFFER_SIZE);
}
int modem_connect(struct bbslist *bbs)
{
int ret;
char respbuf[1024];
init_uifc(TRUE, TRUE);
if(bbs->conn_type == CONN_TYPE_SERIAL) {
if((com=comOpen(bbs->addr)) == COM_HANDLE_INVALID) {
uifcmsg("Cannot Open Port", "`Cannot Open Port`\n\n"
"Cannot open the specified serial device.\n");
conn_api.terminate=-1;
return(-1);
}
if(bbs->bpsrate) {
if(!comSetBaudRate(com, bbs->bpsrate)) {
uifcmsg("Cannot Set Baud Rate", "`Cannot Set Baud Rate`\n\n"
"Cannot open the specified serial device.\n");
conn_api.terminate=-1;
comClose(com);
return(-1);
}
}
if(!comRaiseDTR(com)) {
uifcmsg("Cannot Raise DTR", "`Cannot Raise DTR`\n\n"
"comRaiseDTR() returned an error.\n");
conn_api.terminate=-1;
comClose(com);
return(-1);
}
}
else {
if((com=comOpen(settings.mdm.device_name)) == COM_HANDLE_INVALID) {
uifcmsg("Cannot Open Modem", "`Cannot Open Modem`\n\n"
"Cannot open the specified modem device.\n");
conn_api.terminate=-1;
return(-1);
}
if(settings.mdm.com_rate) {
if(!comSetBaudRate(com, settings.mdm.com_rate)) {
uifcmsg("Cannot Set Baud Rate", "`Cannot Set Baud Rate`\n\n"
"Cannot open the specified modem device.\n");
conn_api.terminate=-1;
comClose(com);
return(-1);
}
}
if(!comRaiseDTR(com)) {
uifcmsg("Cannot Raise DTR", "`Cannot Raise DTR`\n\n"
"comRaiseDTR() returned an error.\n");
conn_api.terminate=-1;
comClose(com);
return(-1);
}
/* drain keyboard input to avoid accidental cancel */
while(kbhit())
getch();
uifc.pop("Initializing...");
comWriteString(com, settings.mdm.init_string);
comWriteString(com, "\r");
/* Wait for "OK" */
while(1) {
if((ret=modem_response(respbuf, sizeof(respbuf), 5))!=0) {
modem_close();
uifc.pop(NULL);
if(ret<0)
uifcmsg("Modem Not Responding", "`Modem Not Responding`\n\n"
"The modem did not respond to the initializtion string\n"
"Check your init string and phone number.\n");
conn_api.terminate=-1;
return(-1);
}
if(strstr(respbuf, settings.mdm.init_string)) /* Echo is on */
continue;
break;
}
if(!strstr(respbuf, "OK")) {
modem_close();
uifc.pop(NULL);
uifcmsg(respbuf, "`Initialization Error`\n\n"
"The modem did not respond favorably to your initialization string.\n");
conn_api.terminate=-1;
return(-1);
}
uifc.pop(NULL);
uifc.pop("Dialing...");
comWriteString(com, settings.mdm.dial_string);
comWriteString(com, bbs->addr);
comWriteString(com, "\r");
/* Wait for "CONNECT" */
while(1) {
if((ret=modem_response(respbuf, sizeof(respbuf), 60))!=0) {
modem_close();
uifc.pop(NULL);
if(ret<0)
uifcmsg(respbuf, "`No Answer`\n\n"
"The modem did not connect within 60 seconds.\n");
conn_api.terminate=-1;
return(-1);
}
if(strstr(respbuf, bbs->addr)) /* Dial command echoed */
continue;
break;
}
if(!strstr(respbuf, "CONNECT")) {
modem_close();
uifc.pop(NULL);
uifcmsg(respbuf, "`Connection Failed`\n\n"
"SyncTERM was unable to establish a connection.\n");
conn_api.terminate=-1;
return(-1);
}
uifc.pop(NULL);
uifc.pop(respbuf);
SLEEP(1000);
uifc.pop(NULL);
}
if(!create_conn_buf(&conn_inbuf, BUFFER_SIZE)) {
conn_api.close();
return(-1);
}
if(!create_conn_buf(&conn_outbuf, BUFFER_SIZE)) {
conn_api.close();
destroy_conn_buf(&conn_inbuf);
return(-1);
}
if(!(conn_api.rd_buf=(unsigned char *)malloc(BUFFER_SIZE))) {
conn_api.close();
destroy_conn_buf(&conn_inbuf);
destroy_conn_buf(&conn_outbuf);
return(-1);
}
conn_api.rd_buf_size=BUFFER_SIZE;
if(!(conn_api.wr_buf=(unsigned char *)malloc(BUFFER_SIZE))) {
conn_api.close();
destroy_conn_buf(&conn_inbuf);
destroy_conn_buf(&conn_outbuf);
FREE_AND_NULL(conn_api.rd_buf);
return(-1);
}
conn_api.wr_buf_size=BUFFER_SIZE;
if(bbs->conn_type == CONN_TYPE_SERIAL) {
_beginthread(modem_output_thread, 0, (void *)-1);
_beginthread(modem_input_thread, 0, (void *)-1);
}
else {
_beginthread(modem_output_thread, 0, NULL);
_beginthread(modem_input_thread, 0, NULL);
}
uifc.pop(NULL);
return(0);
}
//............................................................................
int main(int argc, char *argv[]) {
int optChar;
char comPort[FNAME_SIZE];
char inpFileName[FNAME_SIZE];
char outFileName[FNAME_SIZE];
char savFileName[FNAME_SIZE];
char matFileName[FNAME_SIZE];
TargetLink link = NO_LINK;
// default configuration options
strcpy(inpFileName, "qs.bin");
strcpy(comPort, "COM1");
int tcpPort = 6601;
int baudRate = 38400;
uint8_t quiet = 0;
uint8_t tstampSize = 4;
uint8_t objPtrSize = 4;
uint8_t funPtrSize = 4;
uint8_t sigSize = 1;
uint8_t evtSize = 2;
uint8_t queueCtrSize = 1;
uint8_t poolCtrSize = 2;
uint8_t poolBlkSize = 2;
uint8_t tevtCtrSize = 2;
outFileName[0] = '\0'; // Output file not specified
savFileName[0] = '\0'; // Save file not specified
matFileName[0] = '\0'; // Matlab file not specified
time_t now = time(NULL);
printf("QSPY host application %s\n"
"Copyright (c) Quantum Leaps, LLC.\n"
"%s\n", QSPY_VER, ctime(&now));
static char const help[] =
"Syntax is: qspy [options] * = default\n\n"
"OPTION DEFAULT COMMENT\n"
"----------------------------------------------------------------\n"
"-h help (this message)\n"
"-q quiet mode (no stdout output)\n"
"-o<File_name> produce output to a file\n"
"-s<File_name> save the binary data to a file\n"
"-m<File_name> produce a Matlab file\n"
"-c<COM_port> * COM1 com port input\n"
"-b<Baud_rate> 38400 baud rate selection\n"
"-f<File_name> qs.bin file input\n"
"-t TCP/IP input\n"
"-p<TCP_Port_number> 6601 TCP/IP server port\n"
"-T<tstamp_size> 4 QS timestamp size (bytes)\n"
"-O<pointer_size> 4 object pointer size (bytes)\n"
"-F<pointer_size> 4 function pointer size (bytes)\n"
"-S<signal_size> 1 signal size (bytes)\n"
"-E<event_size> 2 event size size (bytes)\n"
"-Q<queue_counter_size> 1 queue counter size (bytes)\n"
"-P<pool_counter_size> 2 pool counter size (bytes)\n"
"-B<pool_blocksize_size> 2 pool blocksize size (bytes)\n"
"-C<QTimeEvt_counter_size> 2 QTimeEvt counter size\n";
while ((optChar = getopt(argc, argv,
"hqo:s:m:c:b:tp:f:T:O:F:S:E:Q:P:B:C:")) != -1)
{
switch (optChar) {
case 'q': { // quiet mode
quiet = 1;
break;
}
case 'o': { // file output
strncpy(outFileName, optarg, sizeof(outFileName));
printf("-o %s\n", outFileName);
break;
}
case 's': { // save binary data to a file
strncpy(savFileName, optarg, sizeof(savFileName));
printf("-s %s\n", savFileName);
break;
}
case 'm': { // Matlab file output
strncpy(matFileName, optarg, sizeof(matFileName));
printf("-m %s\n", matFileName);
break;
}
case 'c': { // COM port
if ((link != NO_LINK) && (link != SERIAL_LINK)) {
printf("The -c option is incompatible with -p/-f\n");
return -1; // failure
}
strncpy(comPort, optarg, sizeof(comPort));
printf("-c %s\n", comPort);
link = SERIAL_LINK;
break;
}
case 'b': { // baud rate
if ((link != NO_LINK) && (link != SERIAL_LINK)) {
printf("The -b option is incompatible with -p/-f\n");
return -1; // failure
}
if (sscanf(optarg, "%d", &baudRate) != 1) {
printf("incorrect baud rate: %s\n", optarg);
return -1; // failure
}
printf("-b %d\n", baudRate);
link = SERIAL_LINK;
break;
}
case 'f': { // File input
if (link != NO_LINK) {
printf("The -f option is incompatible with -c/-b/-p\n");
return -1; // failure
}
strncpy(inpFileName, optarg, sizeof(inpFileName));
printf("-f %s\n", inpFileName);
link = FILE_LINK;
break;
}
case 't': { // TCP/IP input
if ((link != NO_LINK) && (link != TCP_LINK)) {
printf("The -t option is incompatible with -c/-b/-f\n");
return -1;
}
printf("-t\n");
link = TCP_LINK;
break;
}
case 'p': { // TCP/IP port
if ((link != NO_LINK) && (link != TCP_LINK)) {
printf("The -p option is incompatible with -c/-b/-f\n");
return -1;
}
tcpPort = (int)strtoul(optarg, NULL, 10);
printf("-p %d\n", tcpPort);
link = TCP_LINK;
break;
}
case 'T': { // timestamp size
tstampSize = (uint8_t)strtoul(optarg, 0, 10);
break;
}
case 'F': { // function pointer size
funPtrSize = (uint8_t)strtoul(optarg, 0, 10);
break;
}
case 'O': { // object pointer size
objPtrSize = (uint8_t)strtoul(optarg, 0, 10);
break;
}
case 'S': { // signal size
sigSize = (uint8_t)strtoul(optarg, 0, 10);
break;
}
case 'E': { // event size
evtSize = (uint8_t)strtoul(optarg, 0, 10);
break;
}
case 'Q': { // Queue counter size
queueCtrSize = (uint8_t)strtoul(optarg, 0, 10);
break;
}
case 'P': { // Memory-pool counter size
poolCtrSize = (uint8_t)strtoul(optarg, 0, 10);
break;
}
case 'B': { // Memory-pool blocksize size
poolBlkSize = (uint8_t)strtoul(optarg, 0, 10);
break;
}
case 'C': { // Time event counter size
tevtCtrSize = (uint8_t)strtoul(optarg, 0, 10);
break;
}
case 'h': // help
default: { // unknown option
printf(help);
return -1; // error return
}
}
}
if (argc != optind) {
printf(help);
return -1;
}
// configure the Quantum Spy
FILE *outFile = (outFileName[0] != '\0'
? fopen(outFileName, "w")
: (FILE *)0);
FILE *matFile = (matFileName[0] != '\0'
? fopen(matFileName, "w")
: (FILE*)0);
FILE *savFile = (savFileName[0] != '\0'
? fopen(savFileName, "wb") // open for writing binary
: (FILE*)0);
qsConfig(quiet,
objPtrSize,
funPtrSize,
tstampSize,
sigSize,
evtSize,
queueCtrSize,
poolCtrSize,
poolBlkSize,
tevtCtrSize,
outFile,
matFile);
static unsigned char buf[BUF_SIZE];
int n;
switch (link) {
case NO_LINK: // intentionally fall through
case SERIAL_LINK: { // input trace data from the serial port
if (!comOpen(comPort, baudRate)) {
return -1;
}
else {
printf("\nSerial port %s opened, hit any key to quit...\n\n",
comPort);
}
while ((n = comRead(buf, sizeof(buf))) != -1) {
if (n > 0) {
qsParse(buf, n);
if (savFile != (FILE *)0) {
fwrite(buf, 1, n, savFile);
}
}
}
comClose();
break;
}
case FILE_LINK: { // input trace data from a file
FILE *f = fopen(inpFileName, "rb"); // open for reading binary
if (f == (FILE *)0) {
printf("file %s not found\n", inpFileName);
return -1;
}
do {
n = fread(buf, 1, sizeof(buf), f);
qsParse(buf, n);
} while (n == sizeof(buf));
fclose(f);
break;
}
case TCP_LINK: { // input trace data from the TCP port
if (!tcpOpen(tcpPort)) {
return -1;
}
else {
printf("\nTCP/IP port %d opened, "
"hit any key to quit...\n"
"(the target must be stopped)\n",
tcpPort);
}
while ((n = tcpRead(buf, sizeof(buf))) != -1) {
if (n > 0) {
qsParse(buf, n);
if (savFile != (FILE *)0) {
fwrite(buf, 1, n, savFile);
}
}
}
tcpClose();
break;
}
}
if (savFile != (FILE *)0) {
fclose(savFile);
}
if (matFile != (FILE *)0) {
fclose(matFile);
}
printf("\nDone.\n");
return 0; // success
}
