BugFix: A establishment of websockets fails.

BugFix: A gear change event is sometimes lost.
Add "--uid" command option.

Change-Id: I3aa0a80cbab8eaad6e3e88d51ee702f28ccb86e1
Signed-off-by: Masayuki Sasaki <masayuki.sasaki@mail.toyota-td.jp>

diff --git a/ico-vic-carsim.service b/ico-vic-carsim.service
index 85aa087..e12167a 100644 (file)
--- a/ico-vic-carsim.service
+++ b/ico-vic-carsim.service
@@ -7,4 +7,4 @@ Description=Carsimulator supported by G25 and G27.
 After=ambd.service
 
 [Service]
-ExecStart=/usr/bin/ico-vic-carsim
+ExecStart=/usr/bin/ico-vic-carsim --uid 5000

diff --git a/packaging/ico-vic-carsimulator.changes b/packaging/ico-vic-carsimulator.changes
index 076b9ad..0552b41 100644 (file)
--- a/packaging/ico-vic-carsimulator.changes
+++ b/packaging/ico-vic-carsimulator.changes
@@ -1,3 +1,9 @@
+* Mon Mar 17 2014 Shibata Makoto <shibata@mac.tec.toyota.co.jp> submit/tizen_ivi_release/20140108.220608@33934a4
+- 0.9.10 release
+-- bug fix: A establishment of websockets fails. 
+-- bug fix: A gear change event is sometimes lost.
+-- Add "--uid" command option.
+
 * Thu Dec 26 2013 Shibata Makoto <shibata@mac.tec.toyota.co.jp> accepted/tizen/ivi/20131217.231400@acfe75b
 - 0.9.08 release
 -- revise: add manifest file for SMACK label setting.

diff --git a/packaging/ico-vic-carsimulator.spec b/packaging/ico-vic-carsimulator.spec
index dd9dd28..e79f003 100644 (file)
--- a/packaging/ico-vic-carsimulator.spec
+++ b/packaging/ico-vic-carsimulator.spec
@@ -1,6 +1,6 @@
 Name:       ico-vic-carsimulator
 Summary:    CarSimulator
-Version:    0.9.08
+Version:    0.9.10
 Release:    1.1
 Group:      System Environment/Daemons
 License:    Apache 2.0

diff --git a/src/AmbpiComm.cpp b/src/AmbpiComm.cpp
index 7efe0cc..13bb397 100644 (file)
--- a/src/AmbpiComm.cpp
+++ b/src/AmbpiComm.cpp
@@ -16,6 +16,9 @@
 #include <vector>
 #include <unistd.h>
 #include "AmbpiComm.h"
+
+#include "ico-util/ico_log.h"
+
 using namespace std;
 
 static vector<AmbpiCommIF*> _commList;
@@ -164,6 +167,8 @@ bool AmbpiCommIF::send(const char *msg, const int size)
     reset_ercode();
     ico_uws_send(m_context, m_id, (unsigned char*)msg, (size_t)size);
     if ((ICO_UWS_ERR_UNKNOWN != m_ercode) && (ICO_UWS_ERR_NONE != m_ercode)) {
+        ICO_DBG("uri[%s], protocol[%s], m_context[%x], m_id[%x], msg[%s], size[%d]",
+            m_uri.c_str(), m_pNm.c_str(), (int)m_context, (int)m_id, msg, size);
         return false;
     }
     return true;
@@ -262,6 +267,8 @@ void AmbpiCommIF::event_cb(const ico_uws_evt_e event, const void *id,
     switch (event) {
     case ICO_UWS_EVT_RECEIVE:
     {
+        ICO_DBG("uri[%s], protocol[%s], ICO_UWS_EVT_RECEIVE[m_id=%x]",
+            m_uri.c_str(), m_pNm.c_str(), (int)m_id);
         if (NULL == d) {
             cerr << m_pNm << ":Failed Receive event" << endl;
             break;
@@ -283,15 +290,8 @@ void AmbpiCommIF::event_cb(const ico_uws_evt_e event, const void *id,
     case ICO_UWS_EVT_OPEN:
     {
         m_id = (void*)id;
-        if (0 != pthread_mutex_lock(&m_mutex)) {
-            cerr << m_pNm << ":Failed to lock mutex" << endl;
-        }
-        if (0 != pthread_cond_signal(&m_cond)) {
-            cerr << m_pNm << ":Failed to issue cond_signal" << endl;
-        }
-        if (0 != pthread_mutex_unlock(&m_mutex)) {
-            cerr << m_pNm << ":Failed to unlock mutex" << endl;
-        }
+        ICO_DBG("uri[%s], protocol[%s], ICO_UWS_EVT_OPEN[m_id=%x]",
+            m_uri.c_str(), m_pNm.c_str(), (int)m_id);
         break;
     }
     case ICO_UWS_EVT_ERROR:

diff --git a/src/AmbpiComm.h b/src/AmbpiComm.h
index f15bea9..9dad0f6 100644 (file)
--- a/src/AmbpiComm.h
+++ b/src/AmbpiComm.h
@@ -76,10 +76,14 @@ class AmbpiCommIF
     bool poll();
     static void *loop(void *arg);
     bool isContextMatch(const ico_uws_context* context) const;
-    bool threadCondWait();
+//    bool threadCondWait();
     void event_cb(const ico_uws_evt_e event, const void *id,
                   const ico_uws_detail *detail, void *user_data);
     void reset_ercode();
+    char *getM_id()
+    {
+        return (char *)m_id;
+    };
   private:
     bool init(const char* uri, const char* protocolName);
 
@@ -112,7 +116,7 @@ inline bool AmbpiCommIF::isContextMatch(const ico_uws_context* context) const
 /**
  * @brief thread wait
  * @return true:success / false:fail
- */
+ *//*
 inline bool AmbpiCommIF::threadCondWait()
 {
     if (0 != pthread_cond_wait(&m_cond, &m_mutex)) {
@@ -120,7 +124,7 @@ inline bool AmbpiCommIF::threadCondWait()
     }
     return true;
 }
-
+*/
 /**
  * @brief reset error code
  */

diff --git a/src/CConf.cpp b/src/CConf.cpp
index aa22de0..62aeb5a 100644 (file)
--- a/src/CConf.cpp
+++ b/src/CConf.cpp
@@ -63,9 +63,9 @@ void CConf::LoadConfig(const char*filePath)
     memset(devname, 0, sizeof(devname));
     CConf::GetConfig(m_strConfPath, "DEVICE", "NAME", "Driving Force GT", devname, sizeof(devname));
     m_sDeviceName = std::string(devname);
-       char ambconfig[64];
-       memset(ambconfig, 0, sizeof(ambconfig));
-       CConf::GetConfig(m_strConfPath, "AMBCONFIG", "NAME", "/etc/ambd/config", ambconfig, sizeof(ambconfig));
+    char ambconfig[64];
+    memset(ambconfig, 0, sizeof(ambconfig));
+    CConf::GetConfig(m_strConfPath, "AMBCONFIG", "NAME", "/etc/ambd/config", ambconfig, sizeof(ambconfig));
     m_sAmbConfigName = std::string(ambconfig);
 
     printf("Configuration[%s]:\n",m_sDeviceName.c_str());

diff --git a/src/CConf.h b/src/CConf.h
index 4f71efa..12f8ffb 100644 (file)
--- a/src/CConf.h
+++ b/src/CConf.h
@@ -70,7 +70,7 @@ class CConf
     double m_fLng;
     double m_fLat;
     std::string m_sDeviceName;
-       std::string m_sAmbConfigName;
+    std::string m_sAmbConfigName;
 
 };
 

diff --git a/src/CGtCtrl.cpp b/src/CGtCtrl.cpp
index 7f0e226..602bb54 100644 (file)
--- a/src/CGtCtrl.cpp
+++ b/src/CGtCtrl.cpp
@@ -21,6 +21,9 @@
 #include "CAvgCar.h"
 #include "CCalc.h"
 
+#include <sys/time.h>
+#include "ico-util/ico_log.h"
+
 extern bool gbDevJs;
 
 bool g_bStopFlag;
@@ -189,19 +192,20 @@ bool CGtCtrl::Initialize()
         if (false == m_ambpicomm_client[i].start(uri, protocolsName[i])) {
             return false;   // connect fail
         }
-        if (false == m_ambpicomm_client[i].threadCondWait()) {
-            std::cerr << "Failed to wait signal (" << i << ")" << std::endl;
+        while(m_ambpicomm_client[i].getM_id() == NULL) {
+            usleep(100*1000);
         }
+        ICO_DBG("websocket: OK [%s][%s]", uri, protocolsName[i]);
     }
 
     const char *vi = "LOCATION";
     double location[] = { myConf.m_fLat, myConf.m_fLng, 0 };
     SendVehicleInfo(dataport_def, vi, &location[0], 3);
 
-       if (m_bDemoRunning) {
-               std::cout << "Demo Mode." << std::endl;
-               LoadRouteList();
-       }
+    if (m_bDemoRunning) {
+        std::cout << "Demo Mode." << std::endl;
+        LoadRouteList();
+    }
 
     return true;
 }
@@ -234,9 +238,26 @@ void CGtCtrl::Run()
     int value = -1;
 
     /**
-      * INTERVAL WAIT COUNTER
+      * INTERVAL CONTROL
+      */
+    struct timeval now;
+    struct timeval timeout;
+    long usec;
+    int intervalctl = 0;
+    int fcycle = 0;
+    gettimeofday(&now, NULL);
+    usec = now.tv_usec + D_RUNLOOP_INTERVAL_COUNT*10*1000;
+    timeout.tv_usec = usec % 1000000;
+    if (usec < 1000000) {
+        timeout.tv_sec = now.tv_sec;
+    } else {
+        timeout.tv_sec = now.tv_sec + 1;
+    }
+    /**
+      * LAST STEERING VALUE
       */
-    int iwc = D_RUNLOOP_INTERVAL_COUNT;
+    int last_steering = 0;
+    int last_angle = 0;
     /**
      * RPM/Breake/Speed calc class
      */
@@ -259,9 +280,9 @@ void CGtCtrl::Run()
     int nShiftPosBK = -1;
     char shiftpos = 255;
 
-       bool nextPointFlg = false;
-       geoData next;
-       double dx, dy, rad, theta;
+    bool nextPointFlg = false;
+    geoData next;
+    double dx, dy, rad, theta;
 
     while (g_bStopFlag) {
 
@@ -269,37 +290,31 @@ void CGtCtrl::Run()
 
         m_sendMsgInfo.clear();
 
+        gettimeofday(&now, NULL);
+        if((timeout.tv_sec == now.tv_sec && timeout.tv_usec <= now.tv_sec)
+            || (timeout.tv_sec < now.tv_sec)) {
+            intervalctl = (intervalctl+1)%3;
+            if (intervalctl) {
+                fcycle = intervalctl;
+            }
+            usec = now.tv_usec + D_RUNLOOP_INTERVAL_COUNT*10*1000;
+            timeout.tv_usec = usec % 1000000;
+            if (usec < 1000000) {
+                timeout.tv_sec = now.tv_sec;
+            } else {
+                timeout.tv_sec = now.tv_sec + 1;
+            }
+        }
+//      ICO_DBG("intervalctl[%d], fcycle[%d]", intervalctl, fcycle);
+
         switch (type) {
         case JS_EVENT_AXIS:
             if (number == myConf.m_nSteering) {
-                               int angle = 0;
+                last_steering = value;
                 if (value != 0) {
                     m_stVehicleInfo.nSteeringAngle +=
                         (value * 10 / 65536 - m_stVehicleInfo.nSteeringAngle);
                 }
-                               if (value < 0) {
-                                       angle = (((double)value / 32768) * 450) + 450 + 270;
-                                       angle %= 360;
-                                       //std:: cout << "value * (450 / 32768) = " << ((double)value / 32768) * 450 << "\n";
-                                       //std:: cout << "value * (450 / 32768) + 450 = " << (double)value * (450 / 32768) + 450 << "\n";
-                                       //std:: cout << "value * (450 / 32768) + 450 + 270 = " << (double)value * (450 / 32768) + 450 + 270 << "\n";
-                               }
-                               else {
-                                       angle = ((double)value / 32768) * 450;
-                                       angle %= 360;
-                                       //std:: cout << "value * (450 / 32768) = " << ((double)value / 32768) * 450 << "\n";
-                               }
-                               //std::cout << "Steering value = " << value << " -> angle = " << angle << std::endl;
-                {
-                    const char *vi = "STEERING";
-
-                                       /*
-                    SendVehicleInfo(dataport_def,
-                                    vi, m_stVehicleInfo.nSteeringAngle);
-                                       */
-                    SendVehicleInfo(dataport_def,
-                                    vi, angle);
-                }
             }
             else if (number == myConf.m_nAccel) {
                 if (0 >= value) {
@@ -521,19 +536,7 @@ void CGtCtrl::Run()
             break;
         }
         pmCar.updateAvg();
-        /**
-         * RPM check
-         */
-        int rpmNEW = (int)pmCar.getRPM();
-        if ((iwc==0)&&(rpmNEW != nRPM)) {
-            SendVehicleInfo(dataport_def, sENGINE_SPEED, rpmNEW);
-            nRPM = rpmNEW; // update value
-        }
-        int apoNEW = pmCar.calcAccPedalOpen();
-        if (apoNEW != nAccPedalOpen) {
-            SendVehicleInfo(dataport_def, sACCPEDAL_OPEN, apoNEW);
-            nAccPedalOpen = apoNEW;
-        }
+        /* RealTime */
         /**
          * BRAKE SIGNAL
          */
@@ -545,28 +548,6 @@ void CGtCtrl::Run()
             bBrakeSig = bBrakeNEW; // update value
         }
         /**
-         * BRAKE PRESSURE
-         */
-        int pressureNEW = pmCar.calcPressure(pmCar.getBrakeAvg());
-        m_stVehicleInfo.nBrakeHydraulicPressure = pressureNEW;
-        if (pressureNEW != nBrakePressure) {
-            SendVehicleInfo(dataport_def, sBRAKE_PRESSURE, pressureNEW);
-            nBrakePressure = pressureNEW;
-        }
-
-        /**
-         * VELOCITY (SPEED)
-         */
-        int speedNew = (int)pmCar.getSpeed();
-               if (speedNew > 180) {
-                       speedNew = 180;
-               }
-        if (speedNew != nSpeed) {
-            SendVehicleInfo(dataport_def, sVELOCITY, speedNew);
-            m_stVehicleInfo.nVelocity = speedNew;
-            nSpeed = speedNew;
-        }
-        /**
          * SHIFT
          */
         if (nShiftPosBK != m_stVehicleInfo.nShiftPos) {
@@ -584,137 +565,195 @@ void CGtCtrl::Run()
             data[2] = pmCar.getMode();
             SendVehicleInfo(dataport_def, sSHIFT, &data[0], ShiftSz);
             nShiftPosBK = m_stVehicleInfo.nShiftPos;
+//          ICO_DBG("SHIFT: Send [%d, %d, %d]", data[0], data[1], data[2]);
         }
 
-        if (iwc == 0) {
-                       if (m_bDemoRunning) {
-                               next = routeList.front();
-                               dy = next.lat - m_stVehicleInfo.fLat;
-                               dx = next.lng - m_stVehicleInfo.fLng;
-                               theta = (double) atan2(dy, dx) * 180.0 / (atan(1.0) * 4.0);
-                               if (theta < 0) {
-                                       theta = 360.0 + theta;
-                               }
-
-                               rad = (theta / 180.0) * PIE;
-
-                               m_stVehicleInfo.nDirection = 90 - theta;
-                               if (m_stVehicleInfo.nDirection < 0) {
-                                       m_stVehicleInfo.nDirection = 360 + m_stVehicleInfo.nDirection;
-                               }
-                               SendVehicleInfo(dataport_def, sDIRECTION, m_stVehicleInfo.nDirection);
-
-                               dir = static_cast<double>(m_stVehicleInfo.nDirection);
-                               double runMeters = pmCar.getTripmeter();
-                               pmCar.tripmeterReset();
-                               double tmpLat = m_stVehicleInfo.fLat;
-                               double tmpLng = m_stVehicleInfo.fLng;
-                               POINT pNEW = CalcDest(tmpLat, tmpLng, dir, runMeters);
-                               double tmpLct[] = { pNEW.lat, pNEW.lng, 0 };
-                               SendVehicleInfo(dataport_def, sLOCATION, &tmpLct[0], 3);
-                               m_stVehicleInfo.fLat = pNEW.lat;
-                               m_stVehicleInfo.fLng = pNEW.lng;
-
-                               if (rad == 0) {
-                                       if (m_stVehicleInfo.fLng >= next.lng) {
-                                               nextPointFlg = true;
-                                       }
-                               }
-                               else if (rad > 0 && rad < 0.5 * PIE) {
-                                       if (m_stVehicleInfo.fLng >= next.lng
-                                               && m_stVehicleInfo.fLat >= next.lat) {
-                                               nextPointFlg = true;
-                                       }
-                               }
-                               else if (rad == 0.5 * PIE) {
-                                       if (m_stVehicleInfo.fLat >= next.lat) {
-                                               nextPointFlg = true;
-                                       }
-                               }
-                               else if (rad > 0.5 * PIE && rad < PIE) {
-                                       if (m_stVehicleInfo.fLng <= next.lng
-                                               && m_stVehicleInfo.fLat >= next.lat) {
-                                               nextPointFlg = true;
-                                       }
-                               }
-                               else if (rad == PIE) {
-                                       if (m_stVehicleInfo.fLng <= next.lng) {
-                                               nextPointFlg = true;
-                                       }
-                               }
-                               else if (rad > PIE && rad < 1.5 * PIE) {
-                                       if (m_stVehicleInfo.fLng <= next.lng
-                                               && m_stVehicleInfo.fLat <= next.lat) {
-                                               nextPointFlg = true;
-                                       }
-                               }
-                               else if (rad == 1.5 * PIE) {
-                                       if (m_stVehicleInfo.fLat <= next.lat) {
-                                               nextPointFlg = true;
-                                       }
-                               }
-                               else {
-                                       if (m_stVehicleInfo.fLng >= next.lng
-                                               && m_stVehicleInfo.fLat <= next.lat) {
-                                               nextPointFlg = true;
-                                       }
-                               }
-
-                               if (nextPointFlg) {
-                                       std::cout << "routeList.size() = " << routeList.size() << std::endl;
-                                       routeList.pop();
-                                       if (routeList.empty()) {
-                                               LoadRouteList();
-                                               m_stVehicleInfo.fLng = myConf.m_fLng;
-                                               m_stVehicleInfo.fLat = myConf.m_fLat;
-                                       }
-                                       next = routeList.front();
-                                       nextPointFlg = false;
-                               }
-                       }
-                       else {
-                               /**
-                                 * DIRECTION (AZIMUTH)
-                                 * Front wheel steering angle
-                                 */
-                               double runMeters = pmCar.getTripmeter();
-                               pmCar.tripmeterReset();
-                               if (0 != runMeters) {
-                                       double stear = (double)m_stVehicleInfo.nSteeringAngle;
-                                       double dirNEW = CalcAzimuth(dir, stear, runMeters);
-                                       int nDir = (int)dirNEW;
-                                       dir = dirNEW;
-                                       if (nDir != m_stVehicleInfo.nDirection) {
-                                               SendVehicleInfo(dataport_def, sDIRECTION, nDir);
-                                               m_stVehicleInfo.nDirection = nDir;
-                                       }
-                               }
-                               /**
-                                 * LOCATION
-                                 */
-                               if ((!m_bUseGps) && (0 != runMeters)) {
-                                       double tmpLat = m_stVehicleInfo.fLat;
-                                       double tmpLng = m_stVehicleInfo.fLng;
-                                       POINT pNEW = CalcDest(tmpLat, tmpLng, dir, runMeters);
-                                       if ((tmpLat != pNEW.lat) || (tmpLng != pNEW.lng)){
-                                               double tmpLct[] = { pNEW.lat, pNEW.lng, 0 };
-                                               SendVehicleInfo(dataport_def, sLOCATION, &tmpLct[0], 3);
-                                               m_stVehicleInfo.fLat = pNEW.lat;
-                                               m_stVehicleInfo.fLng = pNEW.lng;
-                                       }
-                               }
-                       } 
-        }
-        if (0 == iwc) {
-            iwc = D_RUNLOOP_INTERVAL_COUNT;
-        }
-        else {
-            iwc--;
+        /* 100ms cycle */
+        if (fcycle == 2) {
+            int angle = 0;
+            if (last_steering < 0) {
+                angle = (((double)last_steering / 32768) * 450) + 450 + 270;
+                angle %= 360;
+                //std:: cout << "last_steering * (450 / 32768) = " << ((double)last_steering / 32768) * 450 << "\n";
+                //std:: cout << "last_steering * (450 / 32768) + 450 = " << (double)last_steering * (450 / 32768) + 450 << "\n";
+                //std:: cout << "last_steering * (450 / 32768) + 450 + 270 = " << (double)last_steering * (450 / 32768) + 450 + 270 << "\n";
+            }
+            else {
+                angle = ((double)last_steering / 32768) * 450;
+                angle %= 360;
+                //std:: cout << "last_steering * (450 / 32768) = " << ((double)last_steering / 32768) * 450 << "\n";
+            }
+            //std::cout << "Steering last_steering = " << last_steering << " -> angle = " << angle << std::endl;
+            if (angle != last_angle) {
+                const char *vi = "STEERING";
+
+                /*
+                SendVehicleInfo(dataport_def,
+                                vi, m_stVehicleInfo.nSteeringAngle);
+                */
+                SendVehicleInfo(dataport_def, vi, angle);
+                last_angle = angle;
+            }
+            /**
+             * RPM check
+             */
+            int rpmNEW = (int)pmCar.getRPM();
+            if (rpmNEW != nRPM) {
+                SendVehicleInfo(dataport_def, sENGINE_SPEED, rpmNEW);
+                nRPM = rpmNEW; // update value
+            }
+            int apoNEW = pmCar.calcAccPedalOpen();
+            if (apoNEW != nAccPedalOpen) {
+                SendVehicleInfo(dataport_def, sACCPEDAL_OPEN, apoNEW);
+                nAccPedalOpen = apoNEW;
+            }
+        } /* if (fcycle == 2) */
+
+        /* 50ms cycle */
+        if (fcycle != 0) {
+            /**
+             * BRAKE PRESSURE
+             */
+            int pressureNEW = pmCar.calcPressure(pmCar.getBrakeAvg());
+            m_stVehicleInfo.nBrakeHydraulicPressure = pressureNEW;
+            if (pressureNEW != nBrakePressure) {
+                SendVehicleInfo(dataport_def, sBRAKE_PRESSURE, pressureNEW);
+                nBrakePressure = pressureNEW;
+            }
+
+            /**
+             * VELOCITY (SPEED)
+             */
+            int speedNew = (int)pmCar.getSpeed();
+            if (speedNew > 180) {
+                speedNew = 180;
+            }
+            if (speedNew != nSpeed) {
+                SendVehicleInfo(dataport_def, sVELOCITY, speedNew);
+                m_stVehicleInfo.nVelocity = speedNew;
+                nSpeed = speedNew;
+            }
+
+            if (m_bDemoRunning) {
+                next = routeList.front();
+                dy = next.lat - m_stVehicleInfo.fLat;
+                dx = next.lng - m_stVehicleInfo.fLng;
+                theta = (double) atan2(dy, dx) * 180.0 / (atan(1.0) * 4.0);
+                if (theta < 0) {
+                    theta = 360.0 + theta;
+                }
+
+                rad = (theta / 180.0) * PIE;
+
+                m_stVehicleInfo.nDirection = 90 - theta;
+                if (m_stVehicleInfo.nDirection < 0) {
+                    m_stVehicleInfo.nDirection = 360 + m_stVehicleInfo.nDirection;
+                }
+                SendVehicleInfo(dataport_def, sDIRECTION, m_stVehicleInfo.nDirection);
+
+                dir = static_cast<double>(m_stVehicleInfo.nDirection);
+                double runMeters = pmCar.getTripmeter();
+                pmCar.tripmeterReset();
+                double tmpLat = m_stVehicleInfo.fLat;
+                double tmpLng = m_stVehicleInfo.fLng;
+                POINT pNEW = CalcDest(tmpLat, tmpLng, dir, runMeters);
+                double tmpLct[] = { pNEW.lat, pNEW.lng, 0 };
+                SendVehicleInfo(dataport_def, sLOCATION, &tmpLct[0], 3);
+                m_stVehicleInfo.fLat = pNEW.lat;
+                m_stVehicleInfo.fLng = pNEW.lng;
+
+                if (rad == 0) {
+                    if (m_stVehicleInfo.fLng >= next.lng) {
+                        nextPointFlg = true;
+                    }
+                }
+                else if (rad > 0 && rad < 0.5 * PIE) {
+                    if (m_stVehicleInfo.fLng >= next.lng
+                        && m_stVehicleInfo.fLat >= next.lat) {
+                        nextPointFlg = true;
+                    }
+                }
+                else if (rad == 0.5 * PIE) {
+                    if (m_stVehicleInfo.fLat >= next.lat) {
+                        nextPointFlg = true;
+                    }
+                }
+                else if (rad > 0.5 * PIE && rad < PIE) {
+                    if (m_stVehicleInfo.fLng <= next.lng
+                        && m_stVehicleInfo.fLat >= next.lat) {
+                        nextPointFlg = true;
+                    }
+                }
+                else if (rad == PIE) {
+                    if (m_stVehicleInfo.fLng <= next.lng) {
+                        nextPointFlg = true;
+                    }
+                }
+                else if (rad > PIE && rad < 1.5 * PIE) {
+                    if (m_stVehicleInfo.fLng <= next.lng
+                        && m_stVehicleInfo.fLat <= next.lat) {
+                        nextPointFlg = true;
+                    }
+                }
+                else if (rad == 1.5 * PIE) {
+                    if (m_stVehicleInfo.fLat <= next.lat) {
+                        nextPointFlg = true;
+                    }
+                }
+                else {
+                    if (m_stVehicleInfo.fLng >= next.lng
+                        && m_stVehicleInfo.fLat <= next.lat) {
+                        nextPointFlg = true;
+                    }
+                }
+
+                if (nextPointFlg) {
+                    std::cout << "routeList.size() = " << routeList.size() << std::endl;
+                    routeList.pop();
+                    if (routeList.empty()) {
+                        LoadRouteList();
+                        m_stVehicleInfo.fLng = myConf.m_fLng;
+                        m_stVehicleInfo.fLat = myConf.m_fLat;
+                    }
+                    next = routeList.front();
+                    nextPointFlg = false;
+                }
+            }
+            else {
+                /**
+                  * DIRECTION (AZIMUTH)
+                  * Front wheel steering angle
+                  */
+                double runMeters = pmCar.getTripmeter();
+                pmCar.tripmeterReset();
+                if (0 != runMeters) {
+                    double stear = (double)m_stVehicleInfo.nSteeringAngle;
+                    double dirNEW = CalcAzimuth(dir, stear, runMeters);
+                    int nDir = (int)dirNEW;
+                    dir = dirNEW;
+                    if (nDir != m_stVehicleInfo.nDirection) {
+                        SendVehicleInfo(dataport_def, sDIRECTION, nDir);
+                        m_stVehicleInfo.nDirection = nDir;
+                    }
+                }
+                /**
+                  * LOCATION
+                  */
+                if ((!m_bUseGps) && (0 != runMeters)) {
+                    double tmpLat = m_stVehicleInfo.fLat;
+                    double tmpLng = m_stVehicleInfo.fLng;
+                    POINT pNEW = CalcDest(tmpLat, tmpLng, dir, runMeters);
+                    if ((tmpLat != pNEW.lat) || (tmpLng != pNEW.lng)){
+                        double tmpLct[] = { pNEW.lat, pNEW.lng, 0 };
+                        SendVehicleInfo(dataport_def, sLOCATION, &tmpLct[0], 3);
+                        m_stVehicleInfo.fLat = pNEW.lat;
+                        m_stVehicleInfo.fLng = pNEW.lng;
+                    }
+                }
+            } 
         }
-        /**
-         * Interval Wait
-         */
-        usleep(g_sleeptime);
+
+        fcycle = 0;
     }
 }
 
@@ -1077,13 +1116,13 @@ void CGtCtrl::Run2()
 
             if (nextPointFlg) {
                 if (routeList.empty()) {
-                                       LoadRouteList();
-                                       m_stVehicleInfo.fLng = myConf.m_fLng;
-                                       m_stVehicleInfo.fLat = myConf.m_fLat;
+                    LoadRouteList();
+                    m_stVehicleInfo.fLng = myConf.m_fLng;
+                    m_stVehicleInfo.fLat = myConf.m_fLat;
                 }
-                               next = routeList.front();
-                               routeList.pop();
-                               nextPointFlg = false;
+                next = routeList.front();
+                routeList.pop();
+                nextPointFlg = false;
             }
         }
     }
@@ -1769,53 +1808,53 @@ void CGtCtrl::CheckSendResult(int mqid)
 }
 
 void CGtCtrl::LoadRouteList() {
-       std::cout << "LoadRouteList:orgmsgQueue[" << orgmsgQueue << "]\n";
-       if (orgmsgQueue == "") {
-               std::ifstream fin;
-               fin.open(g_RouteListFile.c_str());
-               if (!fin) {
-                       std::cerr << "Can't read " << g_RouteListFile << std::endl;
-               }
-               std::string line;
-               while (fin && getline(fin, line)) {
-                       orgmsgQueue.append(line);
-                       orgmsgQueue.append("\n");
-               }
-               msgQueue = orgmsgQueue;
-               fin.close();
-       }
-       else {
-               msgQueue = orgmsgQueue;
-       }
-       if (msgQueue.size() > 0) {
-               char buf[32];
-               int pos = 0;
-               int lastidx = 0;
-               geoData tmpGeo;
-               int i = 0;
-
-               tmpGeo.lat = GEORESET;
-               tmpGeo.lng = GEORESET;
-
-               for (i = 0; i < (int) msgQueue.size(); i++) {
-                       if (msgQueue[i] == ',') {
-                               tmpGeo.lat = atof(buf);
-                               pos = 0;
-                       }
-                       else if (msgQueue[i] == '\n') {
-                               tmpGeo.lng = atof(buf);
-                               pos = 0;
-
-                               routeList.push(tmpGeo);
-                               lastidx = i;
-                               tmpGeo.lat = GEORESET;
-                               tmpGeo.lng = GEORESET;
-                       }
-                       else {
-                               buf[pos++] = msgQueue[i];
-                       }
-               }
-       }
+    std::cout << "LoadRouteList:orgmsgQueue[" << orgmsgQueue << "]\n";
+    if (orgmsgQueue == "") {
+        std::ifstream fin;
+        fin.open(g_RouteListFile.c_str());
+        if (!fin) {
+            std::cerr << "Can't read " << g_RouteListFile << std::endl;
+        }
+        std::string line;
+        while (fin && getline(fin, line)) {
+            orgmsgQueue.append(line);
+            orgmsgQueue.append("\n");
+        }
+        msgQueue = orgmsgQueue;
+        fin.close();
+    }
+    else {
+        msgQueue = orgmsgQueue;
+    }
+    if (msgQueue.size() > 0) {
+        char buf[32];
+        int pos = 0;
+        int lastidx = 0;
+        geoData tmpGeo;
+        int i = 0;
+
+        tmpGeo.lat = GEORESET;
+        tmpGeo.lng = GEORESET;
+
+        for (i = 0; i < (int) msgQueue.size(); i++) {
+            if (msgQueue[i] == ',') {
+                tmpGeo.lat = atof(buf);
+                pos = 0;
+            }
+            else if (msgQueue[i] == '\n') {
+                tmpGeo.lng = atof(buf);
+                pos = 0;
+
+                routeList.push(tmpGeo);
+                lastidx = i;
+                tmpGeo.lat = GEORESET;
+                tmpGeo.lng = GEORESET;
+            }
+            else {
+                buf[pos++] = msgQueue[i];
+            }
+        }
+    }
 }
 
 /**

diff --git a/src/CGtCtrl.h b/src/CGtCtrl.h
index e608524..c74d318 100644 (file)
--- a/src/CGtCtrl.h
+++ b/src/CGtCtrl.h
@@ -323,7 +323,7 @@ class CGtCtrl
     void SetMQKeyData(char *buf, unsigned int bufsize, long &mtype,
                       const char *key, char status[], unsigned int size);
     void CheckSendResult(int mqid);
-       void LoadRouteList();
+    void LoadRouteList();
 };
 
 #endif /* CGTCTRL_H_ */

diff --git a/src/CJoyStick.cpp b/src/CJoyStick.cpp
index c1def11..eb54d2a 100644 (file)
--- a/src/CJoyStick.cpp
+++ b/src/CJoyStick.cpp
@@ -23,6 +23,10 @@
 #include <dirent.h>
 #include <string.h>
 #include "CJoyStick.h"
+
+#include "ico-util/ico_log.h"
+#include <errno.h>
+
 using namespace std;
 
 /**
@@ -74,6 +78,11 @@ int CJoyStick::Open()
             }
             else {
                 fds.fd = m_nJoyStickID;
+                if (pthread_create(&m_threadid, NULL, CJoyStick::loop,
+                                   (void *) this) != 0) {
+                    cerr << "Failed to create thread." << endl;
+                    return -1;
+                }
                 break;
             }
         }
@@ -114,6 +123,48 @@ int CJoyStick::Close()
 }
 
 /**
+ * @brief joystick event read loop entrance
+ * @return
+ */
+void *CJoyStick::loop(void *arg)
+{
+    CJoyStick *src = reinterpret_cast < CJoyStick * >(arg);
+    return (void *) src->recvJS();
+}
+
+/**
+ * @brief joystick event read loop
+ * @return
+ */
+bool CJoyStick::recvJS()
+{
+    while(1) {
+        fds.events = POLLIN;
+        if (poll(&fds, 1, -1) < 0) {
+            ICO_ERR("poll error");
+            return false;
+        }
+        if (0 != (fds.revents & POLLIN)) {
+            /**
+             * read event
+             */
+            struct js_event jse;
+            int r;
+            while ((r = read(m_nJoyStickID, &jse, sizeof(jse))) > 0) {
+                queue.push(jse);
+                if (sizeof(jse) != r) {
+                    ICO_DBG("data not enough [%d/%d]", r, sizeof(jse));
+                }
+            }
+            if (0 > r && errno !=11) {  /* read error */
+                ICO_ERR("read error[ret=%d, errno=%d]", r, errno);
+            }
+        }
+    }
+    return false;
+}
+
+/**
  * @brief get input value
  * @retval  return kind of input(axis or button), if error occurred, return negative value
  * @param[in/out]   nubmer  number of button or axis
@@ -122,33 +173,24 @@ int CJoyStick::Close()
 int CJoyStick::Read(int *number, int *value)
 {
     struct js_event jse;
+    int r=-1;
 
-    fds.events = POLLIN;
-
-    if (poll(&fds, 1, 50) == 0) {
+    r = queue.pop(jse);
+    if (r != 0) {
+        *number = -1;
+        *value = -1;
         return -1;
     }
 
-    if (fds.revents & POLLIN) {
-        int r = read(m_nJoyStickID, &jse, sizeof(jse));
+    int type = jse.type;
+    type = type & JS_EVENT_INIT;
 
-        int type = jse.type;
-        type = type & JS_EVENT_INIT;
-
-        if ((r >= 0) && (type == 0)) {
-            r = jse.type & ~JS_EVENT_INIT;
-            *number = jse.number;
-            *value = jse.value;
-        }
-        else {
-            *number = -1;
-            *value = -1;
-        }
-        return r;
-    }
-    else {
-        return -1;
+    if (type == 0) {
+        r = jse.type & ~JS_EVENT_INIT;
+        *number = jse.number;
+        *value = jse.value;
     }
+    return r;
 }
 
 int CJoyStick::ReadData()

diff --git a/src/CJoyStick.h b/src/CJoyStick.h
index 1423c8d..9b15d25 100644 (file)
--- a/src/CJoyStick.h
+++ b/src/CJoyStick.h
@@ -22,13 +22,110 @@
 #include <string.h>
 #include <poll.h>
 #include <string>
-#include <vector>
+#include <queue>
+#include <pthread.h>
+#include <errno.h>
+#include <sys/time.h>
+#include "ico-util/ico_log.h"
 
 #define D_DEV_DIR_PATH      "/dev/input/"
 #define D_DEV_NAME_PARTS_JS "js"
 #define D_DEV_NAME_G25      "Driving Force GT"
 #define D_DEV_NAME_G27      "G27 Racing Wheel"
 
+template <typename T>
+class CJoyStickQueue
+{
+public:
+    CJoyStickQueue()
+    {
+/*
+        pthread_mutex_init(&mutex, NULL);
+        pthread_cond_init(&cond, NULL);
+*/
+        mutex = PTHREAD_MUTEX_INITIALIZER;
+        cond = PTHREAD_COND_INITIALIZER;
+    }
+
+    ~CJoyStickQueue()
+    {
+        if (pthread_mutex_destroy(&mutex) != 0) {
+            ICO_ERR("pthread_mutex_destroy error[errno=%d]", errno);
+        }
+        if (pthread_cond_destroy(&cond) != 0) {
+            ICO_ERR("pthread_cond_destroy error[errno=%d]", errno);
+        }
+    }
+
+    int size()
+    {
+        if (pthread_mutex_lock(&mutex) != 0) {
+            ICO_ERR("pthread_mutex_lock error[errno=%d]", errno);
+        }
+        int ret = mQueue.size();
+        if (pthread_mutex_unlock(&mutex) != 0) {
+            ICO_ERR("pthread_mutex_unlock error[errno=%d]", errno);
+        }
+        return ret;
+    }
+
+    int pop(T& item)
+    {
+        struct timeval now;
+        struct timespec timeout;
+        long nsec;
+
+        if (pthread_mutex_lock(&mutex) != 0) {
+            ICO_ERR("pthread_mutex_lock error[errno=%d]", errno);
+        }
+
+        if (!mQueue.size()) {
+            gettimeofday(&now, NULL);
+            nsec = now.tv_usec * 1000 + 50*1000000;
+            timeout.tv_nsec = nsec % 1000000000;
+            if (nsec < 1000000000) {
+                timeout.tv_sec = now.tv_sec;
+            } else {
+                timeout.tv_sec = now.tv_sec + 1;
+            }
+            if (pthread_cond_timedwait(&cond, &mutex, &timeout) != 0){
+                if (pthread_mutex_unlock(&mutex) != 0) {
+                    ICO_ERR("pthread_mutex_unlock error[errno=%d]", errno);
+                }
+                return -1;
+            }
+        }
+
+        item = mQueue.front();
+        mQueue.pop();
+
+        if (pthread_mutex_unlock(&mutex) != 0) {
+            ICO_ERR("pthread_mutex_unlock error[errno=%d]", errno);
+        }
+
+        return 0;
+    }
+
+    void push(T item)
+    {
+        if (pthread_mutex_lock(&mutex) != 0) {
+            ICO_ERR("pthread_mutex_lock error[errno=%d]", errno);
+        }
+
+        mQueue.push(item);
+        pthread_cond_signal(&cond);
+
+        if (pthread_mutex_unlock(&mutex) != 0) {
+            ICO_ERR("pthread_mutex_unlock error[errno=%d]", errno);
+        }
+    }
+
+private:
+    pthread_mutex_t mutex;
+    pthread_cond_t cond;
+    std::queue<T> mQueue;
+};
+
 class CJoyStick
 {
   public:
@@ -54,8 +151,10 @@ class CJoyStick
                     const std::vector<std::string>& matching,
                     std::vector<std::string>& filesList) const;
     virtual bool getDeviceName(int fd, char* devNM, size_t sz);
+    bool recvJS();
+    static void *loop(void *arg);
 
-protected:
+  protected:
     char m_strJoyStick[64];
     int m_nJoyStickID;
 
@@ -65,6 +164,9 @@ protected:
 
     fd_set m_stSet;
     struct pollfd fds;
+     CJoyStickQueue<struct js_event > queue;
+ private:
+    pthread_t  m_threadid;
 };
 
 #endif /* CJOYSTICK_H_ */

diff --git a/src/CJoyStickEV.cpp b/src/CJoyStickEV.cpp
index b846b53..14a4caa 100644 (file)
--- a/src/CJoyStickEV.cpp
+++ b/src/CJoyStickEV.cpp
@@ -35,6 +35,10 @@
 #include "CJoyStick.h"
 #include "CJoyStickEV.h"
 #include "CConf.h"
+
+#include "ico-util/ico_log.h"
+#include <errno.h>
+
 using namespace std;
 
 CJoyStickEV::CJoyStickEV()
@@ -70,6 +74,13 @@ int CJoyStickEV::Open()
     m_nJoyStickID = rfd;
 
     fds.fd = m_nJoyStickID;
+
+    if (pthread_create(&m_threadid, NULL, CJoyStickEV::loop,
+                       (void *) this) != 0) {
+        cerr << "Failed to create thread." << endl;
+        return -1;
+    }
+
     return m_nJoyStickID;
 }
 
@@ -86,6 +97,48 @@ int CJoyStickEV::Close()
 }
 
 /**
+ * @brief joystick event read loop entrance
+ * @return
+ */
+void *CJoyStickEV::loop(void *arg)
+{
+    CJoyStickEV *src = reinterpret_cast < CJoyStickEV * >(arg);
+    return (void *) src->recvEV();
+}
+
+/**
+ * @brief joystick event read loop
+ * @return
+ */
+bool CJoyStickEV::recvEV()
+{
+    while(1) {
+        fds.events = POLLIN;
+        if (poll(&fds, 1, -1) < 0) {
+            ICO_ERR("poll error");
+            return false;
+        }
+        if (0 != (fds.revents & POLLIN)) {
+            /**
+             * read event
+             */
+            struct input_event ie;
+            int rc;
+            while ((rc = read(m_nJoyStickID, &ie, sizeof(ie))) > 0) {
+                queue.push(ie);
+                if (sizeof(ie) != rc) {
+                    ICO_DBG("data not enough [%d/%d]", rc, sizeof(ie));
+                }
+            }
+            if (0 > rc && errno !=11) {  /* read error(errno:11=EAGAIN) */
+                ICO_ERR("read error[ret=%d, errno=%d]", rc, errno);
+            }
+        }
+    }
+    return false;
+}
+
+/**
  * @brief joystick event read
  * @param nubmer joystick event data(convert input_event.code)
  * @param value joystick event data(convert input_event.value)
@@ -95,24 +148,15 @@ int CJoyStickEV::Close()
 int CJoyStickEV::Read(int *num, int *val)
 {
     /**
-     * pollong
-     */
-    fds.events = POLLIN;
-    if (poll(&fds, 1, 50) == 0) {
-        return -1;
-    }
-
-    if (0 == (fds.revents & POLLIN)) {
-        return -1;
-    }
-    /**
      * read event
      */
     struct input_event ie;
-    int rc = read(m_nJoyStickID, &ie, sizeof(ie));
-    if (0 > rc) {  /* read error */
+    int ret;
+    ret = queue.pop(ie);
+    if (ret != 0) {
         return -1;
     }
+
     /**
      * event convert
      */

diff --git a/src/CJoyStickEV.h b/src/CJoyStickEV.h
index 0dc1a01..ce453c2 100644 (file)
--- a/src/CJoyStickEV.h
+++ b/src/CJoyStickEV.h
@@ -42,6 +42,8 @@ class CJoyStickEV : public CJoyStick
     int calc2p65535(int val, const struct input_absinfo& ai);
     int calc3p32767(int val, const struct input_absinfo& ai);
     int calc3p32767Reverse(int val, const struct input_absinfo& ai);
+    bool recvEV();
+    static void *loop(void *arg);
     enum {
         E_ABSX = 0, /* ABS_X */
         E_ABSY,     /* ABS_Y */
@@ -53,7 +55,9 @@ class CJoyStickEV : public CJoyStick
     };
     struct input_absinfo m_absInf[E_ABSMAX];
     std::string m_devName;
+    CJoyStickQueue<struct input_event > queue;
   private:
+    pthread_t  m_threadid;
 };
 
 #endif /* CJOYSTICKEV_H_ */

diff --git a/src/CarSim_Daemon.cpp b/src/CarSim_Daemon.cpp
index 719b341..923f5a2 100644 (file)
--- a/src/CarSim_Daemon.cpp
+++ b/src/CarSim_Daemon.cpp
@@ -14,9 +14,13 @@
 #include <sys/types.h>
 #include <iostream>
 #include <unistd.h>
-#include <getopt.h>
-#include <string>
+#include <strings.h>
+#include <stdlib.h>
+#include <errno.h>
+//#include <string>
 #include "CGtCtrl.h"
+#include "ico-util/ico_log.h"
+
 using namespace std;
 
 #define VERSION "0.1.2"
@@ -26,44 +30,64 @@ bool gbDevJs = false;
 int main(int argc, char **argv)
 {
     // parse cmd line
-    int result = 0;
     bool bUseGps = false;
     bool bTestMode = false;
     bool b;
     bool comFlg = false;
-       bool bDemoRunning = false;
+    bool bDemoRunning = false;
+    int ii;
 
+//    printf("ico-vic-carsim: userid='%s', uid=%d, euid=%d)\n", cuserid(NULL), getuid(), geteuid());
     // parse command line
-    while ((result = getopt(argc, argv, "jhvgctr")) != -1) {
-        switch (result) {
-        case 'h':
+    for (ii = 1; ii < argc; ii++) {
+        if (strcmp( argv[ii], "-h") == 0) {
             printf("Usage: CarSim_Daemon [-g]\n");
             printf("  -g\t Get GPS form smartphone\n");
             return 0;
-            break;
-        case 'v':
+        }
+        else if (strcmp( argv[ii], "-v") == 0) {
             printf("CarSim_Daemon version:%s\n", VERSION);
             return 0;
-            break;
-        case 'g':
+        }
+        else if (strcmp( argv[ii], "-g") == 0) {
             bUseGps = true;
-            break;
-        case 'c':
+        }
+        else if (strcmp( argv[ii], "-c") == 0) {
             printf("Using amb plug-in I/F\n");
             comFlg = true;
-            break;
-        case 't':
+        }
+        else if (strcmp( argv[ii], "-t") == 0) {
             bTestMode = true;
-            break;
-        case 'j':
+        }
+        else if (strcmp( argv[ii], "-j") == 0) {
             gbDevJs = true;
-            break;
-               case 'r':
-                       bDemoRunning = true;
-                       break;
+        }
+        else if (strcmp( argv[ii], "-r") == 0) {
+            bDemoRunning = true;
+        }
+        else if (strcasecmp( argv[ii], "--user") == 0) {
+            ii ++;
+            if (ii < argc)  {
+                if (strcmp(argv[ii], cuserid(NULL)) != 0)    {
+                    printf("ico-vic-carsim: abort(cannot run in a '%s' [uid=%d, euid=%d])\n", cuserid(NULL), getuid(), geteuid());
+                    return -1;
+                }
+            }
+        }
+        else if (strcasecmp( argv[ii], "--uid") == 0) {
+            ii ++;
+            if (ii < argc)  {
+                if (strtol(argv[ii], NULL, 10) != (long)getuid()) {
+                    printf("ico-vic-carsim: abort(cannot run in a '%s' [uid=%d, euid=%d])\n", cuserid(NULL), getuid(), geteuid());
+                    return -1;
+                }
+            }
         }
     }
 
+    ico_log_open("ico-vic-carsim");
+    ICO_INF( "START_MODULE ico-vic-carsim" );
+
     if (comFlg) {
         CGtCtrl myGtCtrl;
 
@@ -77,6 +101,7 @@ int main(int argc, char **argv)
             myGtCtrl.Terminate();
         }
 
+        ICO_INF( "END_MODULE ico-vic-carsim" );
         return 0;
     }
     if (bTestMode) {
@@ -108,12 +133,12 @@ int main(int argc, char **argv)
     else {
         // change to super user
         if (setuid(0) < 0)  {
-            printf("can not set super user\n");
+            printf("can not set super user [errno=%d]\n", errno);
         }
 
         CGtCtrl myGtCtrl;
-               myGtCtrl.m_bUseGps = bUseGps;
-               myGtCtrl.m_bDemoRunning = bDemoRunning;
+        myGtCtrl.m_bUseGps = bUseGps;
+        myGtCtrl.m_bDemoRunning = bDemoRunning;
         b = myGtCtrl.Initialize();
 
         if (b) {
@@ -121,6 +146,8 @@ int main(int argc, char **argv)
         }
         myGtCtrl.Terminate();
     }
+
+    ICO_INF( "END_MODULE ico-vic-carsim" );
     return 0;
 }