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