From 0d3b74297286d2ce2260f498caeceddcd90c1a2a Mon Sep 17 00:00:00 2001 From: Masayuki Sasaki Date: Mon, 17 Mar 2014 14:26:59 +0900 Subject: [PATCH] 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 --- ico-vic-carsim.service | 2 +- packaging/ico-vic-carsimulator.changes | 6 + packaging/ico-vic-carsimulator.spec | 2 +- src/AmbpiComm.cpp | 18 +- src/AmbpiComm.h | 10 +- src/CConf.cpp | 6 +- src/CConf.h | 2 +- src/CGtCtrl.cpp | 541 ++++++++++++++++++--------------- src/CGtCtrl.h | 2 +- src/CJoyStick.cpp | 84 +++-- src/CJoyStick.h | 106 ++++++- src/CJoyStickEV.cpp | 70 ++++- src/CJoyStickEV.h | 4 + src/CarSim_Daemon.cpp | 75 +++-- 14 files changed, 598 insertions(+), 330 deletions(-) diff --git a/ico-vic-carsim.service b/ico-vic-carsim.service index 85aa087..e12167a 100644 --- 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 --- a/packaging/ico-vic-carsimulator.changes +++ b/packaging/ico-vic-carsimulator.changes @@ -1,3 +1,9 @@ +* Mon Mar 17 2014 Shibata Makoto 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 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 --- 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 --- a/src/AmbpiComm.cpp +++ b/src/AmbpiComm.cpp @@ -16,6 +16,9 @@ #include #include #include "AmbpiComm.h" + +#include "ico-util/ico_log.h" + using namespace std; static vector _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 --- 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 --- 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 --- 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 --- a/src/CGtCtrl.cpp +++ b/src/CGtCtrl.cpp @@ -21,6 +21,9 @@ #include "CAvgCar.h" #include "CCalc.h" +#include +#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(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(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 --- 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 --- a/src/CJoyStick.cpp +++ b/src/CJoyStick.cpp @@ -23,6 +23,10 @@ #include #include #include "CJoyStick.h" + +#include "ico-util/ico_log.h" +#include + 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 --- a/src/CJoyStick.h +++ b/src/CJoyStick.h @@ -22,13 +22,110 @@ #include #include #include -#include +#include +#include +#include +#include +#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 +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 mQueue; +}; + class CJoyStick { public: @@ -54,8 +151,10 @@ class CJoyStick const std::vector& matching, std::vector& 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 queue; + private: + pthread_t m_threadid; }; #endif /* CJOYSTICK_H_ */ diff --git a/src/CJoyStickEV.cpp b/src/CJoyStickEV.cpp index b846b53..14a4caa 100644 --- 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 + 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 --- 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 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 --- a/src/CarSim_Daemon.cpp +++ b/src/CarSim_Daemon.cpp @@ -14,9 +14,13 @@ #include #include #include -#include -#include +#include +#include +#include +//#include #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; } -- 2.7.4