src/CGtCtrl.cpp

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