#include "CAvgCar.h"
#include "CCalc.h"
+#include <sys/time.h>
+#include "ico-util/ico_log.h"
+
extern bool gbDevJs;
bool g_bStopFlag;
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;
}
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
*/
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) {
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) {
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
*/
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) {
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;
}
}
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;
}
}
}
}
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];
+ }
+ }
+ }
}
/**
projects / profile / ivi / ico-vic-carsimulator.git / blobdiff