src/CGtCtrl.cpp

   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   Pseudo-driving vehicle
  11  *
  12  * @date    Jun-21-2012 create
  13  * @date    Apl-09-2013 RPM/Speed/Breake/GPS/direction Reform
  14  * @file    CGtCtrl.cpp
  15  */
  16 #include <unistd.h>
  17 #include <fstream>
  18 #include "CJoyStick.h"
  19 #include "CJoyStickEV.h"
  20 #include "CGtCtrl.h"
  21 #include "CAvgCar.h"
  22 #include "CCalc.h"
  23
  24 #include <sys/time.h>
  25 #include "ico-util/ico_log.h"
  26
  27 extern bool gbDevJs;
  28
  29 bool g_bStopFlag;
  30 std::queue < geoData > routeList;
  31 std::string msgQueue = "";
  32 std::string orgmsgQueue = "";
  33 int Daemon_MS;
  34 int nClient = 0;
  35 int nDelayedCallback = 0;
  36
  37 int ReadPriv = 0;
  38 int WritePriv = 0;
  39
  40 int Debug = 0;
  41
  42 pthread_mutex_t mutex;
  43
  44 /**
  45  * Average ENGINE or CAR Sample Space size
  46  */
  47 short g_RPM_SAMPLE_SPACE_SIZE    = 60;
  48 short g_SPEED_SAMPLE_SPACE_SIZE  = 180;
  49 short g_BREAKE_SAMPLE_SPACE_SIZE = 10;
  50
  51 /**
  52  * GPS Info update flag
  53  */
  54 bool   g_bSentInMileage   = true;
  55 bool   g_bSentInChgLngLat = true;
  56 double g_SentInMileage    = 3.0;  // 3 meter
  57 double g_StartLatitude    =  35.47945;
  58 double g_StartLongitude   = 139.40026;
  59 double g_GoalLatitude     =  35.49746;
  60 double g_GoalLongitude    = 139.40504;
  61
  62 using namespace std;
  63 #define _D_URI_FORM "ws://%s:%d"
  64 #define _D_HOST_STR "127.0.0.1"
  65 const int protocols_sz = 4;
  66 const char* protocolsName[protocols_sz] = {
  67     "standarddatamessage-only",
  68     "standardcontrolmessage-only",
  69     "customdatamessage-only",
  70     "customcontrolmessage-only"
  71 };
  72
  73
  74 CGtCtrl::CGtCtrl()
  75 {
  76     m_strConfPath = "/usr/bin/CarSimDaemon.conf";
  77     // TODO Auto-generated constructor stub
  78     signal(SIGINT, CGtCtrl::signal_handler);
  79     signal(SIGQUIT, CGtCtrl::signal_handler);
  80     signal(SIGKILL, CGtCtrl::signal_handler);
  81     signal(SIGTERM, CGtCtrl::signal_handler);
  82     signal(SIGCHLD, CGtCtrl::signal_handler);
  83
  84     m_bUseGps = false;
  85     myJS = NULL;
  86 }
  87
  88 CGtCtrl::~CGtCtrl()
  89 {
  90     // TODO Auto-generated destructor stub
  91     if (NULL != myJS) {
  92         delete myJS;
  93         myJS = NULL;
  94     }
  95 }
  96
  97 void CGtCtrl::signal_handler(int signo)
  98 {
  99     switch (signo) {
 100     case SIGINT:
 101     case SIGKILL:
 102     case SIGQUIT:
 103     case SIGTERM:
 104         g_bStopFlag = false;
 105         printf("receive cancel command(%d).\n", signo);
 106         sleep(1);
 107         break;
 108     case SIGALRM:
 109         break;
 110     }
 111 }
 112
 113
 114 bool CGtCtrl::Initialize()
 115 {
 116     m_nJoyStickID = -1;
 117
 118     m_stVehicleInfo.fLng = g_StartLongitude;
 119     m_stVehicleInfo.fLat = g_StartLatitude;
 120     m_stVehicleInfo.nSteeringAngle = 0;
 121     m_stVehicleInfo.nShiftPos = 0;
 122     m_stVehicleInfo.bHazard = false;
 123     m_stVehicleInfo.bWinkR = false;
 124     m_stVehicleInfo.bWinkL = false;
 125     m_stVehicleInfo.nAirconTemp = 250;
 126     m_stVehicleInfo.nHeadLightPos = 1;
 127     m_stVehicleInfo.nVelocity = 0;
 128     m_stVehicleInfo.nDirection = 0;
 129     m_stVehicleInfo.dVelocity = 0.0;
 130     m_stVehicleInfo.nAccel = 0;
 131     m_stVehicleInfo.bHeadLight = false; // HEAD LIGHT OFF(false)
 132
 133     m_bFirstOpen = true;
 134     if (true == gbDevJs) {
 135         myJS = new CJoyStick;
 136         int nRet = myJS->Open();
 137         if (nRet < 0) {
 138             printf("JoyStick open error\n");
 139             return false;
 140         }
 141     }
 142     else {
 143         int i = 0;
 144         do {
 145             switch (i) {
 146             case 0 :
 147                 printf("Load class G27\n");
 148                 myJS = new CJoyStickG27;
 149                 m_strConfPath = g_ConfPathG27;
 150                 break;
 151             case 1 :
 152                 printf("Load class G25\n");
 153                 myJS = new CJoyStickG25;
 154                 m_strConfPath = g_ConfPathG25;
 155                 break;
 156             case 2 :
 157                 printf("Load class EV\n");
 158                 myJS = new CJoyStickEV;
 159                 m_strConfPath = g_ConfPathG27;
 160                 break;
 161             default :
 162                 break;
 163             }
 164             int nRet = myJS->Open();
 165             if (nRet > 0) {
 166                 break;
 167             }
 168             delete myJS;
 169             myJS = NULL;
 170         } while ((++i) < g_JoyStickTypeNum);
 171         if (myJS == NULL) {
 172             return false;
 173         }
 174     }
 175
 176     myConf.LoadConfig(m_strConfPath.c_str());
 177     m_stVehicleInfo.fLng = myConf.m_fLng;
 178     m_stVehicleInfo.fLat = myConf.m_fLat;
 179
 180     m_viList.init();
 181
 182     if (!LoadConfigJson(myConf.m_sAmbConfigName.c_str())) {
 183         printf("AMB configfile read error\n");
 184         return false;
 185     }
 186
 187     m_sendMsgInfo.clear();
 188
 189     char uri[128];
 190     for (int i = 0; i < protocols_sz; i++) {
 191         sprintf(uri, _D_URI_FORM, _D_HOST_STR, m_ambpicomm_port[i]);
 192         if (false == m_ambpicomm_client[i].start(uri, protocolsName[i])) {
 193             return false;   // connect fail
 194         }
 195         while(m_ambpicomm_client[i].getM_id() == NULL) {
 196             usleep(100*1000);
 197         }
 198         ICO_DBG("websocket: OK [%s][%s]", uri, protocolsName[i]);
 199     }
 200
 201     const char *vi = "LOCATION";
 202     double location[] = { myConf.m_fLat, myConf.m_fLng, 0 };
 203     SendVehicleInfo(dataport_def, vi, &location[0], 3);
 204
 205     if (m_bDemoRunning) {
 206         std::cout << "Demo Mode." << std::endl;
 207         LoadRouteList();
 208     }
 209
 210     return true;
 211 }
 212
 213 bool CGtCtrl::Terminate()
 214 {
 215     bool b = true;
 216
 217     if (myJS != NULL) {
 218         myJS->Close();
 219     }
 220     return b;
 221 }
 222
 223 #define sENGINE_SPEED   "ENGINE_SPEED"
 224 #define sBRAKE_SIGNAL   "BRAKE_SIGNAL"
 225 #define sBRAKE_PRESSURE "BRAKE_PRESSURE"
 226 #define sACCPEDAL_OPEN  "ACCPEDAL_OPEN"
 227 #define sVELOCITY       "VELOCITY"
 228 #define sDIRECTION      "DIRECTION"
 229 #define sLOCATION       "LOCATION"
 230 #define sSHIFT          "SHIFT"
 231 #define sLIGHTSTATUS    "LIGHTSTATUS"
 232 void CGtCtrl::Run()
 233 {
 234     g_bStopFlag = true;
 235
 236     int type = -1;
 237     int number = -1;
 238     int value = -1;
 239
 240     /**
 241       * INTERVAL CONTROL
 242       */
 243     struct timeval now;
 244     struct timeval timeout;
 245     long usec;
 246     int intervalctl = 0;
 247     int fcycle = 0;
 248     gettimeofday(&now, NULL);
 249     usec = now.tv_usec + D_RUNLOOP_INTERVAL_COUNT*10*1000;
 250     timeout.tv_usec = usec % 1000000;
 251     if (usec < 1000000) {
 252         timeout.tv_sec = now.tv_sec;
 253     } else {
 254         timeout.tv_sec = now.tv_sec + 1;
 255     }
 256     /**
 257       * LAST STEERING VALUE
 258       */
 259     int last_steering = 0;
 260     int last_angle = 0;
 261     /**
 262      * RPM/Breake/Speed calc class
 263      */
 264     int nRPM = -1;
 265     bool bBrakeSigSend = false;
 266     bool bBrakeSig = false;
 267     int nBrakePressure = -1;
 268     int nAccPedalOpen = -1;
 269     int nSpeed = -1;
 270     CAvgCar pmCar(g_RPM_SAMPLE_SPACE_SIZE, g_SPEED_SAMPLE_SPACE_SIZE,
 271                   g_BREAKE_SAMPLE_SPACE_SIZE);
 272     pmCar.chgGear(CAvgGear::E_SHIFT_PARKING);
 273     /**
 274      * DIRECTION
 275      */
 276     double dir = (double)m_stVehicleInfo.nDirection;
 277     /**
 278      * SHIFT
 279      */
 280     int nShiftPosBK = -1;
 281     char shiftpos = 255;
 282
 283     bool nextPointFlg = false;
 284     geoData next;
 285     double dx, dy, rad, theta;
 286
 287     while (g_bStopFlag) {
 288
 289         type = myJS->Read(&number, &value);
 290
 291         m_sendMsgInfo.clear();
 292
 293         gettimeofday(&now, NULL);
 294         if((timeout.tv_sec == now.tv_sec && timeout.tv_usec <= now.tv_sec)
 295             || (timeout.tv_sec < now.tv_sec)) {
 296             intervalctl = (intervalctl+1)%3;
 297             if (intervalctl) {
 298                 fcycle = intervalctl;
 299             }
 300             usec = now.tv_usec + D_RUNLOOP_INTERVAL_COUNT*10*1000;
 301             timeout.tv_usec = usec % 1000000;
 302             if (usec < 1000000) {
 303                 timeout.tv_sec = now.tv_sec;
 304             } else {
 305                 timeout.tv_sec = now.tv_sec + 1;
 306             }
 307         }
 308 //      ICO_DBG("intervalctl[%d], fcycle[%d]", intervalctl, fcycle);
 309
 310         switch (type) {
 311         case JS_EVENT_AXIS:
 312             if (number == myConf.m_nSteering) {
 313                 last_steering = value;
 314                 if (value != 0) {
 315                     m_stVehicleInfo.nSteeringAngle +=
 316                         (value * 10 / 65536 - m_stVehicleInfo.nSteeringAngle);
 317                 }
 318             }
 319             else if (number == myConf.m_nAccel) {
 320                 if (0 >= value) {
 321                     pmCar.chgThrottle(32767);
 322                 }
 323                 else {
 324                     pmCar.chgThrottle((value - 16384) * -2);
 325                 }
 326             }
 327             else if (number == myConf.m_nBrake) {
 328                 if (0 >= value) {
 329                     pmCar.chgBrake(32767);
 330                 }
 331                 else {
 332                     pmCar.chgBrake((value - 16384) * -2);
 333                 }
 334             }
 335             break;
 336         case JS_EVENT_BUTTON:
 337             if (myConf.m_sDeviceName == D_DEV_NAME_G25) {
 338                 /**
 339                  * Gear Change SHIFT UP
 340                  */
 341                 if (number == myConf.m_nShiftU) {
 342                     //printf("Shift Up[%d]\n",value);
 343                     if (value != 0) {
 344                         pmCar.setShiftUp();
 345                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 346                         shiftpos = pmCar.getValue();
 347                     }
 348                 }
 349                 /**
 350                  * Gear Change SHIFT DOWN
 351                  */
 352                 if (number == myConf.m_nShiftD) {
 353                     //printf("Shift Down[%d]\n",value);
 354                     if (value != 0) {
 355                         pmCar.setShiftDown();
 356                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 357                         shiftpos = pmCar.getValue();
 358                     }
 359                 }
 360             }
 361
 362             else if (myConf.m_sDeviceName == D_DEV_NAME_G27) {
 363                 if (number == myConf.m_nShift1) {
 364                     //printf("Shift 1[%d]\n",value);
 365                     if (value != 0) {
 366                         pmCar.setShiftMT(CAvgGear::E_SHIFT_MT_FIRST);
 367                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 368                         shiftpos = pmCar.getValue();
 369                     }
 370                     else {
 371                         pmCar.setShiftMT(CAvgGear::E_SHIFT_NEUTRAL);
 372                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 373                         shiftpos = pmCar.getValue();
 374                     }
 375                 }
 376
 377                 if (number == myConf.m_nShift2) {
 378                     //printf("Shift 2[%d]\n",value);
 379                     if (value != 0) {
 380                         pmCar.setShiftMT(CAvgGear::E_SHIFT_MT_SECOND);
 381                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 382                         shiftpos = pmCar.getValue();
 383                     }
 384                     else {
 385                         pmCar.setShiftMT(CAvgGear::E_SHIFT_NEUTRAL);
 386                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 387                         shiftpos = pmCar.getValue();
 388                     }
 389                 }
 390
 391                 if (number == myConf.m_nShift3) {
 392                     //printf("Shift 3[%d]\n",value);
 393                     if (value != 0) {
 394                         pmCar.setShiftMT(CAvgGear::E_SHIFT_MT_THIRD);
 395                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 396                         shiftpos = pmCar.getValue();
 397                     }
 398                     else {
 399                         pmCar.setShiftMT(CAvgGear::E_SHIFT_NEUTRAL);
 400                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 401                         shiftpos = pmCar.getValue();
 402                     }
 403                 }
 404
 405                 if (number == myConf.m_nShift4) {
 406                     //printf("Shift 4[%d]\n",value);
 407                     if (value != 0) {
 408                         pmCar.setShiftMT(CAvgGear::E_SHIFT_MT_FOURTH);
 409                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 410                         shiftpos = pmCar.getValue();
 411                     }
 412                     else {
 413                         pmCar.setShiftMT(CAvgGear::E_SHIFT_NEUTRAL);
 414                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 415                         shiftpos = pmCar.getValue();
 416                     }
 417                 }
 418
 419                 if (number == myConf.m_nShift5) {
 420                     //printf("Shift 5[%d]\n",value);
 421                     if (value != 0) {
 422                         pmCar.setShiftMT(CAvgGear::E_SHIFT_MT_FIFTH);
 423                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 424                         shiftpos = pmCar.getValue();
 425                     }
 426                     else {
 427                         pmCar.setShiftMT(CAvgGear::E_SHIFT_NEUTRAL);
 428                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 429                         shiftpos = pmCar.getValue();
 430                     }
 431                 }
 432
 433                 if (number == myConf.m_nShift6) {
 434                     //printf("Shift 6[%d]\n",value);
 435                     if (value != 0) {
 436                         pmCar.setShiftMT(CAvgGear::E_SHIFT_MT_SIXTH);
 437                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 438                         shiftpos = pmCar.getValue();
 439                     }
 440                     else {
 441                         pmCar.setShiftMT(CAvgGear::E_SHIFT_NEUTRAL);
 442                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 443                         shiftpos = pmCar.getValue();
 444                     }
 445                 }
 446
 447                 if (number == myConf.m_nShiftR) {
 448                     //printf("Shift R[%d]\n",value);
 449                     if (value != 0) {
 450                         pmCar.setShiftMT(CAvgGear::E_SHIFT_REVERSE);
 451                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 452                         shiftpos = pmCar.getValue();
 453                     }
 454                     else {
 455                         pmCar.setShiftMT(CAvgGear::E_SHIFT_NEUTRAL);
 456                         m_stVehicleInfo.nShiftPos = pmCar.getSelectGear();
 457                         shiftpos = pmCar.getValue();
 458                     }
 459                 }
 460             }
 461             /**
 462              * TURN SIGNAL(WINKER) & LIGHTSTATUS
 463              */
 464             bool bLIGHTSTATUS = false;
 465             if (number == myConf.m_nWinkR) {
 466                 if (value != 0) {
 467
 468                     m_stVehicleInfo.bWinkR = !m_stVehicleInfo.bWinkR;
 469                     m_stVehicleInfo.bWinkL = false;
 470                     if (m_stVehicleInfo.bWinkR)
 471                         m_stVehicleInfo.nWinkerPos = WINKER_RIGHT;
 472                     else
 473                         m_stVehicleInfo.nWinkerPos = WINKER_OFF;
 474
 475                     const char *vi = "TURN_SIGNAL";
 476                     int wpos =
 477                         m_stVehicleInfo.nWinkerPos == WINKER_RIGHT ? 1 : 0;
 478                     SendVehicleInfo(dataport_def, vi, wpos);
 479                     bLIGHTSTATUS = true;
 480                 }
 481             }
 482
 483             if (number == myConf.m_nWinkL) {
 484                 if (value != 0) {
 485                     m_stVehicleInfo.bWinkL = !m_stVehicleInfo.bWinkL;
 486                     m_stVehicleInfo.bWinkR = false;
 487                     if (m_stVehicleInfo.bWinkL)
 488                         m_stVehicleInfo.nWinkerPos = WINKER_LEFT;
 489                     else
 490                         m_stVehicleInfo.nWinkerPos = WINKER_OFF;
 491
 492                     const char *vi = "TURN_SIGNAL";
 493                     int wpos =
 494                         m_stVehicleInfo.nWinkerPos == WINKER_LEFT ? 2 : 0;
 495                     SendVehicleInfo(dataport_def, vi, wpos);
 496                     bLIGHTSTATUS = true;
 497                 }
 498             }
 499
 500             if (number == myConf.m_nHeadLight) {
 501                 if (0 != value) {
 502                     if (false == m_stVehicleInfo.bHeadLight) { // HEAD LIGHT OFF(false) ?
 503                         m_stVehicleInfo.bHeadLight = true; // HEAD LIGHT ON(true)
 504                     }
 505                     else {
 506                         m_stVehicleInfo.bHeadLight = false; // HEAD LIGHT OFF(false)
 507                     }
 508                     bLIGHTSTATUS = true;
 509                 }
 510             }
 511
 512             if (true == bLIGHTSTATUS) {
 513                 const size_t LSsz = 8;
 514                 char data[LSsz];
 515                     // 0:LIGHT HEAD STATUS ON(1)/OFF(0)
 516                     // 1:LEFT WINKER STATUS ON(1)/OFF(0)
 517                     // 2:RIGHT WINKER STATUS ON(1)/OFF(0)
 518                     // 3:PARKING
 519                     // 4:FOG LAMP
 520                     // 5:HAZARD
 521                     // 6:BRAKE
 522                     // 7:HIGHBEAM
 523                 memset(data, 0, sizeof(data));
 524                 if (true == m_stVehicleInfo.bHeadLight) { // HEAD LIGHT ON ?
 525                     data[0] = 1;
 526                 }
 527                 if (WINKER_LEFT == m_stVehicleInfo.nWinkerPos) {
 528                     data[1] = 1;
 529                 }
 530                 else if (WINKER_RIGHT == m_stVehicleInfo.nWinkerPos) {
 531                     data[2] = 1;
 532                 }
 533                 SendVehicleInfo(dataport_def, sLIGHTSTATUS, &data[0], LSsz);
 534             }
 535
 536             break;
 537         }
 538         pmCar.updateAvg();
 539         /* RealTime */
 540         /**
 541          * BRAKE SIGNAL
 542          */
 543         bool bBrakeNEW = pmCar.isOnBrake();
 544         m_stVehicleInfo.bBrake = bBrakeNEW;
 545         if ((false == bBrakeSigSend) || (bBrakeNEW != bBrakeSig)) {
 546             bBrakeSigSend = true;
 547             SendVehicleInfo(dataport_def, sBRAKE_SIGNAL, bBrakeNEW);
 548             bBrakeSig = bBrakeNEW; // update value
 549         }
 550         /**
 551          * SHIFT
 552          */
 553         if (nShiftPosBK != m_stVehicleInfo.nShiftPos) {
 554             const size_t ShiftSz = 3;
 555             int data[ShiftSz];
 556             int val = pmCar.getValue();
 557             data[0] = pmCar.getSelectGear();
 558             if (data[0] > 10) {
 559                 data[0] = data[0] - 10 + 4;
 560                 if (data[0] >= 8) {
 561                     data[0] = 4;
 562                 }
 563             }
 564             data[1] = pmCar.getValue();
 565             data[2] = pmCar.getMode();
 566             SendVehicleInfo(dataport_def, sSHIFT, &data[0], ShiftSz);
 567             nShiftPosBK = m_stVehicleInfo.nShiftPos;
 568 //          ICO_DBG("SHIFT: Send [%d, %d, %d]", data[0], data[1], data[2]);
 569         }
 570
 571         /* 100ms cycle */
 572         if (fcycle == 2) {
 573             int angle = 0;
 574             if (last_steering < 0) {
 575                 angle = (((double)last_steering / 32768) * 450) + 450 + 270;
 576                 angle %= 360;
 577                 //std:: cout << "last_steering * (450 / 32768) = " << ((double)last_steering / 32768) * 450 << "\n";
 578                 //std:: cout << "last_steering * (450 / 32768) + 450 = " << (double)last_steering * (450 / 32768) + 450 << "\n";
 579                 //std:: cout << "last_steering * (450 / 32768) + 450 + 270 = " << (double)last_steering * (450 / 32768) + 450 + 270 << "\n";
 580             }
 581             else {
 582                 angle = ((double)last_steering / 32768) * 450;
 583                 angle %= 360;
 584                 //std:: cout << "last_steering * (450 / 32768) = " << ((double)last_steering / 32768) * 450 << "\n";
 585             }
 586             //std::cout << "Steering last_steering = " << last_steering << " -> angle = " << angle << std::endl;
 587             if (angle != last_angle) {
 588                 const char *vi = "STEERING";
 589
 590                 /*
 591                 SendVehicleInfo(dataport_def,
 592                                 vi, m_stVehicleInfo.nSteeringAngle);
 593                 */
 594                 SendVehicleInfo(dataport_def, vi, angle);
 595                 last_angle = angle;
 596             }
 597             /**
 598              * RPM check
 599              */
 600             int rpmNEW = (int)pmCar.getRPM();
 601             if (rpmNEW != nRPM) {
 602                 SendVehicleInfo(dataport_def, sENGINE_SPEED, rpmNEW);
 603                 nRPM = rpmNEW; // update value
 604             }
 605             int apoNEW = pmCar.calcAccPedalOpen();
 606             if (apoNEW != nAccPedalOpen) {
 607                 SendVehicleInfo(dataport_def, sACCPEDAL_OPEN, apoNEW);
 608                 nAccPedalOpen = apoNEW;
 609             }
 610         } /* if (fcycle == 2) */
 611
 612         /* 50ms cycle */
 613         if (fcycle != 0) {
 614             /**
 615              * BRAKE PRESSURE
 616              */
 617             int pressureNEW = pmCar.calcPressure(pmCar.getBrakeAvg());
 618             m_stVehicleInfo.nBrakeHydraulicPressure = pressureNEW;
 619             if (pressureNEW != nBrakePressure) {
 620                 SendVehicleInfo(dataport_def, sBRAKE_PRESSURE, pressureNEW);
 621                 nBrakePressure = pressureNEW;
 622             }
 623
 624             /**
 625              * VELOCITY (SPEED)
 626              */
 627             int speedNew = (int)pmCar.getSpeed();
 628             if (speedNew > 180) {
 629                 speedNew = 180;
 630             }
 631             if (speedNew != nSpeed) {
 632                 SendVehicleInfo(dataport_def, sVELOCITY, speedNew);
 633                 m_stVehicleInfo.nVelocity = speedNew;
 634                 nSpeed = speedNew;
 635             }
 636
 637             if (m_bDemoRunning) {
 638                 next = routeList.front();
 639                 dy = next.lat - m_stVehicleInfo.fLat;
 640                 dx = next.lng - m_stVehicleInfo.fLng;
 641                 theta = (double) atan2(dy, dx) * 180.0 / (atan(1.0) * 4.0);
 642                 if (theta < 0) {
 643                     theta = 360.0 + theta;
 644                 }
 645
 646                 rad = (theta / 180.0) * PIE;
 647
 648                 m_stVehicleInfo.nDirection = 90 - theta;
 649                 if (m_stVehicleInfo.nDirection < 0) {
 650                     m_stVehicleInfo.nDirection = 360 + m_stVehicleInfo.nDirection;
 651                 }
 652                 SendVehicleInfo(dataport_def, sDIRECTION, m_stVehicleInfo.nDirection);
 653
 654                 dir = static_cast<double>(m_stVehicleInfo.nDirection);
 655                 double runMeters = pmCar.getTripmeter();
 656                 pmCar.tripmeterReset();
 657                 double tmpLat = m_stVehicleInfo.fLat;
 658                 double tmpLng = m_stVehicleInfo.fLng;
 659                 POINT pNEW = CalcDest(tmpLat, tmpLng, dir, runMeters);
 660                 double tmpLct[] = { pNEW.lat, pNEW.lng, 0 };
 661                 SendVehicleInfo(dataport_def, sLOCATION, &tmpLct[0], 3);
 662                 m_stVehicleInfo.fLat = pNEW.lat;
 663                 m_stVehicleInfo.fLng = pNEW.lng;
 664
 665                 if (rad == 0) {
 666                     if (m_stVehicleInfo.fLng >= next.lng) {
 667                         nextPointFlg = true;
 668                     }
 669                 }
 670                 else if (rad > 0 && rad < 0.5 * PIE) {
 671                     if (m_stVehicleInfo.fLng >= next.lng
 672                         && m_stVehicleInfo.fLat >= next.lat) {
 673                         nextPointFlg = true;
 674                     }
 675                 }
 676                 else if (rad == 0.5 * PIE) {
 677                     if (m_stVehicleInfo.fLat >= next.lat) {
 678                         nextPointFlg = true;
 679                     }
 680                 }
 681                 else if (rad > 0.5 * PIE && rad < PIE) {
 682                     if (m_stVehicleInfo.fLng <= next.lng
 683                         && m_stVehicleInfo.fLat >= next.lat) {
 684                         nextPointFlg = true;
 685                     }
 686                 }
 687                 else if (rad == PIE) {
 688                     if (m_stVehicleInfo.fLng <= next.lng) {
 689                         nextPointFlg = true;
 690                     }
 691                 }
 692                 else if (rad > PIE && rad < 1.5 * PIE) {
 693                     if (m_stVehicleInfo.fLng <= next.lng
 694                         && m_stVehicleInfo.fLat <= next.lat) {
 695                         nextPointFlg = true;
 696                     }
 697                 }
 698                 else if (rad == 1.5 * PIE) {
 699                     if (m_stVehicleInfo.fLat <= next.lat) {
 700                         nextPointFlg = true;
 701                     }
 702                 }
 703                 else {
 704                     if (m_stVehicleInfo.fLng >= next.lng
 705                         && m_stVehicleInfo.fLat <= next.lat) {
 706                         nextPointFlg = true;
 707                     }
 708                 }
 709
 710                 if (nextPointFlg) {
 711                     std::cout << "routeList.size() = " << routeList.size() << std::endl;
 712                     routeList.pop();
 713                     if (routeList.empty()) {
 714                         LoadRouteList();
 715                         m_stVehicleInfo.fLng = myConf.m_fLng;
 716                         m_stVehicleInfo.fLat = myConf.m_fLat;
 717                     }
 718                     next = routeList.front();
 719                     nextPointFlg = false;
 720                 }
 721             }
 722             else {
 723                 /**
 724                   * DIRECTION (AZIMUTH)
 725                   * Front wheel steering angle
 726                   */
 727                 double runMeters = pmCar.getTripmeter();
 728                 pmCar.tripmeterReset();
 729                 if (0 != runMeters) {
 730                     double stear = (double)m_stVehicleInfo.nSteeringAngle;
 731                     double dirNEW = CalcAzimuth(dir, stear, runMeters);
 732                     int nDir = (int)dirNEW;
 733                     dir = dirNEW;
 734                     if (nDir != m_stVehicleInfo.nDirection) {
 735                         SendVehicleInfo(dataport_def, sDIRECTION, nDir);
 736                         m_stVehicleInfo.nDirection = nDir;
 737                     }
 738                 }
 739                 /**
 740                   * LOCATION
 741                   */
 742                 if ((!m_bUseGps) && (0 != runMeters)) {
 743                     double tmpLat = m_stVehicleInfo.fLat;
 744                     double tmpLng = m_stVehicleInfo.fLng;
 745                     POINT pNEW = CalcDest(tmpLat, tmpLng, dir, runMeters);
 746                     if ((tmpLat != pNEW.lat) || (tmpLng != pNEW.lng)){
 747                         double tmpLct[] = { pNEW.lat, pNEW.lng, 0 };
 748                         SendVehicleInfo(dataport_def, sLOCATION, &tmpLct[0], 3);
 749                         m_stVehicleInfo.fLat = pNEW.lat;
 750                         m_stVehicleInfo.fLng = pNEW.lng;
 751                     }
 752                 }
 753             }
 754         }
 755
 756         fcycle = 0;
 757     }
 758 }
 759
 760 void CGtCtrl::Run2()
 761 {
 762     msgQueue = "";
 763     g_bStopFlag = true;
 764     geoData next;
 765     double theta = 0;
 766     double rad = 0;
 767     double dx, dy;
 768     bool nextPointFlg = true;
 769     bool routeDriveFlg = true;
 770     std::string tmpstr = "";
 771
 772     int type = -1;
 773     int number = -1;
 774     int value = -1;
 775
 776     pthread_t thread[2];
 777
 778     pthread_mutex_init(&mutex, NULL);
 779
 780     while (g_bStopFlag) {
 781         type = myJS->Read(&number, &value);
 782
 783         pthread_mutex_lock(&mutex);
 784         if (msgQueue.size() > 0) {
 785             char buf[32];
 786             int pos = 0;
 787             int lastidx = 0;
 788             geoData tmpGeo;
 789             int i = 0;
 790
 791             tmpGeo.lat = GEORESET;
 792             tmpGeo.lng = GEORESET;
 793
 794             for (i = 0; i < (int) msgQueue.size(); i++) {
 795                 if (msgQueue[i] == ',') {
 796                     tmpGeo.lat = atof(buf);
 797                     pos = 0;
 798                 }
 799                 else if (msgQueue[i] == '\n') {
 800                     tmpGeo.lng = atof(buf);
 801                     pos = 0;
 802
 803                     routeList.push(tmpGeo);
 804                     lastidx = i;
 805                     tmpGeo.lat = GEORESET;
 806                     tmpGeo.lng = GEORESET;
 807                 }
 808                 else {
 809                     buf[pos++] = msgQueue[i];
 810                 }
 811             }
 812             for (int idx = 0; msgQueue[lastidx] != '\0'; idx++, lastidx++) {
 813                 tmpstr[idx] = msgQueue[lastidx];
 814             }
 815             msgQueue = tmpstr;
 816
 817         }
 818         pthread_mutex_unlock(&mutex);
 819
 820         switch (type) {
 821         case JS_EVENT_AXIS:
 822             if (number == myConf.m_nSteering) {
 823                 if (value != 0)
 824                     m_stVehicleInfo.nSteeringAngle +=
 825                         (value * 10 / 65536 - m_stVehicleInfo.nSteeringAngle);
 826
 827             }
 828
 829             if (number == myConf.m_nAccel) {
 830                 if (value < 0) {
 831                     m_stVehicleInfo.bBrake = false;
 832                     m_stVehicleInfo.nBrakeHydraulicPressure = 0;
 833
 834                 }
 835                 else if (0 < value) {
 836                     m_stVehicleInfo.bBrake = true;
 837                     m_stVehicleInfo.nBrakeHydraulicPressure =
 838                         (value * 100) / MAX_SPEED;
 839                 }
 840                 else {
 841                     m_stVehicleInfo.bBrake = false;
 842                     m_stVehicleInfo.nBrakeHydraulicPressure = 0;
 843                 }
 844                 m_stVehicleInfo.nAccel = value;
 845
 846                 const char *vi1 = "BRAKE_SIGNAL";
 847                 const char *vi2 = "BRAKE_PRESSURE";
 848                 SendVehicleInfo(dataport_def, vi1, m_stVehicleInfo.bBrake);
 849
 850                 SendVehicleInfo(dataport_def,
 851                                 vi2, m_stVehicleInfo.nBrakeHydraulicPressure);
 852             }
 853             break;
 854         case JS_EVENT_BUTTON:
 855             if (number == myConf.m_nShiftU) {
 856                 if (value != 0) {
 857                     char shiftpos = 255;
 858                     if (m_stVehicleInfo.nShiftPos > PARKING) {
 859                         switch (m_stVehicleInfo.nShiftPos) {
 860                         case FIRST:
 861                             m_stVehicleInfo.nShiftPos = SECOND;
 862                             shiftpos = 2;
 863                             break;
 864                         case SECOND:
 865                             m_stVehicleInfo.nShiftPos = THIRD;
 866                             shiftpos = 3;
 867                             break;
 868                         case THIRD:
 869                             m_stVehicleInfo.nShiftPos = DRIVE;
 870                             shiftpos = 4;
 871                             break;
 872                         case DRIVE:
 873                             m_stVehicleInfo.nShiftPos = NEUTRAL;
 874                             shiftpos = 0;
 875                             break;
 876                         case NEUTRAL:
 877                             m_stVehicleInfo.nShiftPos = REVERSE;
 878                             shiftpos = 128;
 879                             break;
 880                         case REVERSE:
 881                             m_stVehicleInfo.nShiftPos = PARKING;
 882                             shiftpos = 255;
 883                             break;
 884                         }
 885                     }
 886
 887                     char data[] = { shiftpos, shiftpos, 0 };
 888                     const char *vi = "SHIFT";
 889                     SendVehicleInfo(dataport_def, vi, &data[0]);
 890
 891                 }
 892             }
 893
 894             if (number == myConf.m_nShiftD) {
 895                 if (value != 0) {
 896                     char shiftpos = 1;
 897                     if (m_stVehicleInfo.nShiftPos < SPORTS) {
 898                         switch (m_stVehicleInfo.nShiftPos) {
 899                         case SHIFT_UNKNOWN:
 900                             m_stVehicleInfo.nShiftPos = PARKING;
 901                             shiftpos = 255;
 902                             break;
 903                         case PARKING:
 904                             m_stVehicleInfo.nShiftPos = REVERSE;
 905                             shiftpos = 128;
 906                             break;
 907                         case REVERSE:
 908                             m_stVehicleInfo.nShiftPos = NEUTRAL;
 909                             shiftpos = 0;
 910                             break;
 911                         case NEUTRAL:
 912                             m_stVehicleInfo.nShiftPos = DRIVE;
 913                             shiftpos = 4;
 914                             break;
 915                         case DRIVE:
 916                             m_stVehicleInfo.nShiftPos = THIRD;
 917                             shiftpos = 3;
 918                             break;
 919                         case THIRD:
 920                             m_stVehicleInfo.nShiftPos = SECOND;
 921                             shiftpos = 2;
 922                             break;
 923                         case SECOND:
 924                             m_stVehicleInfo.nShiftPos = FIRST;
 925                             shiftpos = 1;
 926                             break;
 927                         }
 928                     }
 929
 930                     char data[] = { shiftpos, shiftpos, 0 };
 931                     const char *vi = "SHIFT";
 932                     SendVehicleInfo(dataport_def, vi, &data[0], 3);
 933                 }
 934             }
 935
 936             if (number == myConf.m_nWinkR) {
 937                 if (value != 0) {
 938                     m_stVehicleInfo.bWinkR = !m_stVehicleInfo.bWinkR;
 939                     if (m_stVehicleInfo.bWinkR)
 940                         m_stVehicleInfo.nWinkerPos = WINKER_RIGHT;
 941                     else
 942                         m_stVehicleInfo.nWinkerPos = WINKER_OFF;
 943
 944                     const char *vi = "TURN_SIGNAL";
 945                     int wpos =
 946                         m_stVehicleInfo.nWinkerPos == WINKER_RIGHT ? 1 : 0;
 947                     SendVehicleInfo(dataport_def, vi, wpos);
 948                 }
 949             }
 950
 951             if (number == myConf.m_nWinkL) {
 952                 if (value != 0) {
 953                     m_stVehicleInfo.bWinkL = !m_stVehicleInfo.bWinkL;
 954                     if (m_stVehicleInfo.bWinkL)
 955                         m_stVehicleInfo.nWinkerPos = WINKER_LEFT;
 956                     else
 957                         m_stVehicleInfo.nWinkerPos = WINKER_OFF;
 958
 959                     const char *vi = "TURN_SIGNAL";
 960                     int wpos =
 961                         m_stVehicleInfo.nWinkerPos == WINKER_LEFT ? 2 : 0;
 962                     SendVehicleInfo(dataport_def, vi, wpos);
 963                 }
 964             }
 965
 966             break;
 967         }
 968
 969         if (m_stVehicleInfo.nAccel < 0) {
 970             m_stVehicleInfo.dVelocity +=
 971                 ((abs(m_stVehicleInfo.nAccel) / 10000.0) -
 972                  (m_stVehicleInfo.dVelocity / 100.0));
 973         }
 974         else if (0 < m_stVehicleInfo.nAccel) {
 975             m_stVehicleInfo.dVelocity -=
 976                 ((abs(m_stVehicleInfo.nAccel) / 10000.0) +
 977                  (m_stVehicleInfo.dVelocity / 100.0));
 978         }
 979         else {
 980             m_stVehicleInfo.dVelocity -= (m_stVehicleInfo.dVelocity / 1000);
 981         }
 982
 983         if (m_stVehicleInfo.dVelocity > MAX_SPEED)
 984             m_stVehicleInfo.dVelocity = MAX_SPEED;
 985         if (m_stVehicleInfo.dVelocity < 0)
 986             m_stVehicleInfo.dVelocity = 0.0;
 987
 988         if (m_stVehicleInfo.nVelocity != (int) m_stVehicleInfo.dVelocity) {
 989             m_stVehicleInfo.nVelocity = (int) m_stVehicleInfo.dVelocity;
 990             const char *vi = "VELOCITY";
 991             SendVehicleInfo(dataport_def, vi, m_stVehicleInfo.nVelocity);
 992         }
 993
 994
 995         if (routeList.empty()) {
 996             if (routeDriveFlg) {
 997                 printf("FreeDriving\n");
 998             }
 999             routeDriveFlg = false;
1000
1001             m_stVehicleInfo.nDirection += m_stVehicleInfo.nSteeringAngle;
1002
1003             while (m_stVehicleInfo.nDirection > 359)
1004                 m_stVehicleInfo.nDirection -= 360;
1005             while (m_stVehicleInfo.nDirection < 0)
1006                 m_stVehicleInfo.nDirection += 360;
1007
1008             {
1009                 const char *vi = "DIRECTION";
1010                 SendVehicleInfo(dataport_def, vi, m_stVehicleInfo.nDirection);
1011             }
1012
1013
1014             if (!m_bUseGps) {
1015                 double rad = 0.0;
1016                 if (m_stVehicleInfo.nDirection != 0)
1017                     rad =
1018                         (double) m_stVehicleInfo.nDirection / 180.0 *
1019                         3.14159265;
1020
1021                 double dx = (double) m_stVehicleInfo.nVelocity * sin(rad);
1022                 double dy = (double) m_stVehicleInfo.nVelocity * cos(rad);
1023
1024                 m_stVehicleInfo.fLat += dx * 0.000003;
1025                 m_stVehicleInfo.fLng += dy * 0.000003;
1026
1027                 const char *vi = "LOCATION";
1028                 double location[] =
1029                     { m_stVehicleInfo.fLat, m_stVehicleInfo.fLng, 0 };
1030             }
1031         }
1032         else {
1033             if (!routeDriveFlg) {
1034                 printf("route Driving\n");
1035                 m_stVehicleInfo.fLat = routeList.front().lat;
1036                 m_stVehicleInfo.fLng = routeList.front().lng;
1037                 routeList.pop();
1038             }
1039             routeDriveFlg = true;
1040             next = routeList.front();
1041             dy = next.lat - m_stVehicleInfo.fLat;
1042             dx = next.lng - m_stVehicleInfo.fLng;
1043             theta = (double) atan2(dy, dx) * 180.0 / (atan(1.0) * 4.0);
1044             if (theta < 0) {
1045                 theta = 360.0 + theta;
1046             }
1047
1048             rad = (theta / 180.0) * PIE;
1049
1050             m_stVehicleInfo.nDirection = 90 - theta;
1051             if (m_stVehicleInfo.nDirection < 0) {
1052                 m_stVehicleInfo.nDirection = 360 + m_stVehicleInfo.nDirection;
1053             }
1054
1055             {
1056                 const char *vi = "DIRECTION";
1057                 SendVehicleInfo(dataport_def, vi, m_stVehicleInfo.nDirection);
1058             }
1059             m_stVehicleInfo.fLat +=
1060                 (m_stVehicleInfo.nVelocity * sin(rad) / (72 * 1000)) /
1061                 (cos((m_stVehicleInfo.fLat / 180) * PIE) * 111.111);
1062             m_stVehicleInfo.fLng +=
1063                 (m_stVehicleInfo.nVelocity * cos(rad) / (72 * 1000)) * (1 /
1064                                                                         111.111);
1065
1066             {
1067                 const char *vi = "LOCATION";
1068                 double location[] =
1069                     { m_stVehicleInfo.fLat, m_stVehicleInfo.fLng, 0 };
1070             }
1071
1072             if (rad == 0) {
1073                 if (m_stVehicleInfo.fLng >= next.lng) {
1074                     nextPointFlg = true;
1075                 }
1076             }
1077             else if (rad > 0 && rad < 0.5 * PIE) {
1078                 if (m_stVehicleInfo.fLng >= next.lng
1079                     && m_stVehicleInfo.fLat >= next.lat) {
1080                     nextPointFlg = true;
1081                 }
1082             }
1083             else if (rad == 0.5 * PIE) {
1084                 if (m_stVehicleInfo.fLat >= next.lat) {
1085                     nextPointFlg = true;
1086                 }
1087             }
1088             else if (rad > 0.5 * PIE && rad < PIE) {
1089                 if (m_stVehicleInfo.fLng <= next.lng
1090                     && m_stVehicleInfo.fLat >= next.lat) {
1091                     nextPointFlg = true;
1092                 }
1093             }
1094             else if (rad == PIE) {
1095                 if (m_stVehicleInfo.fLng <= next.lng) {
1096                     nextPointFlg = true;
1097                 }
1098             }
1099             else if (rad > PIE && rad < 1.5 * PIE) {
1100                 if (m_stVehicleInfo.fLng <= next.lng
1101                     && m_stVehicleInfo.fLat <= next.lat) {
1102                     nextPointFlg = true;
1103                 }
1104             }
1105             else if (rad == 1.5 * PIE) {
1106                 if (m_stVehicleInfo.fLat <= next.lat) {
1107                     nextPointFlg = true;
1108                 }
1109             }
1110             else {
1111                 if (m_stVehicleInfo.fLng >= next.lng
1112                     && m_stVehicleInfo.fLat <= next.lat) {
1113                     nextPointFlg = true;
1114                 }
1115             }
1116
1117             if (nextPointFlg) {
1118                 if (routeList.empty()) {
1119                     LoadRouteList();
1120                     m_stVehicleInfo.fLng = myConf.m_fLng;
1121                     m_stVehicleInfo.fLat = myConf.m_fLat;
1122                 }
1123                 next = routeList.front();
1124                 routeList.pop();
1125                 nextPointFlg = false;
1126             }
1127         }
1128     }
1129
1130     pthread_join(thread[0], NULL);
1131     pthread_join(thread[1], NULL);
1132     while (!routeList.empty()) {
1133         routeList.pop();
1134     }
1135     pthread_mutex_destroy(&mutex);
1136
1137 }
1138
1139 void *Comm(void *s)
1140 {
1141     struct KeyDataMsg_t ret;
1142     int *recvid = (int *) s;
1143
1144     while (g_bStopFlag) {
1145         if (msgrcv(*recvid, &ret, sizeof(struct KeyDataMsg_t), 31, MSG_EXCEPT)
1146             == -1) {
1147             return (void *) NULL;
1148         }
1149     }
1150     return 0;
1151 }
1152
1153
1154 /*--------------------------------------------------------------------------*/
1155 /**
1156  * @brief   function of vehicle information access
1157  *
1158  * @param[in]   SigNo       signal number
1159  * @return      none
1160  */
1161 /*--------------------------------------------------------------------------*/
1162 void DaemonTerminate(int SigNo)
1163 {
1164
1165     if ((SigNo > 0) &&
1166         (SigNo != SIGHUP) && (SigNo != SIGINT) && (SigNo != SIGTERM)) {
1167         (void) signal(SigNo, DaemonTerminate);
1168         return;
1169     }
1170
1171     exit(0);
1172 }
1173
1174 /*--------------------------------------------------------------------------*/
1175 /**
1176  * @brief   function of vehicle information set to AMB
1177  *
1178  * @param[in]   sendque_id  que id for sending
1179  * @param[in]   recvque_id  que id for receiving
1180  * @param[in]   mtype       priority
1181  * @param[in]   key         name of vehicle information
1182  * @param[in]   comstat     common_status
1183  * @param[in]   data        value of vehicle information
1184  * @param[in]   len         length of vehicle information
1185  * @param[out]  ret         struct from AMB
1186  * @return  bool    true:success,false:fail
1187  */
1188 /*--------------------------------------------------------------------------*/
1189 bool CGtCtrl::SendVehicleInfo( /*int & send_id, long mtype, */
1190                               ProtocolType type, const char *key, bool data)
1191 {
1192     return sendVehicleInfo( /*send_id, mtype, */ type, key, (void *) &data,
1193                            sizeof(bool), 1);
1194 }
1195
1196 bool CGtCtrl::SendVehicleInfo( /*int & send_id, long mtype, */
1197                               ProtocolType type, const char *key, int data)
1198 {
1199     return sendVehicleInfo( /*send_id, mtype, */ type, key, (void *) &data,
1200                            sizeof(int), 1);
1201 }
1202
1203 bool CGtCtrl::SendVehicleInfo( /*int & send_id, long mtype, */
1204                               ProtocolType type, const char *key, int data[],
1205                               int len)
1206 {
1207     return sendVehicleInfo( /*send_id, mtype, */ type, key, (void *) data,
1208                            sizeof(int), len);
1209 }
1210
1211 bool CGtCtrl::SendVehicleInfo(/*int & send_id, long mtype */
1212                               ProtocolType type, const char *key,
1213                               double data[], int len)
1214 {
1215     return sendVehicleInfo( /*send_id, mtype, */ type, key, (void *) data,
1216                            sizeof(double), len);
1217 }
1218
1219 bool CGtCtrl::SendVehicleInfo( /*int & send_id, long mtype, */
1220                               ProtocolType type, const char *key, char data[],
1221                               int len)
1222 {
1223     return sendVehicleInfo( /*send_id, mtype, */ type, key, (void *) data,
1224                            sizeof(char), len);
1225 }
1226
1227 /*--------------------------------------------------------------------------*/
1228 /**
1229  * @brief   vehicle information struct for AMB
1230  *
1231  * @param[in]   buf     buffer for vehicle information struct
1232  * @param[in]   bufsize size of buffer
1233  * @param[in]   mtype   priority
1234  * @param[in]   key     name of vehicle information
1235  * @param[in]   tm      time
1236  * @param[in]   nstat   common_status
1237  * @param[in]   status  value of vehicle information
1238  * @return  none
1239  */
1240 /*--------------------------------------------------------------------------*/
1241 void CGtCtrl::SetMQKeyData(char *buf, unsigned int bufsize, long &mtype,
1242                            const char *key, char status[], unsigned int size)
1243 {
1244     KeyDataMsg_t *tmp_t = (KeyDataMsg_t *) buf;
1245
1246     memset(buf, 0x00, bufsize);
1247     strcpy(tmp_t->KeyEventType, key);
1248     gettimeofday(&tmp_t->recordtime, NULL);
1249     tmp_t->data.common_status = 0;
1250     memcpy(&tmp_t->data.status[0], &status[0], size);
1251 }
1252
1253 /*--------------------------------------------------------------------------*/
1254 /**
1255  * @brief   set vehicle information for AMB
1256  *
1257  * @param[in]   sendque_id  queue ID
1258  * @param[in]   priority    priority of queue
1259  * @param[in]   key         KeyEventType
1260  * @param[in]   data        value of vehicle info
1261  * @param[in]   unit_size   size of send data unit
1262  * @param[in]   unit_cnt    number of send data unit
1263  * @return  bool    true:success,false:failure
1264  */
1265 /*--------------------------------------------------------------------------*/
1266 bool CGtCtrl::sendVehicleInfo( /*int & send_id, long priority, */
1267                               ProtocolType type, const char *key, void *data,
1268                               unsigned int unit_size, int unit_cnt)
1269 {
1270     long priority = 1;
1271
1272     if (unit_size == 0 || unit_cnt <= 0 || data == NULL)
1273         return false;
1274
1275     char adata[unit_size * unit_cnt];
1276     char mqMsg[sizeof(KeyDataMsg_t) + sizeof(adata)];
1277
1278     for (int i = 0; i < unit_cnt; i++) {
1279         int pos = i * unit_size;
1280         memcpy(&adata[pos], (char *) data + pos, unit_size);
1281     }
1282
1283     SetMQKeyData(&mqMsg[0], sizeof(mqMsg), priority, key, adata,
1284                  sizeof(adata));
1285
1286     if (!m_ambpicomm_client[type].
1287         send(reinterpret_cast < char *>(mqMsg), sizeof(mqMsg)))
1288     {
1289         std::cerr << "Failed to send data(" << errno << ")." << std::endl;
1290         return false;
1291     }
1292
1293     m_sendMsgInfo.push_back(string(key));
1294
1295     return true;
1296 }
1297
1298
1299 /*--------------------------------------------------------------------------*/
1300 /**
1301  * @brief   JOSN parser
1302  *
1303  * @param[in]   fname   file name of configuration file(full path)
1304  * @return  bool    false:failure
1305  */
1306 /*--------------------------------------------------------------------------*/
1307 bool CGtCtrl::LoadConfigJson(const char *fname)
1308 {
1309     char confpath[1024];
1310     JsonParser *parser = NULL;
1311     JsonNode *node = NULL;
1312     JsonReader *reader = NULL;
1313
1314     if (!LoadConfigAMBJson(fname, &confpath[0], sizeof(confpath))) {
1315         return false;
1316     }
1317
1318     //g_type_init();
1319     printf("conf=%s\nvehicleinfo conf=%s\n", fname, confpath);
1320
1321     parser = json_parser_new();
1322     GError *error = NULL;
1323
1324     json_parser_load_from_file(parser, confpath, &error);
1325     if (error) {
1326         printf("Failed to load config: %s\n", error->message);
1327         DelJsonObj(parser, reader);
1328         return false;
1329     }
1330
1331     node = json_parser_get_root(parser);
1332     if (node == NULL) {
1333         printf("Unable to get JSON root object.\n");
1334         DelJsonObj(parser, reader);
1335         return false;
1336     }
1337
1338     reader = json_reader_new(node);
1339     if (reader == NULL) {
1340         printf("Unable to create JSON reader\n");
1341         DelJsonObj(parser, reader);
1342         return false;
1343     }
1344
1345     if (!LoadConfigJsonCarSim(reader)) {
1346         DelJsonObj(parser, reader);
1347         return false;
1348     }
1349
1350     json_reader_set_root(reader, node);
1351     if (!LoadConfigJsonCommon(reader)) {
1352         DelJsonObj(parser, reader);
1353         return false;
1354     }
1355
1356     return true;
1357 }
1358
1359 /*--------------------------------------------------------------------------*/
1360 /**
1361  * @brief   release JSON parser object
1362  *
1363  * @param[in]   parser  parser object
1364  * @param[in]   node    node object
1365  * @param[in]   reader  reader object
1366  * @return  none
1367  */
1368 /*--------------------------------------------------------------------------*/
1369 void CGtCtrl::DelJsonObj(JsonParser *parser, JsonReader *reader)
1370 {
1371     if (reader != NULL)
1372         g_object_unref(reader);
1373     if (parser != NULL)
1374         g_object_unref(parser);
1375 }
1376
1377 /*--------------------------------------------------------------------------*/
1378 /**
1379  * @brief   get JSON file path form configuration
1380  *
1381  * @param[in]   fname       full path of configuration file
1382  * @param[out]  jsonfname   buffer of JSON file name
1383  * @param[in]   size        size of second argument buffer size
1384  * @return  bool        false:failure
1385  */
1386 /*--------------------------------------------------------------------------*/
1387 bool CGtCtrl::LoadConfigAMBJson(const char *fname, char *jsonfname, int size)
1388 {
1389     JsonParser *ps = NULL;
1390     JsonNode *nd = NULL;
1391     JsonReader *rd = NULL;
1392
1393     memset(jsonfname, 0x00, size);
1394     //g_type_init();
1395
1396     ps = json_parser_new();
1397     GError *error = NULL;
1398
1399     json_parser_load_from_file(ps, fname, &error);
1400     if (error) {
1401         printf("Failed to load AMBconfig: %s\n", error->message);
1402         DelJsonObj(ps, rd);
1403         return false;
1404     }
1405
1406     nd = json_parser_get_root(ps);
1407     if (nd == NULL) {
1408         printf("Unable to get AMB-JSON root object.\n");
1409         DelJsonObj(ps, rd);
1410         return false;
1411     }
1412
1413     rd = json_reader_new(nd);
1414     if (rd == NULL) {
1415         printf("Unable to create AMB-JSON reader\n");
1416         DelJsonObj(ps, rd);
1417         return false;
1418     }
1419
1420     json_reader_read_member(rd, "sources");
1421     error = (GError *) json_reader_get_error(rd);
1422     if (error != NULL) {
1423         printf("Error getting AMB sources member. %s\n", error->message);
1424         DelJsonObj(ps, rd);
1425         return false;
1426     }
1427     g_assert(json_reader_is_array(rd));
1428     for (int i = 0; i < json_reader_count_elements(rd); i++) {
1429         json_reader_read_element(rd, i);
1430         json_reader_read_member(rd, "name");
1431         std::string section = json_reader_get_string_value(rd);
1432         json_reader_end_member(rd);
1433         if ("VehicleSource" == section) {
1434             char val[size];
1435             if (GetConfigValue(rd, "configfile", &val[0], size)) {
1436                 strcpy(jsonfname, val);
1437             }
1438             break;
1439         }
1440         json_reader_end_element(rd);
1441     }
1442     json_reader_end_member(rd);
1443     DelJsonObj(ps, rd);
1444     return true;
1445 }
1446
1447 /*--------------------------------------------------------------------------*/
1448 /**
1449  * @brief   get JSON value(CarSim)
1450  *
1451  * @param[in]   reader  JSON reader object
1452  * @return  bool    false:failure
1453  */
1454 /*--------------------------------------------------------------------------*/
1455 bool CGtCtrl::LoadConfigJsonCarSim(JsonReader *reader)
1456 {
1457     json_reader_read_member(reader, "Config");
1458     const GError *confreaderror = json_reader_get_error(reader);
1459     if (confreaderror != NULL) {
1460         printf("Error getting sources member. %s\n", confreaderror->message);
1461         return false;
1462     }
1463     g_assert(json_reader_is_array(reader));
1464
1465     int elementnum = json_reader_count_elements(reader);
1466     for (int i = 0; i < elementnum; i++) {
1467         json_reader_read_element(reader, i);
1468         json_reader_read_member(reader, "Section");
1469         std::string section = json_reader_get_string_value(reader);
1470         json_reader_end_member(reader);
1471
1472         if ("CarSim" == section) {
1473             const char *key1 = "DefaultInfoMQKey";
1474             const char *key2 = "CustomizeInfoMQKey";
1475             const char *key3 = "VehicleInfoList";
1476             char val[9];
1477
1478             if (!json_reader_read_member(reader, key3)) {
1479                 confreaderror = json_reader_get_error(reader);
1480                 printf("Error getting %s. %s\n", key3,
1481                        confreaderror->message);
1482                 return false;
1483             }
1484             else {
1485                 confreaderror = json_reader_get_error(reader);
1486                 if (confreaderror != NULL) {
1487                     confreaderror = json_reader_get_error(reader);
1488                     printf("Error getting %s member. %s\n", key3,
1489                            confreaderror->message);
1490                     return false;
1491                 }
1492                 g_assert(json_reader_is_array(reader));
1493                 m_viList.length = json_reader_count_elements(reader);
1494                 for (int j = 0; j < m_viList.length; j++) {
1495                     json_reader_read_element(reader, j);
1496                     std::string str = json_reader_get_string_value(reader);
1497                     strcpy(m_viList.name[j], str.c_str());
1498                     json_reader_end_element(reader);
1499                 }
1500                 json_reader_end_member(reader);
1501             }
1502         }
1503         json_reader_end_element(reader);
1504     }
1505     json_reader_end_member(reader);
1506
1507     return true;
1508 }
1509
1510 /*--------------------------------------------------------------------------*/
1511 /**
1512  * @brief   get JSON value(Common)
1513  *
1514  * @param[in]   reader  JSON reader object
1515  * @return  bool    false:failure
1516  */
1517 /*--------------------------------------------------------------------------*/
1518 bool CGtCtrl::LoadConfigJsonCommon(JsonReader *reader)
1519 {
1520     json_reader_read_member(reader, "Config");
1521     const GError *confreaderror = json_reader_get_error(reader);
1522     if (confreaderror != NULL) {
1523         printf("Error getting sources member. %s\n", confreaderror->message);
1524         return false;
1525     }
1526     g_assert(json_reader_is_array(reader));
1527
1528     int elementnum = json_reader_count_elements(reader);
1529     int i;
1530     for (i = 0; i < elementnum; i++) {
1531         json_reader_read_element(reader, i);
1532         if (!json_reader_read_member(reader, "Section")) {
1533             confreaderror = json_reader_get_error(reader);
1534             printf("Error getting Section. %s\n", confreaderror->message);
1535             return false;
1536         }
1537         std::string section = json_reader_get_string_value(reader);
1538         json_reader_end_member(reader);
1539
1540         if ("Common" == section) {
1541             const char *key1 = "VehicleInfoDefine";
1542
1543             if (!json_reader_read_member(reader, key1)) {
1544                 confreaderror = json_reader_get_error(reader);
1545                 printf("Error getting %s. %s\n", key1,
1546                        confreaderror->message);
1547                 return false;
1548             }
1549
1550             confreaderror = json_reader_get_error(reader);
1551             if (confreaderror != NULL) {
1552                 confreaderror = json_reader_get_error(reader);
1553                 printf("Error getting %s member. %s\n", key1,
1554                        confreaderror->message);
1555                 return false;
1556             }
1557             g_assert(json_reader_is_array(reader));
1558
1559             for (int j = 0; j < json_reader_count_elements(reader); j++) {
1560                 json_reader_read_element(reader, j);
1561                 if (!json_reader_read_member(reader, "KeyEventType")) {
1562                     confreaderror = json_reader_get_error(reader);
1563                     printf("Error getting %s. %s\n", key1,
1564                            confreaderror->message);
1565                     continue;
1566                 }
1567                 else {
1568                     std::string str = json_reader_get_string_value(reader);
1569                     json_reader_end_member(reader);
1570                     if (m_viList.isContainVehicleName(str.c_str())) {
1571                         int priority;
1572                         if (GetConfigValue(reader, "Priority", &priority, 0)) {
1573                             m_viList.setPriority(str.c_str(), priority);
1574                         }
1575                     }
1576                 }
1577                 json_reader_end_element(reader);
1578             }
1579             json_reader_end_member(reader);
1580             const char key2[][32] = {
1581                 "DefaultInfoPort",
1582                 "CustomizeInfoPort"
1583             };
1584             const char *subkey1 = "DataPort";
1585             const char *subkey2 = "CtrlPort";
1586
1587             for (int i = 0; i < 2; i++) {
1588                 if (!json_reader_read_member(reader, key2[i])) {
1589                     confreaderror = json_reader_get_error(reader);
1590                     printf("Error getting %s. %s\n", key2[i],
1591                            confreaderror->message);
1592                     return false;
1593                 }
1594                 confreaderror = json_reader_get_error(reader);
1595                 if (confreaderror != NULL) {
1596                     confreaderror = json_reader_get_error(reader);
1597                     printf("Error getting %s member. %s\n", key2[i],
1598                            confreaderror->message);
1599                     return false;
1600                 }
1601
1602                 if (!json_reader_read_member(reader, subkey1)) {
1603                     confreaderror = json_reader_get_error(reader);
1604                     printf("Error getting %s->%s. %s\n", key2[i],
1605                            subkey1, confreaderror->message);
1606                     return false;
1607                 }
1608                 confreaderror = json_reader_get_error(reader);
1609                 if (confreaderror != NULL) {
1610                     confreaderror = json_reader_get_error(reader);
1611                     printf("Error getting %s->%s member. %s\n", key2[i],
1612                            subkey1, confreaderror->message);
1613                     return false;
1614                 }
1615                 m_ambpicomm_port[i * 2] = json_reader_get_int_value(reader);
1616                 json_reader_end_member(reader);
1617
1618                 if (!json_reader_read_member(reader, subkey2)) {
1619                     confreaderror = json_reader_get_error(reader);
1620                     printf("Error getting %s->%s. %s\n", key2[i],
1621                            subkey2, confreaderror->message);
1622                     return false;
1623                 }
1624                 confreaderror = json_reader_get_error(reader);
1625                 if (confreaderror != NULL) {
1626                     confreaderror = json_reader_get_error(reader);
1627                     printf("Error getting %s->%s member. %s\n", key2[i],
1628                            subkey2, confreaderror->message);
1629                     return false;
1630                 }
1631                 m_ambpicomm_port[i * 2 + 1] =
1632                     json_reader_get_int_value(reader);
1633                 json_reader_end_member(reader);
1634                 json_reader_end_member(reader);
1635             }
1636         }
1637         json_reader_end_element(reader);
1638     }
1639     json_reader_end_member(reader);
1640
1641     return true;
1642 }
1643
1644 /*--------------------------------------------------------------------------*/
1645 /**
1646  * @brief   get value form configuration file(text data)
1647  *
1648  * @param[in]   r       JsonReader object
1649  * @param[in]   key     key
1650  * @param[out]  buf     data buffer
1651  * @param[in]   size    size of data buffer
1652  * @return  bool    false:failure
1653  */
1654 /*--------------------------------------------------------------------------*/
1655 bool CGtCtrl::GetConfigValText(JsonReader *r, const char *key, char *buf,
1656                                int size)
1657 {
1658     std::string val;
1659     int len;
1660
1661     memset(buf, 0x00, size);
1662     if (!json_reader_read_member(r, key)) {
1663         printf("Error getting string value.\n");
1664         return false;
1665     }
1666     else {
1667         val = json_reader_get_string_value(r);
1668         json_reader_end_member(r);
1669         len =
1670             (int) val.length() > (size - 1) ? (size - 1) : (int) val.length();
1671         memcpy(buf, val.c_str(), len);
1672     }
1673     return true;
1674 }
1675
1676 /*--------------------------------------------------------------------------*/
1677 /**
1678  * @brief   get value form configuration file(integer data)
1679  *
1680  * @param[in]   r       JsonReader object
1681  * @param[in]   key     key
1682  * @param[out]  buf     data buffer
1683  * @return  bool    false:failure
1684  */
1685 /*--------------------------------------------------------------------------*/
1686 bool CGtCtrl::GetConfigValInt(JsonReader *r, const char *key, int *buf)
1687 {
1688
1689     *buf = 0;
1690     if (!json_reader_read_member(r, key)) {
1691         printf("Error getting integer value.\n");
1692         return false;
1693     }
1694     else {
1695         *buf = (int) json_reader_get_int_value(r);
1696         json_reader_end_member(r);
1697     }
1698     return true;
1699 }
1700
1701 /*--------------------------------------------------------------------------*/
1702 /**
1703  * @brief   get value form configuration file(double data)
1704  *
1705  * @param[in]   r       JsonReader object
1706  * @param[in]   key     key
1707  * @param[out]  buf     data buffer
1708  * @return  bool    false:failure
1709  */
1710 /*--------------------------------------------------------------------------*/
1711 bool CGtCtrl::GetConfigValDouble(JsonReader *r, const char *key, double *buf)
1712 {
1713
1714     *buf = 0.0;
1715     if (!json_reader_read_member(r, key)) {
1716         printf("Error getting floating point value.\n");
1717         return false;
1718     }
1719     else {
1720         *buf = (double) json_reader_get_double_value(r);
1721         json_reader_end_member(r);
1722     }
1723     return true;
1724 }
1725
1726 /*--------------------------------------------------------------------------*/
1727 /**
1728  * @brief   get value form configuration file(boolean data)
1729  *
1730  * @param[in]   r       JsonReader object
1731  * @param[in]   key     key
1732  * @param[out]  buf     data buffer
1733  * @return  bool    false:failure
1734  */
1735 /*--------------------------------------------------------------------------*/
1736 bool CGtCtrl::GetConfigValBool(JsonReader *r, const char *key, bool *buf)
1737 {
1738
1739     *buf = false;
1740     if (!json_reader_read_member(r, key)) {
1741         printf("Error getting boolean value.\n");
1742         return false;
1743     }
1744     else {
1745         *buf = (double) json_reader_get_boolean_value(r);
1746         json_reader_end_member(r);
1747     }
1748     return true;
1749 }
1750
1751 /*--------------------------------------------------------------------------*/
1752 /**
1753  * @brief   get value form configuration file
1754  *
1755  * @param[in]   r       JsonReader object
1756  * @param[in]   key     key
1757  * @param[out]  buf     data buffer
1758  * @param[in]   size    size of buf
1759  * @return  bool    false:failure
1760  */
1761 /*--------------------------------------------------------------------------*/
1762 bool CGtCtrl::GetConfigValue(JsonReader *r, const char *key, char *buf,
1763                              int size)
1764 {
1765     return GetConfigValText(r, key, buf, size);
1766 }
1767
1768 bool CGtCtrl::GetConfigValue(JsonReader *r, const char *key, int *buf,
1769                              int size)
1770 {
1771     return GetConfigValInt(r, key, buf);
1772 }
1773
1774 bool CGtCtrl::GetConfigValue(JsonReader *r, const char *key, double *buf,
1775                              int size)
1776 {
1777     return GetConfigValDouble(r, key, buf);
1778 }
1779
1780 bool CGtCtrl::GetConfigValue(JsonReader *r, const char *key, bool *buf,
1781                              int size)
1782 {
1783     return GetConfigValBool(r, key, buf);
1784 }
1785
1786
1787 void CGtCtrl::CheckSendResult(int mqid)
1788 {
1789
1790     KeyDataMsg_t ret;
1791
1792     while (1) {
1793         if (msgrcv(mqid, &ret, sizeof(KeyDataMsg_t), 31, IPC_NOWAIT) == -1) {
1794             break;
1795         }
1796         else {
1797             // ERROR
1798             list < string >::iterator pos;
1799             pos =
1800                 find(m_sendMsgInfo.begin(), m_sendMsgInfo.end(),
1801                      string(ret.KeyEventType));
1802             if (pos != m_sendMsgInfo.end()) {
1803                 printf("send error: AMB cannot receive %s\n",
1804                        ret.KeyEventType);
1805             }
1806         }
1807     }
1808 }
1809
1810 void CGtCtrl::LoadRouteList() {
1811     std::cout << "LoadRouteList:orgmsgQueue[" << orgmsgQueue << "]\n";
1812     if (orgmsgQueue == "") {
1813         std::ifstream fin;
1814         fin.open(g_RouteListFile.c_str());
1815         if (!fin) {
1816             std::cerr << "Can't read " << g_RouteListFile << std::endl;
1817         }
1818         std::string line;
1819         while (fin && getline(fin, line)) {
1820             orgmsgQueue.append(line);
1821             orgmsgQueue.append("\n");
1822         }
1823         msgQueue = orgmsgQueue;
1824         fin.close();
1825     }
1826     else {
1827         msgQueue = orgmsgQueue;
1828     }
1829     if (msgQueue.size() > 0) {
1830         char buf[32];
1831         int pos = 0;
1832         int lastidx = 0;
1833         geoData tmpGeo;
1834         int i = 0;
1835
1836         tmpGeo.lat = GEORESET;
1837         tmpGeo.lng = GEORESET;
1838
1839         for (i = 0; i < (int) msgQueue.size(); i++) {
1840             if (msgQueue[i] == ',') {
1841                 tmpGeo.lat = atof(buf);
1842                 pos = 0;
1843             }
1844             else if (msgQueue[i] == '\n') {
1845                 tmpGeo.lng = atof(buf);
1846                 pos = 0;
1847
1848                 routeList.push(tmpGeo);
1849                 lastidx = i;
1850                 tmpGeo.lat = GEORESET;
1851                 tmpGeo.lng = GEORESET;
1852             }
1853             else {
1854                 buf[pos++] = msgQueue[i];
1855             }
1856         }
1857     }
1858 }
1859
1860 /**
1861  * End of File. (CGtCtrl.cpp)
1862  */