tool/ico_set_vehicleinfo.c

   1 /*
   2  * Copyright (c) 2013, TOYOTA MOTOR CORPORATION.
   3  *
   4  * This program is licensed under the terms and conditions of the
   5  * Apache License, version 2.0.  The full text of the Apache License is at
   6  * http://www.apache.org/licenses/LICENSE-2.0
   7  *
   8  */
   9 /**
  10  * @brief   System Test Tool for set VehicleInfo
  11  *
  12  * @date    Apr-09-2013
  13  */
  14
  15 #include    <stdio.h>
  16 #include    <stdlib.h>
  17 #include    <unistd.h>
  18 #include    <string.h>
  19 #include    <errno.h>
  20 #include    <pthread.h>
  21 #include    <sys/ioctl.h>
  22 #include    <sys/ipc.h>
  23 #include    <sys/msg.h>
  24 #include    <sys/time.h>
  25 #include    <sys/types.h>
  26 #include    <ico_uws.h>
  27
  28 #define TYPE_NULL   0
  29 #define TYPE_BOOL   1
  30 #define TYPE_BYTE   2
  31 #define TYPE_INT16  3
  32 #define TYPE_UINT16 4
  33 #define TYPE_INT32  5
  34 #define TYPE_UINT32 6
  35 #define TYPE_DOUBLE 7
  36 #define TYPE_STRING 8
  37 #define TYPE_SHIFT  12
  38
  39 #define LWS_DEFAULTIP       "127.0.0.1" /* connection default ip(localhost) */
  40 #define LWS_DEFAULTPORT     25010       /* connection default port          */
  41 #define LWS_PROTOCOLNAME    "standarddatamessage-only"
  42                                         /* connection protocol name         */
  43 static const struct {
  44     char        *prop;
  45     char        *eventtype;
  46     unsigned char   datatype[4];
  47 }               vehicleinfo_key[] = {
  48     { "VehicleSpeed", "VELOCITY", {TYPE_INT32, TYPE_NULL, 0,0} },
  49     { "Speed", "VELOCITY", {TYPE_INT32, TYPE_NULL, 0,0} },
  50     { "Velocity", "VELOCITY", {TYPE_INT32, TYPE_NULL, 0,0} },
  51     { "Location", "LOCATION", {TYPE_DOUBLE, TYPE_DOUBLE, TYPE_DOUBLE, TYPE_NULL} },
  52     { "Direction", "DIRECTION", {TYPE_INT32, TYPE_NULL, 0,0} },
  53     { "EngineSpeed", "ENGINE_SPEED", {TYPE_INT32, TYPE_NULL, 0, 0} },
  54     { "Engine", "ENGINE_SPEED", {TYPE_INT32, TYPE_NULL, 0, 0} },
  55     { "Shift", "SHIFT", {TYPE_SHIFT, TYPE_BYTE, TYPE_NULL, 0} },
  56     { "ShiftPosition", "SHIFT", {TYPE_SHIFT, TYPE_BYTE, TYPE_NULL, 0} },
  57     { "Break_Signal", "BRAKE_SIGNAL", {TYPE_BOOL, TYPE_NULL, 0,0} },
  58     { "BreakSignal", "BRAKE_SIGNAL", {TYPE_BOOL, TYPE_NULL, 0,0} },
  59     { "Break", "BRAKE_SIGNAL", {TYPE_BOOL, TYPE_NULL, 0,0} },
  60     { "Blinker", "TURN_SIGNAL", {TYPE_INT32, TYPE_NULL, 0, 0} },
  61     { "Winker", "TURN_SIGNAL", {TYPE_INT32, TYPE_NULL, 0, 0} },
  62     { "TurnSignal", "TURN_SIGNAL", {TYPE_INT32, TYPE_NULL, 0, 0} },
  63     { "Turn", "TURN_SIGNAL", {TYPE_INT32, TYPE_NULL, 0, 0} },
  64     { "lightStatus", "LIGHTSTATUS", {TYPE_BOOL, TYPE_BOOL, TYPE_BOOL, TYPE_BOOL} },
  65     { "light", "LIGHTSTATUS", {TYPE_BOOL, TYPE_BOOL, TYPE_BOOL, TYPE_BOOL} },
  66     { "WATER_TEMP", "WATER_TEMP", {TYPE_INT32, TYPE_NULL, 0, 0} },
  67     { "EXTERIORBRIGHTNESS", "EXTERIORBRIGHTNESS", {TYPE_UINT16, TYPE_NULL, 0, 0} },
  68     { "EXTERIOR", "EXTERIORBRIGHTNESS", {TYPE_UINT16, TYPE_NULL, 0, 0} },
  69     { "\0", "\0", {TYPE_NULL, 0,0,0} } };
  70
  71 struct KeyDataMsg_t
  72 {
  73     char KeyEventType[64];
  74     struct timeval recordtime;
  75     struct KeyData
  76     {
  77         int common_status;
  78         char status[];
  79     } data;
  80 };
  81
  82 static struct ico_uws_context   *context = NULL;
  83                                         /* connection context               */
  84 static void                     *connect_id = NULL;
  85                                         /* connection connection id         */
  86 static int  connected = 0;              /* connection flag                  */
  87
  88 static void uws_callback(const struct ico_uws_context *context,
  89                          const ico_uws_evt_e reason, const void *id,
  90                          const ico_uws_detail *detail, void *user_data);
  91
  92 static void
  93 uws_callback(const struct ico_uws_context *context, const ico_uws_evt_e reason,
  94              const void *id, const ico_uws_detail *detail, void *user_data)
  95 {
  96     if (reason == ICO_UWS_EVT_OPEN)  {
  97         connect_id = (void *)id;
  98         connected = 1;
  99     }
 100     else if (reason == ICO_UWS_EVT_ERROR)   {
 101         fprintf(stderr, "Communication Error[%d]\n", detail->_ico_uws_error.code);
 102     }
 103     /* do nothing       */
 104 }
 105
 106 static void
 107 init_comm(const int port, const char *spadr)
 108 {
 109     int     rep;
 110     char    uri_name[128];
 111
 112     snprintf(uri_name, sizeof(uri_name), "ws://%s:%d", spadr, port);
 113     connected = 0;
 114     context = ico_uws_create_context(uri_name, LWS_PROTOCOLNAME);
 115     if (context == NULL)    {
 116         fprintf(stderr, "Can not create conextion context(uri=%s port=%d)\n",
 117                 uri_name, port);
 118         exit(2);
 119     }
 120     if (ico_uws_set_event_cb(context, uws_callback, NULL) !=
 121         ICO_UWS_ERR_NONE) {
 122         fprintf(stderr, "Can not set callback.\n");
 123         exit(2);
 124     }
 125
 126     /* wait for connection          */
 127     for (rep = 0; rep < (2*1000); rep += 50)    {
 128         if (connected)  break;
 129         ico_uws_service(context);
 130         usleep(50 * 1000);
 131     }
 132 }
 133
 134 static void
 135 set_vehicleinfo(const char *cmd)
 136 {
 137     int     i, j;
 138     int     idx, key, pt;
 139     int     msgsize;
 140     short   *sp;
 141     int     *ip;
 142     double  *dp;
 143     char    prop[64];
 144     char    value[128];
 145     int     sec, msec;
 146     struct  {
 147         struct KeyDataMsg_t     msg;
 148         char    dummy[128];
 149     }       msg;
 150
 151     j = 0;
 152     for (i = 0; cmd[i]; i++)    {
 153         if ((cmd[i] == '=') || (cmd[i] == ' ')) break;
 154         if (j < (int)(sizeof(prop)-1))  {
 155             prop[j++] = cmd[i];
 156         }
 157     }
 158
 159     prop[j] = 0;
 160     j = 0;
 161     if (cmd[i] != 0)    {
 162         for (i++; cmd[i]; i++)  {
 163             if (cmd[i] == ' ')  continue;
 164             if (j < (int)(sizeof(value)-1)) {
 165                 value[j++] = cmd[i];
 166             }
 167         }
 168     }
 169     value[j] = 0;
 170
 171     if (strcasecmp(prop, "sleep") == 0) {
 172         sec = 0;
 173         msec = 0;
 174         for (i = 0; value[i]; i++)  {
 175             if (value[i] == '.')        break;
 176             sec = sec * 10 + (value[i] & 0x0f);
 177         }
 178         if (value[i] == '.')    {
 179             i++;
 180             if (value[i] != 0)  {
 181                 msec = (value[i] & 0x0f) * 100;
 182                 i++;
 183             }
 184             if (value[i] != 0)  {
 185                 msec = msec + (value[i] & 0x0f) * 10;
 186                 i++;
 187             }
 188             if (value[i] != 0)  {
 189                 msec = msec + (value[i] & 0x0f);
 190             }
 191         }
 192         if (sec > 0)    sleep(sec);
 193         if (msec > 0)   usleep(msec * 1000);
 194
 195         return;
 196     }
 197
 198     for (key = 0; vehicleinfo_key[key].prop[0]; key++)  {
 199         if (strcasecmp(prop, vehicleinfo_key[key].prop) == 0)   break;
 200     }
 201     if (! vehicleinfo_key[key].prop[0]) {
 202         fprintf(stderr, "VehicleInfo UnKnown property[%s]\n", prop);
 203         return;
 204     }
 205
 206     memset(&msg, 0, sizeof(msg));
 207     strcpy(msg.msg.KeyEventType, vehicleinfo_key[key].eventtype);
 208     gettimeofday(&(msg.msg.recordtime), NULL);
 209     msgsize = sizeof(msg) - 128;
 210
 211     i = 0;
 212     pt = 0;
 213     for (idx = 0; idx < 4; idx++)   {
 214         if (vehicleinfo_key[key].datatype[idx] == TYPE_NULL)    break;
 215
 216         if (value[i])   {
 217             for (j = i; value[j]; j++)  {
 218                 if ((value[j] == ',') || (value[j] == ';') ||
 219                     (value[j] == ':') || (value[j] == ' ')) break;
 220             }
 221             if (value[j] != 0)  {
 222                 value[j++] = 0;
 223             }
 224             switch (vehicleinfo_key[key].datatype[idx] % 10) {
 225             case TYPE_BOOL:
 226             case TYPE_BYTE:
 227                 if (vehicleinfo_key[key].datatype[idx] == TYPE_SHIFT)   {
 228                     if ((strcasecmp(&value[i], "sp") == 0) ||
 229                         (strcasecmp(&value[i], "s0") == 0)) {
 230                         strcpy(&value[i], "0");
 231                     }
 232                     else if (strcasecmp(&value[i], "sr") == 0)  {
 233                         strcpy(&value[i], "1");
 234                     }
 235                     else if (strcasecmp(&value[i], "sn") == 0)  {
 236                         strcpy(&value[i], "2");
 237                     }
 238                     else if ((strcasecmp(&value[i], "sd") == 0) ||
 239                              (strcasecmp(&value[i], "s4") == 0))    {
 240                         strcpy(&value[i], "4");
 241                     }
 242                     else if ((strcasecmp(&value[i], "s1") == 0) ||
 243                              (strcasecmp(&value[i], "sl") == 0))    {
 244                         strcpy(&value[i], "5");
 245                     }
 246                     else if (strcasecmp(&value[i], "s2") == 0)  {
 247                         strcpy(&value[i], "6");
 248                     }
 249                     else if (strcasecmp(&value[i], "s3") == 0)  {
 250                         strcpy(&value[i], "7");
 251                     }
 252                 }
 253                 msg.msg.data.status[pt++] = strtoul(&value[i], (char **)0, 0);
 254                 msgsize += 1;
 255                 break;
 256             case TYPE_INT16:
 257             case TYPE_UINT16:
 258                 sp = (short *)&msg.msg.data.status[pt];
 259                 *sp = strtol(&value[i], (char **)0, 0);
 260                 pt += sizeof(short);
 261                 msgsize += sizeof(short);
 262                 break;
 263             case TYPE_INT32:
 264             case TYPE_UINT32:
 265                 ip = (int *)&msg.msg.data.status[pt];
 266                 *ip = strtol(&value[i], (char **)0, 0);
 267                 pt += sizeof(int);
 268                 msgsize += sizeof(int);
 269                 break;
 270             case TYPE_DOUBLE:
 271                 dp = (double *)&msg.msg.data.status[pt];
 272                 *dp = strtod(&value[i], (char **)0);
 273                 pt += sizeof(double);
 274                 msgsize += sizeof(double);
 275                 break;
 276             default:
 277                 break;
 278             }
 279             i = j;
 280         }
 281         else    {
 282             switch (vehicleinfo_key[key].datatype[idx]) {
 283             case TYPE_BOOL:
 284             case TYPE_BYTE:
 285                 msgsize += 1;
 286                 break;
 287             case TYPE_INT16:
 288             case TYPE_UINT16:
 289                 msgsize += sizeof(short);
 290                 break;
 291             case TYPE_INT32:
 292             case TYPE_UINT32:
 293                 msgsize += sizeof(int);
 294                 break;
 295             case TYPE_DOUBLE:
 296                 msgsize += sizeof(double);
 297                 break;
 298             default:
 299                 break;
 300             }
 301         }
 302     }
 303
 304     ico_uws_send(context, connect_id, (unsigned char *)&msg, msgsize);
 305 }
 306
 307 static void
 308 usage(const char *prog)
 309 {
 310     fprintf(stderr, "Usage: %s [-port=port] [-ip=ip_addr] [propaty=value] [propaty=value] ...\n", prog);
 311     exit(0);
 312 }
 313
 314 int
 315 main(int argc, char *argv[])
 316 {
 317     int     i, j;
 318     int     port = LWS_DEFAULTPORT;
 319     char    spadr[64];
 320     char    buf[240];
 321
 322     strcpy(spadr, LWS_DEFAULTIP);
 323
 324     j = 0;
 325     for (i = 1; i < argc; i++)  {
 326         if (argv[i][0] == '-')  {
 327             if (strncasecmp(argv[i], "-port=", 6) == 0)   {
 328                 port = strtoul(&argv[i][6], (char **)0, 0);
 329             }
 330             else if (strncasecmp(argv[i], "-ip=", 4) == 0)   {
 331                 memset(spadr, 0, sizeof(spadr));
 332                 strncpy(spadr, &argv[i][4], sizeof(spadr)-1);
 333             }
 334             else    {
 335                 usage(argv[0]);
 336             }
 337         }
 338         else    {
 339             j++;
 340         }
 341     }
 342
 343     init_comm(port, spadr);
 344
 345     if (!connected) {
 346         fprintf(stderr, "Can't connect server.\n");
 347         exit(-1);
 348     }
 349
 350     if (j <= 0) {
 351         while (fgets(buf, sizeof(buf), stdin))  {
 352             j = -1;
 353             for (i = 0; buf[i]; i++)    {
 354                 if ((buf[i] == '#') || (buf[i] == '\n') || (buf[i] == '\r'))    break;
 355                 if (buf[i] == '\t') buf[i] = ' ';
 356                 if ((j < 0) && (buf[i] != ' ')) j = i;
 357             }
 358             if (j < 0)  continue;
 359             buf[i] = 0;
 360             set_vehicleinfo(&buf[j]);
 361         }
 362     }
 363     else    {
 364         for (i = 1; i < argc; i++)  {
 365             if (argv[i][0] == '-')  continue;
 366             set_vehicleinfo(argv[i]);
 367         }
 368     }
 369     if (context)    {
 370         ico_uws_unset_event_cb(context);
 371         ico_uws_close(context);
 372     }
 373
 374     exit(0);
 375 }
 376