// -------------------------------------------------- int WSAClientSocket::readUpto(void *p, int l) { int bytesRead=0; while (l) { int r = recv(sockNum, (char *)p, l, 0); if (r == SOCKET_ERROR) { // non-blocking sockets always fall through to here checkTimeout(true,false); }else if (r == 0) { break; }else { stats.add(Stats::BYTESIN,r); if (host.localIP()) stats.add(Stats::LOCALBYTESIN,r); updateTotals(r,0); bytesRead += r; l -= r; p = (char *)p+r; } } return bytesRead; }
void DistanceEstimation::updateFeatureIntern(const base::samples::LaserScan &feature, const Eigen::Affine3d &featureInOdometry) { checkTimeout(); if(feature.isValidBeam(0) && feature.ranges.front() > base::samples::MAX_RANGE_ERROR) { if(actualFeature.ranges.front() <= base::samples::MAX_RANGE_ERROR) { actualFeature.ranges.front() = feature.ranges.front(); } else { double new_range = (double)feature.ranges.front() * 0.001; double actual_range = (double)actualFeature.ranges.front() * 0.001; double dist = abs(new_range - actual_range); if(dist > max_distance) { // limit distance to the next point new_range = actual_range + ((new_range - actual_range) / dist) * max_distance; } actual_range = actual_range * weightOldValue + new_range * weightNewValue; actualFeature.ranges.front() = (uint32_t)(actual_range * 1000.0); } actualFeature.start_angle = feature.start_angle; actualFeature.time = feature.time; } }
bool Socket::think() { if(connecting) { fd_set monitor; timeval tv; tv.tv_sec = 0; tv.tv_usec = 0; FD_ZERO(&monitor); FD_SET(s, &monitor); select(s+1, 0, &monitor, 0, &tv); if(FD_ISSET(s, &monitor)) { int status; socklen_t len = sizeof(status); getsockopt(s, SOL_SOCKET, SO_ERROR, (char *)&status, &len); if(status != 0) // Error { error = ErrorConnect; return true; } connected = true; connecting = false; resetTimer(); } else checkTimeout(); } return error != ErrorNone; }
bool Socket::readChunk() { dataBegin = dataEnd = 0; int r = recv(s, staticBuffer, 1024, 0); if(r != -1) { if(r == 0) { connected = false; return true; // No more data } dataBegin = staticBuffer; dataEnd = staticBuffer + r; resetTimer(); } else if(sockError() != EWOULDBLOCK) { error = ErrorRecv; return true; // Error } else { checkTimeout(); } return error != ErrorNone; }
void SolverDefaultImplementation::writeToFile(const int& stp, const double& t, const double& h) { #ifdef RUNTIME_PROFILING MEASURETIME_REGION_DEFINE(writeFunctionStartValues, "solverWriteOutput"); if(MeasureTime::getInstance() != NULL) { MEASURETIME_START(writeFunctionStartValues, solverWriteOutputHandler, "solverWriteOutput"); } #endif if(_settings->getGlobalSettings()->getOutputFormat()!= EMPTY) { IWriteOutput* writeoutput_system = dynamic_cast<IWriteOutput*>(_system); if(_outputCommand & IWriteOutput::WRITEOUT) { writeoutput_system->writeOutput(_outputCommand); } } checkTimeout(); #ifdef RUNTIME_PROFILING if(MeasureTime::getInstance() != NULL) { MEASURETIME_END(writeFunctionStartValues, writeFunctionEndValues, measureTimeFunctionsArray[0], solverWriteOutputHandler); } #endif }
bool Socket::trySend(char const*& b, char const* e) { assert(b <= e); if(b == e) return true; int sent = ::send(s, b, e - b, 0); if(sent == -1) { if(sockError() != EWOULDBLOCK) // Error { error = ErrorSend; return true; } else checkTimeout(); } else { b += sent; if(b == e) return true; } return error != ErrorNone; }
// -------------------------------------------------- void WSAClientSocket::write(const void *p, int l) { while (l) { int r = send(sockNum, (char *)p, l, 0); if (r == SOCKET_ERROR) { checkTimeout(false,true); } else if (r == 0) { throw SockException("Closed on write"); } else if (r > 0) { stats.add(Stats::BYTESOUT,r); if (host.localIP()) stats.add(Stats::LOCALBYTESOUT,r); updateTotals(0,r); l -= r; p = (char *)p+r; } } }
double DistanceEstimation::getActualDistance() { checkTimeout(); if(actualFeature.ranges.front() <= base::samples::MAX_RANGE_ERROR) return -1; return (double)actualFeature.ranges.front() * 0.001; }
void throughPutTest(void) { static enum {INIT,LOADING} state = INIT; static whoPacket_t clear = {CLEAR_STATS}, get = {GET_STATS}; switch(state) { case INIT: clear.who.to = testdest; clear.who.from = whoami(); if (sendNormalPacketLink((Byte *)&clear, sizeof(clear), loadLink)) { whoPacket_t *wp = (whoPacket_t *)testPacket; print ("\nstarting throughput test..."); flush(); setPacketHandler(STATS, testResults); state = LOADING; framesOut = packetsInGroup; setTimeout(0, &testLength_to); setTimeout(1 TO_SEC, &sendwait_to); get.who.to = testdest; get.who.from = whoami(); wp->who.to = testdest; wp->who.from = whoami(); } break; case LOADING: if (framesOut) { bool flag; if (loadLink->enableSps) flag = sendSecurePacketLink(testPacket, sizeof(testPacket), loadLink); else flag = sendNormalPacketLink(testPacket, sizeof(testPacket), loadLink); if (flag) { framesOut--; setTimeout(1 TO_SEC, &sendwait_to); } if (checkTimeout(&sendwait_to)) { print ("\n timedout - "); printDec((Long)sinceTimeout(&testLength_to)); print(" ms; giving up throughput test\n"); state = INIT; return; } } else if (sendNormalPacketLink((Byte *)&get, sizeof(get), loadLink)) { state = INIT; print (" done. getting results..."); flush(); return; } break; } activate(throughPutTest); }
void GLTouchScreen::onTouchMove(MotionEvent* event) { checkTimeout(); if (touchDown) { MotionPointer* pointer = event->getPointer(0); record(pointer->x, pointer->y); } }
/* Mode Getter */ bool MAVLink::getMode(uint32_t &mode) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SET_MODE); if (mm == NULL) { return false; } mode = mavlink_msg_set_mode_get_mode(&mm->msg); return checkTimeout(mm,MAVLINK_MODE_TIMEOUT); }
void *loop(void *threadid) { while (1) { sleep(SECOND); checkTimeout(); } return NULL ; }
static bool checkLauncherWithTimeout(uint32_t testDelay, double startTime, double timeout, uint64_t launchId, MpiOperatorContext* ctx) { bool rc = checkLauncher(testDelay, launchId, ctx); assert(rc); return (checkTimeout(startTime, timeout, launchId, ctx) && rc); }
void GLTouchScreen::onTouchEnd(MotionEvent* event) { checkTimeout(); if (touchDown) { touchDown = false; touchTime = currentTimeMillis(); MotionPointer* pointer = event->getPointer(0); analyze(pointer->x, pointer->y); } }
/* Attitude Command Getter */ bool MAVLink::getAttitudeCommand(float &roll, float &pitch, float &yaw, float &thrust) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST); if (mm == NULL) { return false; } roll = mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(&mm->msg); pitch = mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(&mm->msg); yaw = mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(&mm->msg); thrust = mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(&mm->msg); return checkTimeout(mm,MAVLINK_ATTITUDE_TIMEOUT); }
void daemonHandlerThread::work() { MVERBOSE_LOG(logWrapper(), verboseFlag_, "started."); while (!die_) { // Check if timeouts occured every few seconds. checkTimeout(); btg::core::os::Sleep::sleepSecond(1); } }
/* VFR HUD Getter */ bool MAVLink::getVFRHUD(float &airspeed, float &groundspeed, int16_t &heading, uint16_t &throttle, float &alt, float &climb) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_VFR_HUD); if (mm == NULL) { return false; } airspeed = mavlink_msg_vfr_hud_get_airspeed(&mm->msg); groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&mm->msg); heading = mavlink_msg_vfr_hud_get_heading(&mm->msg); throttle = mavlink_msg_vfr_hud_get_throttle(&mm->msg); alt = mavlink_msg_vfr_hud_get_alt(&mm->msg); climb = mavlink_msg_vfr_hud_get_climb(&mm->msg); return checkTimeout(mm,MAVLINK_VFR_HUD_TIMEOUT); }
/* Attitude Getter */ bool MAVLink::getAttitude(float &roll, float &pitch, float &yaw, float &p, float &q, float &r) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_ATTITUDE); if (mm == NULL) { return false; } roll = mavlink_msg_attitude_get_roll(&mm->msg); pitch = mavlink_msg_attitude_get_pitch(&mm->msg); yaw = mavlink_msg_attitude_get_yaw(&mm->msg); p = mavlink_msg_attitude_get_rollspeed(&mm->msg); q = mavlink_msg_attitude_get_pitchspeed(&mm->msg); r = mavlink_msg_attitude_get_yawspeed(&mm->msg); return checkTimeout(mm,MAVLINK_ATTITUDE_TIMEOUT); }
bool MAVLink::getGlobalPositionInt(int32_t &lat, int32_t &lng, int32_t &alt, int16_t &vx, int16_t &vy, int16_t &vz) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_GLOBAL_POSITION_INT); if (mm == NULL) { return false; } lat = mavlink_msg_global_position_int_get_lat(&mm->msg); lng = mavlink_msg_global_position_int_get_lon(&mm->msg); alt = mavlink_msg_global_position_int_get_alt(&mm->msg); vx = mavlink_msg_global_position_int_get_vx(&mm->msg); vy = mavlink_msg_global_position_int_get_vy(&mm->msg); vz = mavlink_msg_global_position_int_get_vz(&mm->msg); return checkTimeout(mm,MAVLINK_RAW_GPS_TIMEOUT); }
bool MAVLink::getSystemStatus(uint8_t &mode, uint8_t &nav_mode, uint8_t &status, uint16_t &load, uint16_t &vbat, uint16_t &battery_remaining, uint16_t &packet_drop) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SYS_STATUS); if (mm == NULL) { return false; } mode = mavlink_msg_sys_status_get_mode(&mm->msg); nav_mode = mavlink_msg_sys_status_get_nav_mode(&mm->msg); status = mavlink_msg_sys_status_get_status(&mm->msg); load = mavlink_msg_sys_status_get_load(&mm->msg); vbat = mavlink_msg_sys_status_get_vbat(&mm->msg); battery_remaining = mavlink_msg_sys_status_get_battery_remaining(&mm->msg); packet_drop = mavlink_msg_sys_status_get_packet_drop(&mm->msg); return checkTimeout(mm,MAVLINK_STATUS_TIMEOUT); }
/* RC Override Getter */ bool MAVLink::getRCOverride(uint16_t &c1, uint16_t &c2, uint16_t &c3, uint16_t &c4, uint16_t &c5, uint16_t &c6, uint16_t &c7, uint16_t &c8) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE); if (mm == NULL) { return false; } c1 = mavlink_msg_rc_channels_override_get_chan1_raw(&mm->msg); c2 = mavlink_msg_rc_channels_override_get_chan2_raw(&mm->msg); c3 = mavlink_msg_rc_channels_override_get_chan3_raw(&mm->msg); c4 = mavlink_msg_rc_channels_override_get_chan4_raw(&mm->msg); c5 = mavlink_msg_rc_channels_override_get_chan5_raw(&mm->msg); c6 = mavlink_msg_rc_channels_override_get_chan6_raw(&mm->msg); c7 = mavlink_msg_rc_channels_override_get_chan7_raw(&mm->msg); c8 = mavlink_msg_rc_channels_override_get_chan8_raw(&mm->msg); return checkTimeout(mm,MAVLINK_RC_OVERRIDE_TIMEOUT); }
/* Raw GPS Getter */ bool MAVLink::getRawGPS(int &fix, float &lat, float &lng, float &alt, float &eph, float &epv, float &v, float &crs) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_GPS_RAW); if (mm == NULL) { return false; } fix = mavlink_msg_gps_raw_get_fix_type(&mm->msg); lat = mavlink_msg_gps_raw_get_lat(&mm->msg); lng = mavlink_msg_gps_raw_get_lon(&mm->msg); alt = mavlink_msg_gps_raw_get_alt(&mm->msg); eph = mavlink_msg_gps_raw_get_eph(&mm->msg); epv = mavlink_msg_gps_raw_get_epv(&mm->msg); v = mavlink_msg_gps_raw_get_v(&mm->msg); crs = mavlink_msg_gps_raw_get_hdg(&mm->msg); return checkTimeout(mm,MAVLINK_RAW_GPS_TIMEOUT); }
/* Raw Servo Getter */ bool MAVLink::getRawServos(uint16_t &s1, uint16_t &s2, uint16_t &s3, uint16_t &s4, uint16_t &s5, uint16_t &s6, uint16_t &s7, uint16_t &s8) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW); if (mm == NULL) { return false; } s1 = mavlink_msg_servo_output_raw_get_servo1_raw(&mm->msg); s2 = mavlink_msg_servo_output_raw_get_servo2_raw(&mm->msg); s3 = mavlink_msg_servo_output_raw_get_servo3_raw(&mm->msg); s4 = mavlink_msg_servo_output_raw_get_servo4_raw(&mm->msg); s5 = mavlink_msg_servo_output_raw_get_servo5_raw(&mm->msg); s6 = mavlink_msg_servo_output_raw_get_servo6_raw(&mm->msg); s7 = mavlink_msg_servo_output_raw_get_servo7_raw(&mm->msg); s8 = mavlink_msg_servo_output_raw_get_servo8_raw(&mm->msg); return checkTimeout(mm,MAVLINK_SERVO_TIMEOUT); }
/* Raw Servo Getter */ bool MAVLink::getScaledServos(int16_t &s1, int16_t &s2, int16_t &s3, int16_t &s4, int16_t &s5, int16_t &s6, int16_t &s7, int16_t &s8, uint8_t &rssi) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); if (mm == NULL) { return false; } s1 = mavlink_msg_rc_channels_scaled_get_chan1_scaled(&mm->msg); s2 = mavlink_msg_rc_channels_scaled_get_chan2_scaled(&mm->msg); s3 = mavlink_msg_rc_channels_scaled_get_chan3_scaled(&mm->msg); s4 = mavlink_msg_rc_channels_scaled_get_chan4_scaled(&mm->msg); s5 = mavlink_msg_rc_channels_scaled_get_chan5_scaled(&mm->msg); s6 = mavlink_msg_rc_channels_scaled_get_chan6_scaled(&mm->msg); s7 = mavlink_msg_rc_channels_scaled_get_chan7_scaled(&mm->msg); s8 = mavlink_msg_rc_channels_scaled_get_chan8_scaled(&mm->msg); return checkTimeout(mm,MAVLINK_SERVO_TIMEOUT); }
// -------------------------------------------------- int UClientSocket::read(void *p, int l) { int bytesRead=0; while (l) { if (rbDataSize >= l) { memcpy(p, &apReadBuf[rbPos], l); rbPos += l; rbDataSize -= l; return l; } else if (rbDataSize > 0) { memcpy(p, &apReadBuf[rbPos], rbDataSize); p = (char *) p + rbDataSize; l -= rbDataSize; bytesRead += rbDataSize; } rbPos = 0; rbDataSize = 0; //int r = recv(sockNum, (char *)p, l, 0); int r = recv(sockNum, apReadBuf, RBSIZE, 0); if (r == SOCKET_ERROR) { // non-blocking sockets always fall through to here checkTimeout(true,false); }else if (r == 0) { throw SockException("Closed on read"); }else { stats.add(Stats::BYTESIN,r); if (host.localIP()) stats.add(Stats::LOCALBYTESIN,r); updateTotals(r,0); //bytesRead += r; //l -= r; //p = (char *)p+r; rbDataSize += r; } } return bytesRead; }
void GLTouchScreen::onTouchStart(MotionEvent* event) { checkTimeout(); MotionPointer* pointer = event->getPointer(0); float x = pointer->x; float y = pointer->y; if (checkBounds(x, y)) { if (canStartTouch()) { touchDown = true; touchList.clear(); touchLast = NULL; touchStart = currentTimeMillis(); record(x, y); if (!touchCancel) { touchTimeout = touchStart + 1000; } } } }
void loop() { checkSerialAPI(); checkTimeout(); switch(toupper(CMD[0])){ case 'R': runningLights(DEL, BRI); break; case 'F': flashingLights(DEL, BRI); break; case 'P': police(DEL, BRI); break; case 'S': still(BRI); break; case 'D': disco(DEL, BRI); break; case 'G': glow(DEL, BRI); break; case 'O': default: off(); } }
TTYBase::TTY_RESPONSE TTYBase::readSection(uint8_t *buffer, uint32_t nsize, uint8_t stop_byte, uint8_t timeout, uint32_t *nbytes_read) { #ifdef _WIN32 return TTY_ERRNO; #else if (m_PortFD == -1) return TTY_ERRNO; int bytesRead = 0; TTY_RESPONSE timeoutResponse = TTY_OK; *nbytes_read = 0; memset(buffer, 0, nsize); uint8_t *read_char = nullptr; DEBUGFDEVICE(m_DriverName, m_DebugChannel, "%s: Request to read until stop char '%#02X' with %d timeout for m_PortFD %d", __FUNCTION__, stop_byte, timeout, m_PortFD); for (;;) { if ((timeoutResponse = checkTimeout(timeout))) return timeoutResponse; read_char = reinterpret_cast<uint8_t*>(buffer + *nbytes_read); bytesRead = ::read(m_PortFD, read_char, 1); if (bytesRead < 0) return TTY_READ_ERROR; DEBUGFDEVICE(m_DriverName, m_DebugChannel, "%s: buffer[%d]=%#X (%c)", __FUNCTION__, (*nbytes_read), *read_char, *read_char); (*nbytes_read)++; if (*read_char == stop_byte) return TTY_OK; else if (*nbytes_read >= nsize) return TTY_OVERFLOW; } #endif }
TTYBase::TTY_RESPONSE TTYBase::read(uint8_t *buffer, uint32_t nbytes, uint8_t timeout, uint32_t *nbytes_read) { #ifdef _WIN32 return TTY_ERRNO; #else if (m_PortFD == -1) return TTY_ERRNO; uint32_t numBytesToRead = nbytes; int bytesRead = 0; TTY_RESPONSE timeoutResponse = TTY_OK; *nbytes_read = 0; if (nbytes <= 0) return TTY_PARAM_ERROR; DEBUGFDEVICE(m_DriverName, m_DebugChannel, "%s: Request to read %d bytes with %d timeout for m_PortFD %d", __FUNCTION__, nbytes, timeout, m_PortFD); while (numBytesToRead > 0) { if ((timeoutResponse = checkTimeout(timeout))) return timeoutResponse; bytesRead = ::read(m_PortFD, buffer + (*nbytes_read), numBytesToRead); if (bytesRead < 0) return TTY_READ_ERROR; DEBUGFDEVICE(m_DriverName, m_DebugChannel, "%d bytes read and %d bytes remaining...", bytesRead, numBytesToRead - bytesRead); for (uint32_t i = *nbytes_read; i < (*nbytes_read + bytesRead); i++) DEBUGFDEVICE(m_DriverName, m_DebugChannel, "%s: buffer[%d]=%#X (%c)", __FUNCTION__, i, buffer[i], buffer[i]); *nbytes_read += bytesRead; numBytesToRead -= bytesRead; } return TTY_OK; #endif }
// -------------------------------------------------- void UClientSocket::write(const void *p, int l) { while (l) { int r = send(sockNum, (char *)p, l, MSG_DONTWAIT|MSG_NOSIGNAL); if (r == SOCKET_ERROR) { // non-blocking sockets always fall through to here checkTimeout(false,true); }else if (r == 0) { throw SockException("Closed on write"); }else { stats.add(Stats::BYTESOUT,r); if (host.localIP()) stats.add(Stats::LOCALBYTESOUT,r); updateTotals(0,r); l -= r; p = (char *)p+r; } } }