ypprotocol.c
Go to the documentation of this file.
00001 // Copyright (c) 2010-2016 The YP-Spur Authors, except where otherwise indicated.
00002 //
00003 // Permission is hereby granted, free of charge, to any person obtaining a copy
00004 // of this software and associated documentation files (the "Software"), to
00005 // deal in the Software without restriction, including without limitation the
00006 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
00007 // sell copies of the Software, and to permit persons to whom the Software is
00008 // furnished to do so, subject to the following conditions:
00009 //
00010 // The above copyright notice and this permission notice shall be included in
00011 // all copies or substantial portions of the Software.
00012 //
00013 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00014 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00015 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
00016 // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00017 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00018 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
00019 // SOFTWARE.
00020 
00021 #include <stdio.h>
00022 #include <stdlib.h>
00023 #include <string.h>
00024 #include <sys/time.h>
00025 #include <sys/types.h>
00026 #include <unistd.h>
00027 
00028 #include <errno.h>
00029 
00030 #ifdef HAVE_CONFIG_H
00031 #include <config.h>
00032 #endif  // HAVE_CONFIG_H
00033 
00034 #include <communication.h>
00035 #include <ypprotocol.h>
00036 #include <serial.h>
00037 #include <utility.h>
00038 #include <yprintf.h>
00039 
00040 #define RECEIVE_BUFFER_SIZE 2048
00041 
00042 int ss_receive(char *buf, int len, double receive_time, void *data)
00043 {
00044   buf[len] = 0;
00045   if (len + strlen((char *)data) > RECEIVE_BUFFER_SIZE)
00046   {
00047     return -3;
00048   }
00049   strcat((char *)data, buf);
00050   if (strstr((char *)data, "\n\n"))
00051   {
00052     return -2;
00053   }
00054   return 0;
00055 }
00056 
00057 int set_baudrate(int baud)
00058 {
00059   /* Send & Recive Buffer */
00060   char buf[RECEIVE_BUFFER_SIZE];
00061   /* Temporary */
00062 
00063   strcpy(buf, "\n\n\n\n");
00064   serial_write(buf, strlen(buf));
00065   yp_usleep(50000);
00066   serial_flush_in();
00067   yp_usleep(50000);
00068   sprintf(buf, "SS%06d\n", baud);
00069   serial_write(buf, strlen(buf));
00070 
00071   buf[0] = 0;
00072   if (serial_recieve(ss_receive, buf) != -2)
00073     return 0;
00074 
00075   if (strstr(buf, "\n00P\n") != NULL)
00076   {
00077     yp_usleep(100000);
00078     return serial_change_baudrate(baud);
00079   }
00080   // RS-232cインターフェース未搭載
00081   else if (strstr(buf, "\n04T\n") != NULL)
00082   {
00083     SER_BAUDRATE = 0;
00084     yp_usleep(100000);
00085     return 4;
00086   }
00087 
00088   return 0;
00089 }
00090 
00091 int vv_receive(char *buf, int len, double receive_time, void *data)
00092 {
00093   buf[len] = 0;
00094   if (len + strlen((char *)data) > RECEIVE_BUFFER_SIZE)
00095   {
00096     return -3;
00097   }
00098   strcat((char *)data, buf);
00099   if (strstr((char *)data, "\n\n"))
00100   {
00101     return -2;
00102   }
00103   return 0;
00104 }
00105 
00111 int get_version(Ver_t *apVer)
00112 {
00113   /* Send & Recive Buffer */
00114   char buf[RECEIVE_BUFFER_SIZE], *readpos;
00115   /* Temporary */
00116   char *tmp, *lf, *val;
00117   char *tag, *wbuf;
00118 
00119   readpos = buf;
00120   memset(apVer, 0, sizeof(Ver_t));
00121 
00122   strcpy(buf, "\n\n\n\n");
00123   serial_write(buf, strlen(buf));
00124   yp_usleep(50000);
00125   serial_flush_in();
00126   yp_usleep(50000);
00127   strcpy(buf, "VV\n");
00128   serial_write(buf, strlen(buf));
00129 
00130   buf[0] = 0;
00131   if (serial_recieve(vv_receive, buf) != -2)
00132     return 0;
00133   if (strstr(buf, "\n00P\n") == 0)
00134     return 0;
00135 
00136   while (1)
00137   {
00138     if ((lf = strchr(readpos, '\n')) == NULL)
00139       break;
00140     *lf = 0;
00141     if ((tag = strstr(readpos, "VEND:")) != 0)
00142     {
00143       wbuf = (apVer->vender);
00144     }
00145     else if ((tag = strstr(readpos, "PROD:")) != 0)
00146     {
00147       wbuf = (apVer->product);
00148     }
00149     else if ((tag = strstr(readpos, "FIRM:")) != 0)
00150     {
00151       wbuf = (apVer->firmware);
00152     }
00153     else if ((tag = strstr(readpos, "PROT:")) != 0)
00154     {
00155       wbuf = (apVer->protocol);
00156     }
00157     else if ((tag = strstr(readpos, "SERI:")) != 0)
00158     {
00159       wbuf = (apVer->serialno);
00160     }
00161     else
00162     {
00163       readpos = lf + 1;
00164       continue;
00165     }
00166     tmp = strchr(tag, ';');
00167     val = strchr(tag, ':');
00168     if (tmp && val)
00169     {
00170       *tmp = 0;
00171       if (wbuf)
00172       {
00173         strcpy(wbuf, val + 1);
00174       }
00175     }
00176     readpos = lf + 1;
00177   }
00178   return 1;
00179 }
00180 
00186 int get_parameter(Param_t *apParam)
00187 {
00188   /* Send & Recive Buffer */
00189   char buf[RECEIVE_BUFFER_SIZE], *readpos;
00190   /* Temporary */
00191   char *tmp, *lf, *val;
00192   char *tag, *wbuf;
00193 
00194   readpos = buf;
00195   memset(apParam, 0, sizeof(Param_t));
00196 
00197   strcpy(buf, "\n\n\n\n");
00198   serial_write(buf, strlen(buf));
00199   yp_usleep(50000);
00200   serial_flush_in();
00201   yp_usleep(50000);
00202   strcpy(buf, "PP\n");
00203   serial_write(buf, strlen(buf));
00204 
00205   buf[0] = 0;
00206   if (serial_recieve(vv_receive, buf) != -2)
00207     return 0;
00208   if (strstr(buf, "\n00P\n") == 0)
00209     return 0;
00210 
00211   while (1)
00212   {
00213     if ((lf = strchr(readpos, '\n')) == NULL)
00214       break;
00215     *lf = 0;
00216     if ((tag = strstr(readpos, "PWMRES:")) != 0)
00217     {
00218       wbuf = (apParam->pwm_resolution);
00219     }
00220     else if ((tag = strstr(readpos, "MOTORNUM:")) != 0)
00221     {
00222       wbuf = (apParam->motor_num);
00223     }
00224     else if ((tag = strstr(readpos, "NAME:")) != 0)
00225     {
00226       wbuf = (apParam->robot_name);
00227     }
00228     else
00229     {
00230       readpos = lf + 1;
00231       continue;
00232     }
00233     tmp = strchr(tag, ';');
00234     val = strchr(tag, ':');
00235     if (tmp && val)
00236     {
00237       *tmp = 0;
00238       if (wbuf)
00239       {
00240         strcpy(wbuf, val + 1);
00241       }
00242     }
00243     readpos = lf + 1;
00244   }
00245   return 1;
00246 }
00247 
00248 int get_embedded_param(char *param)
00249 {
00250   /* Send & Recive Buffer */
00251   char buf[RECEIVE_BUFFER_SIZE], *readpos;
00252   /* Temporary */
00253   char *lf;
00254 
00255   readpos = buf;
00256   memset(buf, 0, sizeof(buf));
00257 
00258   strcpy(buf, "\n\n\n\n");
00259   serial_write(buf, strlen(buf));
00260   yp_usleep(50000);
00261   serial_flush_in();
00262   yp_usleep(50000);
00263   strcpy(buf, "GETEMBEDDEDPARAM\n");
00264   serial_write(buf, strlen(buf));
00265 
00266   buf[0] = 0;
00267   if (serial_recieve(vv_receive, buf) != -2)
00268     return 0;
00269   if (strstr(buf, "\n00P\n") == 0)
00270     return 0;
00271 
00272   while (1)
00273   {
00274     if ((lf = strchr(readpos, '\n')) == NULL)
00275       break;
00276     *lf = 0;
00277     if (strstr(readpos, "00P") != 0)
00278     {
00279       lf++;
00280       break;
00281     }
00282     readpos = lf + 1;
00283   }
00284   if (lf == NULL)
00285     return 0;
00286   strcpy(param, lf);
00287   return 1;
00288 }


yp-spur
Author(s):
autogenerated on Fri May 10 2019 02:52:19