int modem_close(void)
{
time_t start;
conn_api.terminate=1;
if((comGetModemStatus(com)&COM_DCD)==0) /* DCD already low */
goto CLOSEIT;
/* TODO: We need a drain function */
SLEEP(500);
if(!comLowerDTR(com))
goto CLOSEIT;
start=time(NULL);
while(time(NULL)-start <= 10) {
if((comGetModemStatus(com)&COM_DCD) == 0)
goto CLOSEIT;
SLEEP(1000);
}
CLOSEIT:
while(conn_api.input_thread_running || conn_api.output_thread_running)
SLEEP(1);
comClose(com);
destroy_conn_buf(&conn_inbuf);
destroy_conn_buf(&conn_outbuf);
FREE_AND_NULL(conn_api.rd_buf);
FREE_AND_NULL(conn_api.wr_buf);
return(0);
}
void cleanup(void)
{
terminated=TRUE;
lprintf(LOG_INFO,"Cleaning up ...");
if(com_handle!=COM_HANDLE_INVALID) {
if(!mdm_null && mdm_cleanup[0])
modem_command(com_handle, mdm_cleanup);
if(!com_handle_passed)
comClose(com_handle);
}
close_socket(&sock);
#ifdef _WINSOCKAPI_
WSACleanup();
#endif
lprintf(LOG_INFO,"Done (handled %lu calls).", total_calls);
#if defined(_WIN32)
if(daemonize && svc_status_handle!=0) {
svc_status.dwCurrentState=SERVICE_STOPPED;
SetServiceStatus(svc_status_handle, &svc_status);
} else
#endif
if(pause_on_exit) {
printf("Hit enter to continue...");
getchar();
}
}
void Opa::disconnect()
{
state = OPA_DISCONNECTED;
if (port == -1) return;
comClose(port);
port = -1;
}
int serial_close(void)
{
conn_api.terminate=1;
while(conn_api.input_thread_running || conn_api.output_thread_running)
SLEEP(1);
comClose(com);
destroy_conn_buf(&conn_inbuf);
destroy_conn_buf(&conn_outbuf);
FREE_AND_NULL(conn_api.rd_buf);
FREE_AND_NULL(conn_api.wr_buf);
return(0);
}
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;
}
int comOpen(int index, int baudrate)
{
if (index >= noDevices || index < 0)
return 0;
// Close if already open
COMDevice * com = &comDevices[index];
if (com->handle >= 0) comClose(index);
// Open port
int handle = open(comGetInternalName(index), O_RDWR | O_NOCTTY | O_NDELAY);
if (handle < 0) {
printf("Error writing to %s (%i)(%s) \n", comGetInternalName(index), errno,strerror(errno));
return 0;
}
// General configuration
struct termios config;
memset(&config, 0, sizeof(config));
tcgetattr(handle, &config);
config.c_iflag &= ~(INLCR | ICRNL);
config.c_iflag |= IGNPAR | IGNBRK;
config.c_oflag &= ~(OPOST | ONLCR | OCRNL);
config.c_cflag &= ~(PARENB | PARODD | CSTOPB | CSIZE | CRTSCTS);
config.c_cflag |= CLOCAL | CREAD | CS8;
config.c_lflag &= ~(ICANON | ISIG | ECHO);
int flag = _BaudFlag(baudrate);
cfsetospeed(&config, flag);
cfsetispeed(&config, flag);
// Timeouts configuration
config.c_cc[VTIME] = 1;
config.c_cc[VMIN] = 0;
//fcntl(handle, F_SETFL, FNDELAY);
// Validate configuration
if (tcsetattr(handle, TCSANOW, &config) < 0) {
close(handle);
return 0;
}
com->handle = handle;
return 1;
}
void comCloseAll()
{
for (int i = 0; i < noDevices; i++)
comClose(i);
}
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
}
