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