dynpick_driver.cpp
Go to the documentation of this file.
00001 /*
00002  * This program is based on http://github.com/tork-a/dynpick_driver and http://github.com/start-jsk/rtmros_hironx/blob/indigo-devel/hironx_ros_bridge/robot/nitta/jr3_driver.cpp
00003 
00004   This driver is stored in this robot-specific package for not many reasons than they are slightly customized for the robot (as of Apr 2016 it takes 2 sensor inputs in a single cpp file. It also assumes the specific device file name). So if you can separate those as a standalone, generic package that'll be appreciated (please just let us know if you will at https://github.com/start-jsk/rtmros_hironx/issues).
00005 */
00006 /*
00007  * This program is for Dynpick F/T sensor (developed from JR3 sensor).
00008  * Copyright(C) by Waseda University, Nitta Coropration. 2002.
00009  *
00010  * Copyright (c) 2016, TORK (Tokyo Opensource Robotics Kyokai Association)
00011  * All rights reserved.
00012  * # Redistribution and use in source and binary forms, with or without
00013  * modification, are permitted provided that the following conditions
00014  * are met:
00015  * #  * Redistributions of source code must retain the above copyright
00016  *    notice, this list of conditions and the following disclaimer.
00017  *  * Redistributions in binary form must reproduce the above
00018  *    copyright notice, this list of conditions and the following
00019  *    disclaimer in the documentation and/or other materials provided
00020  *    with the distribution.
00021  *  * Neither the name of TOKYO. nor the names of its contributors may be
00022  *    used to endorse or promote products derived from this software without
00023  *    specific prior written permission.
00024  * # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
00036  */
00037 
00038 #include <errno.h>
00039 #include <fcntl.h>
00040 #include <stdio.h>
00041 #include <stdlib.h>
00042 #include <string.h>
00043 #include <sys/iofunc.h>
00044 #include <sys/dispatch.h>
00045 #include <sys/mman.h>
00046 #include <sys/neutrino.h>
00047 #include <sys/slog.h>
00048 #include <sys/time.h>
00049 #include <termios.h>
00050 #include <unistd.h>
00051 
00052 unsigned short force_sensor_data_0[6];
00053 unsigned short force_sensor_data_1[6];
00054 
00055 /*
00056  * serial stuff
00057  */
00058 
00059 #define cfsetspeed(term, baudrate)\
00060    cfsetispeed(term, baudrate);\
00061    cfsetospeed(term, baudrate);
00062 
00063 int SetComAttr(int fdc) {
00064         int res = -1;
00065         if (fdc < 0) {
00066                 char *pmesg = strerror(errno);
00067                 fprintf(stderr, "failed to open : %s\n", pmesg);
00068                 goto over;
00069         }
00070 
00071         struct termios term;
00072         res = tcgetattr(fdc, &term);
00073 
00074         if (res < 0) {
00075                 char *pmesg = strerror(errno);
00076                 fprintf(stderr, "failed to tcgetattr(): %s\n", pmesg);
00077                 goto over;
00078         }
00079         cfmakeraw(&term);
00080         res = cfsetspeed(&term, 921600)
00081         ;
00082         if (res < 0) {
00083 
00084                 char *pmesg = strerror(errno);
00085                 fprintf(stderr, "failed to cfsetspeed(): %s\n", pmesg);
00086                 goto over;
00087         }
00088 
00089          // settings for qnx
00090          term.c_iflag |= IGNPAR;            // Ignore characters with parity errors
00091          term.c_cflag |= (CLOCAL | CREAD);  // needed for QNX 6.3.2
00092          term.c_cflag &= ~PARENB;           // disable parity check
00093          term.c_cflag |= CS8;               // 8 data bit
00094          term.c_cflag &= ~CSTOPB;           // 1 stop bit
00095          //term.c_lflag = IEXTEN;
00096          term.c_oflag = 0; 
00097          term.c_lflag &= ~(ECHO | ECHOCTL | ECHONL);  // disable ECHO
00098            
00099          //
00100          term.c_iflag &= ~INPCK;
00101 
00102          // settings for dynpick
00103            term.c_cc[VINTR]    = 0;     /* Ctrl-c */
00104            term.c_cc[VQUIT]    = 0;     /* Ctrl-? */       term.c_cc[VERASE]   = 0;     /* del */
00105            term.c_cc[VKILL]    = 0;     /* @ */
00106            term.c_cc[VEOF]     = 4;     /* Ctrl-d */
00107            term.c_cc[VTIME]    = 0;
00108            term.c_cc[VMIN]     = 0;
00109            //term.c_cc[VSWTC]    = 0;     /* '?0' */
00110            term.c_cc[VSTART]   = 0;     /* Ctrl-q */ 
00111            term.c_cc[VSTOP]    = 0;     /* Ctrl-s */
00112            term.c_cc[VSUSP]    = 0;     /* Ctrl-z */
00113            term.c_cc[VEOL]     = 0;     /* '?0' */
00114            term.c_cc[VREPRINT] = 0;     /* Ctrl-r */
00115            term.c_cc[VDISCARD] = 0;     /* Ctrl-u */
00116            term.c_cc[VWERASE]  = 0;     /* Ctrl-w */
00117            term.c_cc[VLNEXT]   = 0;     /* Ctrl-v */
00118            term.c_cc[VEOL2]    = 0;     /* '?0' */
00119 #ifdef __QNX__
00120         term.c_cflag &= ~(IHFLOW | OHFLOW);
00121 #endif
00122 
00123         // settings for qnx
00124         term.c_cc[VMIN] = 100;
00125         term.c_cc[VTIME] = 0;
00126 
00127         res = tcsetattr(fdc, TCSANOW, &term);
00128         if (res < 0) {
00129                 char *pmesg = strerror(errno);
00130                 fprintf(stderr, "failed to tcsetattr(): %s\n", pmesg);
00131                 goto over;
00132         }
00133 
00134         over: return (res);
00135 }
00136 
00137 int ReadCom(int fdc, unsigned short *data) {
00138         int tick;
00139         char str[255];
00140         int n, len;
00141 
00142 #define DATA_LENGTH 27
00143         if (fdc < 0)
00144                 return DATA_LENGTH; // dummy data
00145 
00146         // Data Request
00147         n = write(fdc, "R", 1);
00148         //tcdrain(fdc);  // Old legacy that is no longer needed. Besideds that, on QNX6.5.0 this is found taking unnecessarily too long (say 100msec).
00149         printf("write data ret=%d, fd=%d\n", n, fdc);
00150 
00151         // Get Singale data
00152         len = 0;
00153         bzero(str, 27);
00154         while (len < DATA_LENGTH) {
00155                 n = read(fdc, str + len, DATA_LENGTH - len);
00156                 printf("read data %d (%d)\n", n, len);
00157                 if (n > 0) {
00158                         len += n;
00159                 } else {
00160                         char *pmesg = strerror(errno);
00161                         fprintf(stderr, "failed to read data (ret=%d, fd=%d): %s (%d)\n", n,
00162                                         fdc, pmesg, errno);
00163                         goto loop_exit;
00164                 }
00165         }
00166         loop_exit: {
00167                 int i;
00168                 for (i = 0; i < DATA_LENGTH; i++) {
00169                         fprintf(stderr, "%02x:", 0x0000ff & str[i]);
00170                 }
00171                 fprintf(stderr, "\n");
00172         }
00173 
00174         sscanf(str, "%1d%4hx%4hx%4hx%4hx%4hx%4hx", &tick, &data[0], &data[1],
00175                         &data[2], &data[3], &data[4], &data[5]);
00176 
00177         sprintf(str, "%d,%05d,%05d,%05d,%05d,%05d,%05d\n", tick, data[0], data[1],
00178                         data[2], data[3], data[4], data[5]);
00179 
00180         return len;
00181 }
00182 
00183 /*
00184  * message stuff
00185  */
00186 
00187 #define min(a, b) ((a) < (b) ? (a) : (b))
00188 
00189 int io_read(resmgr_context_t * ctp, io_read_t * msg, RESMGR_OCB_T * ocb);       //
00190 static char *buffer = (char *) "Hello world\n"; //
00191 
00192 void wait_t(void);              // wait for 3 sec.
00193 
00194 static resmgr_connect_funcs_t ConnectFuncs;     //
00195 static resmgr_io_funcs_t IoFuncs;       //
00196 static iofunc_attr_t IoFuncAttr;        //
00197 
00198 typedef struct {
00199         uint16_t msg_no;
00200         char msg_data[255];
00201 } server_msg_t;
00202 
00203 int message_callback(message_context_t * ctp, int type, unsigned flags,
00204                 void *handle) {
00205         server_msg_t *msg;
00206         int num;
00207         char msg_reply[255];
00208 
00209         /* cast a pointer to the message data */
00210         msg = (server_msg_t *) ctp->msg;
00211 
00212         /* Print out some usefull information on the message */
00213         //printf( "\n\nServer Got Message:\n" );
00214         //printf( "  type: %d\n" , type );
00215         //printf( "  data: %s\n\n", msg->msg_data );
00216 #if 0
00217         force_sensor_data *data_0;
00218         force_sensor_data *data_1;
00219         data_0 = (force_sensor_data *) (Jr3BaseAddress0H + (Jr3DmAddrMask << 2));
00220         data_1 = (force_sensor_data *) (Jr3BaseAddress1H + (Jr3DmAddrMask << 2));
00221 #endif
00222         /* Build the reply message */
00223         num = type - _IO_MAX;
00224         switch (num) {
00225         case 1:                 // get data
00226         {
00227 #if 0  // FIX ME
00228                 float tmp[12] = {
00229                         -1.0 * (float) data_0->filter0.fx / (float) data_0->full_scale.fx,
00230                         -1.0 * (float) data_0->filter0.fy / (float) data_0->full_scale.fy,
00231                         -1.0 * (float) data_0->filter0.fz / (float) data_0->full_scale.fz,
00232                         -1.0 * (float) data_0->filter0.mx / (float) data_0->full_scale.mx * 0.1, // Newton*meter*10
00233                         -1.0 * (float) data_0->filter0.my / (float) data_0->full_scale.my * 0.1,
00234                         -1.0 * (float) data_0->filter0.mz / (float) data_0->full_scale.mz * 0.1,
00235                         -1.0 * (float) data_1->filter0.fx / (float) data_1->full_scale.fx,
00236                         -1.0 * (float) data_1->filter0.fy / (float) data_1->full_scale.fy,
00237                         -1.0 * (float) data_1->filter0.fz / (float) data_1->full_scale.fz,
00238                         -1.0 * (float) data_1->filter0.mx / (float) data_1->full_scale.mx * 0.1,
00239                         -1.0 * (float) data_1->filter0.my / (float) data_1->full_scale.my * 0.1,
00240                         -1.0 * (float) data_1->filter0.mz / (float) data_1->full_scale.mz * 0.1
00241                 };
00242 #endif   
00243                 float tmp[12] = { (force_sensor_data_0[0] - 8192) / 1000.0,
00244                                 (force_sensor_data_0[1] - 8192) / 1000.0,
00245                                 (force_sensor_data_0[2] - 8192) / 1000.0,
00246                                 (force_sensor_data_0[3] - 8192) / 1000.0,
00247                                 (force_sensor_data_0[4] - 8192) / 1000.0,
00248                                 (force_sensor_data_0[5] - 8192) / 1000.0,
00249                                 (force_sensor_data_1[0] - 8192) / 1000.0,
00250                                 (force_sensor_data_1[1] - 8192) / 1000.0,
00251                                 (force_sensor_data_1[2] - 8192) / 1000.0,
00252                                 (force_sensor_data_1[3] - 8192) / 1000.0,
00253                                 (force_sensor_data_1[4] - 8192) / 1000.0,
00254                                 (force_sensor_data_1[5] - 8192) / 1000.0, };
00255                 memcpy(msg_reply, tmp, sizeof(float) * 12);
00256 
00257         }
00258                 break;
00259         case 2:                 // update offset
00260 #if 0 // FIX ME
00261         data_0->offsets.fx += data_0->filter0.fx;
00262         data_0->offsets.fy += data_0->filter0.fy;
00263         data_0->offsets.fz += data_0->filter0.fz;
00264         data_0->offsets.mx += data_0->filter0.mx;
00265         data_0->offsets.my += data_0->filter0.my;
00266         data_0->offsets.mz += data_0->filter0.mz;
00267         data_1->offsets.fx += data_1->filter0.fx;
00268         data_1->offsets.fy += data_1->filter0.fy;
00269         data_1->offsets.fz += data_1->filter0.fz;
00270         data_1->offsets.mx += data_1->filter0.mx;
00271         data_1->offsets.my += data_1->filter0.my;
00272         data_1->offsets.mz += data_1->filter0.mz;
00273 #endif       
00274                 break;
00275         case 3:                 // set filter
00276                 break;
00277         case 4:                 // get data
00278 #if 0        // FIX ME
00279         get_force_sensor_info (data_0, msg_reply);
00280         get_force_sensor_info (data_1, msg_reply + strlen (msg_reply));
00281 #endif       
00282                 break;
00283         }
00284 
00285         /* Send a reply to the waiting (blocked) client */
00286         MsgReply(ctp->rcvid, EOK, msg_reply, 255);
00287         return 0;
00288 }
00289 
00290 int main(int argc, char **argv) {
00291         resmgr_attr_t resmgr_attr;
00292         message_attr_t message_attr;
00293         dispatch_t *dpp;
00294         dispatch_context_t *ctp, *ctp_ret;
00295         int resmgr_id, message_id;
00296         int fd1, fd2;
00297 
00298         /* Open Serial port */
00299         fd1 = open("/dev/serusb1", O_RDWR | O_NOCTTY | O_NONBLOCK);
00300         if (fd1 < 0) {
00301                 fprintf(stderr, "could not open /dev/serusb1\n");
00302         }
00303         fd2 = open("/dev/serusb2", O_RDWR | O_NOCTTY | O_NONBLOCK);
00304         if (fd2 < 0) {
00305                 fprintf(stderr, "could not open /dev/serusb2\n");
00306         }
00307         #ifdef __QNX__ 
00308             slogf(0, _SLOG_INFO, "Started  fd1 = %d, fd2 = %d\n", fd1, fd2);
00309         #endif 
00310         SetComAttr(fd1);
00311         SetComAttr(fd2);
00312 
00313         /* Create the Dispatch Interface */
00314         dpp = dispatch_create();
00315         if (dpp == NULL) {
00316                 fprintf(stderr, "dispatch_create() failed: %s\n", strerror(errno));
00317                 return EXIT_FAILURE;
00318         }
00319 
00320         memset(&resmgr_attr, 0, sizeof(resmgr_attr));
00321         resmgr_attr.nparts_max = 1;
00322         resmgr_attr.msg_max_size = 2048;
00323 
00324         /* Setup the default I/O functions to handle open/read/write/... */
00325         iofunc_func_init(_RESMGR_CONNECT_NFUNCS, &ConnectFuncs, _RESMGR_IO_NFUNCS,
00326                         &IoFuncs);
00327 
00328         IoFuncs.read = io_read;
00329 
00330         /* Setup the attribute for the entry in the filesystem */
00331         iofunc_attr_init(&IoFuncAttr, S_IFNAM | 0666, 0, 0);
00332         IoFuncAttr.nbytes = strlen(buffer) + 1; //
00333 
00334         resmgr_id = resmgr_attach(dpp, &resmgr_attr, "/dev/jr3q", _FTYPE_ANY, 0,
00335                         &ConnectFuncs, &IoFuncs, &IoFuncAttr);
00336         if (resmgr_id == -1) {
00337                 fprintf(stderr, "resmgr_attach() failed: %s\n", strerror(errno));
00338                 return EXIT_FAILURE;
00339         }
00340 
00341         /* Setup Serial Port */
00342 
00343         /* Setup our message callback */
00344         memset(&message_attr, 0, sizeof(message_attr));
00345         message_attr.nparts_max = 1;
00346         message_attr.msg_max_size = 4096;
00347 
00348         /* Attach a callback (handler) for two message types */
00349         message_id = message_attach(dpp, &message_attr, _IO_MAX + 1, _IO_MAX + 10,
00350                         message_callback, NULL);
00351         if (message_id == -1) {
00352                 fprintf(stderr, "message_attach() failed: %s\n", strerror(errno));
00353                 return EXIT_FAILURE;
00354         }
00355 
00356         /* Setup a context for the dispatch layer to use */
00357         ctp = dispatch_context_alloc(dpp);
00358         if (ctp == NULL) {
00359                 fprintf(stderr, "dispatch_context_alloc() failed: %s\n",
00360                                 strerror(errno));
00361                 return EXIT_FAILURE;
00362         }
00363 
00364         /* The "Data Pump" - get and process messages */
00365         while (1) {
00366                 /* do serial read */
00367                 ReadCom(fd1, force_sensor_data_0);
00368                 ReadCom(fd2, force_sensor_data_1);
00369 
00370                 printf("dispatch\n");
00371                 /* process message */
00372                 ctp_ret = dispatch_block(ctp);
00373                 if (ctp_ret) {
00374                         dispatch_handler(ctp);
00375                 } else {
00376                         fprintf(stderr, "dispatch_block() failed: %s\n", strerror(errno));
00377                         return EXIT_FAILURE;
00378                 }
00379         }
00380 
00381         return EXIT_SUCCESS;
00382 }
00383 
00384 int io_read(resmgr_context_t * ctp, io_read_t * msg, RESMGR_OCB_T * ocb) {
00385         int nleft;
00386         int nbytes;
00387         int nparts;
00388         int status;
00389 
00390         if ((status = iofunc_read_verify(ctp, msg, ocb, NULL)) != EOK)
00391                 return (status);
00392 
00393         if ((msg->i.xtype & _IO_XTYPE_MASK) != _IO_XTYPE_NONE)
00394                 return (ENOSYS);
00395 
00396         /*
00397          * on all reads (first and subsequent) calculate
00398          * how many bytes we can return to the client,
00399          * based upon the number of bytes available (nleft)
00400          * and the client's buffer size
00401          */
00402 
00403         nleft = ocb->attr->nbytes - ocb->offset;
00404         nbytes = min(msg->i.nbytes, nleft);
00405 
00406         if (nbytes > 0) {
00407                 /* set up the return data IOV */
00408                 SETIOV(ctp->iov, buffer + ocb->offset, nbytes);
00409 
00410                 /* set up the number of bytes (returned by client's read()) */
00411                 _IO_SET_READ_NBYTES(ctp, nbytes);
00412 
00413                 /*
00414                  * advance the offset by the number of bytes
00415                  * returned to the client.
00416                  */
00417 
00418                 ocb->offset += nbytes;
00419                 nparts = 1;
00420         } else {
00421                 /* they've adked for zero bytes or they've already previously
00422                  * read everything
00423                  */
00424 
00425                 _IO_SET_READ_NBYTES(ctp, 0);
00426                 nparts = 0;
00427         }
00428 
00429         /* mark the access time as invalid (we just accessed it) */
00430 
00431         if (msg->i.nbytes > 0)
00432                 ocb->attr->flags |= IOFUNC_ATTR_ATIME;
00433 
00434         return (_RESMGR_NPARTS(nparts));
00435 
00436 }
00437 
00438 void wait_t(void) {
00439         struct timeval tv, tv1;
00440 
00441         gettimeofday(&tv1, NULL);
00442 
00443         do {
00444                 gettimeofday(&tv, NULL);
00445         } while (tv.tv_sec - tv1.tv_sec < 3);   // wait for 3 sec.
00446 
00447         return;
00448 }


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu Feb 21 2019 03:42:37