00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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
00060 char buf[RECEIVE_BUFFER_SIZE];
00061
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
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
00114 char buf[RECEIVE_BUFFER_SIZE], *readpos;
00115
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
00189 char buf[RECEIVE_BUFFER_SIZE], *readpos;
00190
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
00251 char buf[RECEIVE_BUFFER_SIZE], *readpos;
00252
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 }