s32 tcp_socket (void) { s32 s, res; s = net_socket (PF_INET, SOCK_STREAM, 0); if (s < 0) { printDebugMsg(NORMAL_DEBUG_MESSAGE,"net_socket failed: %d\n", s); return s; } res = net_fcntl (s, F_GETFL, 0); if (res < 0) { printDebugMsg(NORMAL_DEBUG_MESSAGE,"F_GETFL failed: %d\n", res); net_close (s); return res; } res = net_fcntl (s, F_SETFL, res | 4); if (res < 0) { printDebugMsg(NORMAL_DEBUG_MESSAGE,"F_SETFL failed: %d\n", res); net_close (s); return res; } return s; }
s32 tcp_connect (char *host, const u16 port) { struct hostent *hp; struct sockaddr_in sa; s32 s, res; s64 t; hp = net_gethostbyname (host); if (!hp || !(hp->h_addrtype == PF_INET)) { printDebugMsg(NORMAL_DEBUG_MESSAGE,"net_gethostbyname failed: %d\n", errno); return errno; } s = tcp_socket (); if (s < 0) return s; memset (&sa, 0, sizeof (struct sockaddr_in)); sa.sin_family= PF_INET; sa.sin_len = sizeof (struct sockaddr_in); sa.sin_port= htons (port); memcpy ((char *) &sa.sin_addr, hp->h_addr_list[0], hp->h_length); t = gettime (); while (true) { if (ticks_to_millisecs (diff_ticks (t, gettime ())) > TCP_CONNECT_TIMEOUT) { printDebugMsg(NORMAL_DEBUG_MESSAGE,"tcp_connect timeout\n"); net_close (s); return -ETIMEDOUT; } res = net_connect (s, (struct sockaddr *) &sa, sizeof (struct sockaddr_in)); if (res < 0) { if (res == -EISCONN) break; if (res == -EINPROGRESS || res == -EALREADY) { usleep (20 * 1000); continue; } printDebugMsg(NORMAL_DEBUG_MESSAGE,"net_connect failed: %d\n", res); net_close (s); return res; } break; } return s; }
bool tcp_read (const s32 s, u8 **buffer, const u32 length) { u8 *p; u32 step, left, block, received; s64 t; s32 res; step = 0; p = *buffer; left = length; received = 0; t = gettime (); while (left) { if (ticks_to_millisecs (diff_ticks (t, gettime ())) > TCP_BLOCK_RECV_TIMEOUT) { printDebugMsg(NORMAL_DEBUG_MESSAGE,"tcp_read timeout\n"); break; } block = left; if (block > 2048) block = 2048; res = net_read (s, p, block); if ((res == 0) || (res == -EAGAIN)) { usleep (20 * 1000); continue; } if (res < 0) { printDebugMsg(NORMAL_DEBUG_MESSAGE,"net_read failed: %d\n", res); break; } received += res; left -= res; p += res; if ((received / TCP_BLOCK_SIZE) > step) { t = gettime (); step++; } } return left == 0; }
bool tcp_write (const s32 s, const u8 *buffer, const u32 length) { const u8 *p; u32 step, left, block, sent; s64 t; s32 res; step = 0; p = buffer; left = length; sent = 0; t = gettime (); while (left) { if (ticks_to_millisecs (diff_ticks (t, gettime ())) > TCP_BLOCK_SEND_TIMEOUT) { printDebugMsg(NORMAL_DEBUG_MESSAGE,"tcp_write timeout\n"); break; } block = left; if (block > 2048) block = 2048; res = net_write (s, p, block); if ((res == 0) || (res == -56)) { usleep (20 * 1000); continue; } if (res < 0) { printDebugMsg(NORMAL_DEBUG_MESSAGE,"net_write failed: %d\n", res); break; } sent += res; left -= res; p += res; if ((sent / TCP_BLOCK_SIZE) > step) { t = gettime (); step++; } } return left == 0; }
char * tcp_readln (const s32 s, const u16 max_length, const u64 start_time, const u32 timeout) { char *buf; u16 c; s32 res; char *ret; buf = (char *) malloc (max_length); c = 0; ret = NULL; while (true) { if (ticks_to_millisecs (diff_ticks (start_time, gettime ())) > timeout) break; res = net_read (s, &buf[c], 1); if ((res == 0) || (res == -EAGAIN)) { usleep (20 * 1000); continue; } if (res < 0) { printDebugMsg(NORMAL_DEBUG_MESSAGE,"tcp_readln failed: %d\n", res); break; } if ((c > 0) && (buf[c - 1] == '\r') && (buf[c] == '\n')) { if (c == 1) { ret = strdup (""); break; } ret = strndup (buf, c - 1); break; } c++; if (c == max_length) break; } free (buf); return ret; }
void esp::Worklist::solveWorklist(){ while(works.size() != 0){ Work *currentWork = selectAWork(); removeFromWorklist(currentWork); //do real work here if(currentWork != NULL) doEachWork(currentWork); printDebugMsg("Instructions in worklist:"); for(set<Work*>::iterator it = works.begin(); it!=works.end(); it++){ Work *workInList = (*it); workInList->printContent(); } } }
bool MSLane::isEmissionSuccess(MSVehicle* aVehicle, SUMOReal speed, SUMOReal pos, bool patchSpeed, size_t startStripId) throw(ProcessError) { // and the speed is not too high (vehicle should decelerate) // try to get a leader on consecutive lanes // we have to do this even if we have found a leader on our lane because it may // be driving into another direction //std::cerr<<"EMISSION speed:"<<speed<<std::endl; std::cerr<<"EMISSION vehicle:"<<aVehicle->getID()<<std::endl; size_t endStripId = startStripId + aVehicle->getWidth() - 1; assert(startStripId >=0 && endStripId < myStrips.size()); aVehicle->getBestLanes(true, this); const MSCFModel &cfModel = aVehicle->getCarFollowModel(); const std::vector<MSLane*> &bestLaneConts = aVehicle->getBestLanesContinuation(this); std::vector<MSLane*>::const_iterator ri = bestLaneConts.begin(); SUMOReal seen = getLength() - pos; SUMOReal dist = cfModel.brakeGap(speed); const MSRoute &r = aVehicle->getRoute(); MSRouteIterator ce = r.begin(); MSLane *currentLane = this; MSLane *nextLane = this; while (seen<dist&&ri!=bestLaneConts.end()&&nextLane!=0/*&&ce!=r.end()*/) { // get the next link used... MSLinkCont::const_iterator link = currentLane->succLinkSec(*aVehicle, 1, *currentLane, bestLaneConts); // ...and the next used lane (including internal) if (!currentLane->isLinkEnd(link) && (*link)->havePriority() && (*link)->getState()!=MSLink::LINKSTATE_TL_RED) { // red may have priority? #ifdef HAVE_INTERNAL_LANES bool nextInternal = false; nextLane = (*link)->getViaLane(); if (nextLane==0) { nextLane = (*link)->getLane(); } else { nextInternal = true; } #else nextLane = (*link)->getLane(); #endif } else { nextLane = 0; } // check how next lane effects the journey if (nextLane!=0) { SUMOReal gap = 0; //TODO: fix get & set partial occupator to strip level MSVehicle * leader = 0;//currentLane->getPartialOccupator(); if (leader!=0) { gap = getPartialOccupatorEnd(); } else { // check leader on next lane leader = nextLane->getLastVehicle(aVehicle->getStrips()); if (leader!=0) { gap = seen+leader->getPositionOnLane()-leader->getVehicleType().getLength(); } } if (leader!=0) { SUMOReal nspeed = gap>=0 ? cfModel.ffeV(aVehicle, speed, gap, leader->getSpeed()) : 0; if (nspeed<speed) { if (patchSpeed) { speed = MIN2(nspeed, speed); dist = cfModel.brakeGap(speed); } else { // we may not drive with the given velocity - we crash into the leader return false; } } } // check next lane's maximum velocity SUMOReal nspeed = nextLane->getMaxSpeed(); if (nspeed<speed) { // patch speed if needed if (patchSpeed) { speed = MIN2(cfModel.ffeV(aVehicle, speed, seen, nspeed), speed); dist = cfModel.brakeGap(speed); } else { // we may not drive with the given velocity - we would be too fast on the next lane return false; } } // check traffic on next junctions const SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / speed); #ifdef HAVE_INTERNAL_LANES const SUMOTime leaveTime = (*link)->getViaLane()==0 ? arrivalTime + TIME2STEPS((*link)->getLength() * speed) : arrivalTime + TIME2STEPS((*link)->getViaLane()->getLength() * speed); #else const SUMOTime leaveTime = arrivalTime + TIME2STEPS((*link)->getLength() * speed); #endif if ((*link)->hasApproachingFoe(arrivalTime, leaveTime)) { SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0); if (nspeed<speed) { if (patchSpeed) { speed = MIN2(nspeed, speed); dist = cfModel.brakeGap(speed); } else { // we may not drive with the given velocity - we crash into the leader return false; } } } else { // we can only drive to the end of the current lane... SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0); if (nspeed<speed) { if (patchSpeed) { speed = MIN2(nspeed, speed); dist = cfModel.brakeGap(speed); } else { // we may not drive with the given velocity - we crash into the leader return false; } } } seen += nextLane->getLength(); ++ce; ++ri; currentLane = nextLane; } } if (seen<dist) { SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0); if (nspeed<speed) { if (patchSpeed) { speed = MIN2(nspeed, speed); dist = cfModel.brakeGap(speed); } else { // we may not drive with the given velocity - we crash into the leader MsgHandler::getErrorInstance()->inform("Vehicle '" + aVehicle->getID() + "' will not be able to emit using given velocity!"); // !!! we probably should do something else... return false; } } } // get the pointer to the vehicle next in front of the given position MSVehicle *pred; std::vector<MSVehicle *> predCont; std::vector<MSVehicle *>::iterator predIt, it; for (unsigned int i=startStripId; i<=endStripId; ++i) { predCont.push_back(myStrips.at(i)->getPredAtPos(pos)); } predIt = predCont.begin(); SUMOReal currMin = -1; if (*predIt != 0) { currMin = (*predIt)->getPositionOnLane(); } else { // signals no leader in front predIt = predCont.end(); } for (it = predCont.begin(); it != predCont.end(); ++it) { if (*it == 0) continue; if ((*it)->getPositionOnLane() < currMin) { predIt = it; currMin = (*it)->getPositionOnLane(); } } if (predIt != predCont.end()) { // ok, there is one (a leader) MSVehicle* leader = *predIt; SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed())); SUMOReal gap = MSVehicle::gap(leader->getPositionOnLane(), leader->getVehicleType().getLength(), pos); if (gap<frontGapNeeded) { // too close to the leader on this lane return false; } } // FIXME: implement look back // check back vehicle if (false/*predIt!=myVehicles.begin()*/) { // there is direct follower on this lane MSVehicle *follower = *(predIt-1); SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed)); SUMOReal gap = MSVehicle::gap(pos, aVehicle->getVehicleType().getLength(), follower->getPositionOnLane()); if (gap<backGapNeeded) { // too close to the follower on this lane return false; } } else if (false) { // check approaching vehicle (consecutive follower) SUMOReal lspeed = getMaxSpeed(); // in order to look back, we'd need the minimum braking ability of vehicles in the net... // we'll assume it to be 4m/s^2 // !!!revisit SUMOReal dist = lspeed * lspeed * SUMOReal(1./2.*4.) + SPEED2DIST(lspeed); std::pair<const MSVehicle * const, SUMOReal> approaching = getFollowerOnConsecutive(dist, 0, speed, pos - aVehicle->getVehicleType().getLength()); if (approaching.first!=0) { const MSVehicle *const follower = approaching.first; SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed)); SUMOReal gap = approaching.second - pos - aVehicle->getVehicleType().getLength(); if (gap<backGapNeeded) { // too close to the consecutive follower return false; } } // check for in-lapping vehicle MSVehicle* leader = getPartialOccupator(); if (leader!=0) { SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed())); SUMOReal gap = getPartialOccupatorEnd() - pos; if (gap<=frontGapNeeded) { // too close to the leader on this lane return false; } } } // may got negative while adaptation if (speed<0) { return false; } // enter //XXX: later change to enterStripAtEmit()? //if (speed < 0.0001) speed += 10.0; StripCont strips; strips.resize(aVehicle->getWidth()); StripCont::iterator start = myStrips.begin() + startStripId; std::copy(start, start + aVehicle->getWidth(), strips.begin()); aVehicle->enterLaneAtEmit(this, pos, speed, strips); bool wasInactive = getVehicleNumber()==0; if (true/*predIt==myVehicles.end()*/) { // vehicle will be the first on the lane //std::cerr<<"startStripId:"<<startStripId<<", NumStrips:"<<strips.size()<<", VehWidth:"<<aVehicle->getWidth()<<std::endl; for (size_t i=startStripId; i<startStripId+strips.size(); ++i) { this->getStrip(i)->pushIntoStrip(aVehicle); this->getStrip(i)->setVehLenSum(this->getStrip(i)->getVehLenSum() + aVehicle->getVehicleType().getLength()); } aVehicle->printDebugMsg("Emitting"); printDebugMsg(); } else { //this->getStrip(0).insert(0, aVehicle); } //myVehicleLengthSum += aVehicle->getVehicleType().getLength(); if (wasInactive) { MSNet::getInstance()->getEdgeControl().gotActive(this); } return true; }
bool Calc::optimizeZ3() { double maxGuess; double upperGuess, lowerGuess; double upperError, lowerError; // Our residual error threshold const double maxErr = 0.01*radPerDeg; int ntries; // Calculate the true angular separation of the two stars, // based on their RA/Dec. This need be calculated only once. const double trueSep = calcSeparation(alignRA[1], alignDec[1], alignRA[2], alignDec[2]); if (!MathLib::finite(trueSep)) { printDebugMsg("Can't calculate star separation!"); return false; } if (trueSep < 5.0*radPerDeg) { printDebugMsg("Stars too close together!"); return false; } maxGuess = 10.0*radPerDeg; if (maxGuess > trueSep/2) maxGuess = trueSep/2; Z3 = upperGuess = maxGuess; upperError = calcApparentSeparation() - trueSep; if (MathLib::fabs(upperError) <= maxErr) { printDebugMsg("Lucky pos. first guess"); return true; // Lucky guess! } Z3 = lowerGuess = -maxGuess; lowerError = calcApparentSeparation() - trueSep; if (MathLib::fabs(lowerError) <= maxErr) { printDebugMsg("Lucky neg. first guess"); return true; // Lucky guess! } // For the interpolation to succeed, the upper and // lower errors must be of opposite sign. if (upperError * lowerError > 0.0) { printDebugMsg("Correct Z3 out of range"); return false; } ntries = 0; for (;;) { double newGuess, newError; ntries++; // Interpolate between the upper and lower guesses to // get a new guess. Z3 = newGuess = lowerGuess + ((lowerError/(lowerError-upperError))*(upperGuess-lowerGuess)); newError = calcApparentSeparation() - trueSep; if (MathLib::fabs(newError) <= maxErr) { std::cout << "Converged after " << ntries << " tries: Z3= " << Z3*degPerRad << std::endl; return true; // We have converged! Z3 is set. } if (ntries > 25) { printDebugMsg("No convergence after 25 tries"); return false; // Failure to converge } // NewGuess replaces either lowerGuess or upperGuess, depending // on the sign of the newError. if (lowerError * newError > 0) { // newError has the same sign as lowerError lowerGuess = newGuess; lowerError = newError; } else { upperGuess = newGuess; upperError = newError; } } // We should never get here! printDebugMsg("Broke out of loop!?!"); return false; }