void SIPRegistrarClient::run() {
  DBG("SIPRegistrarClient starting...\n");
  AmDynInvokeFactory* uac_auth_f = AmPlugIn::instance()->getFactory4Di("uac_auth");
  if (uac_auth_f == NULL) {
    DBG("unable to get a uac_auth factory. registrations will not be authenticated.\n");
    DBG("(do you want to load uac_auth module?)\n");
  } else {
    uac_auth_i = uac_auth_f->getInstance();
  }

  while (!stop_requested.get()) {
    if (registrations.size()) {
      unsigned int cnt = 250;
      while (cnt > 0) {
	usleep(2000); // every 2 ms
	processEvents();
	cnt--;
      }
      checkTimeouts();
    } else {
      waitForEvent();
      processEvents();
    }
  }
}
UdpServerListener::UdpServerListener(QHostAddress hostAddress, quint16 hostPort, QObject *parent) :
    IPBlocksSources(hostAddress,hostPort, parent)
{
    flags |= REFLEXION_OPTIONS ;
    type = SERVER;
    udpSocket = nullptr;
    connect(&connectionsTimer, SIGNAL(timeout()), this, SLOT(checkTimeouts()));
    connectionsTimer.setInterval(GuiConst::DEFAULT_UDP_TIMEOUT_MS);
    connectionsTimer.moveToThread(&serverThread);
    moveToThread(&serverThread);
    serverThread.start();

}
Exemple #3
0
int
comm_select(int msec)
{
    static double last_timeout = 0.0;
    int rc;
    double start = current_dtime;

    debug(5, 3) ("comm_select: timeout %d\n", msec);

    if (msec > MAX_POLL_TIME)
	msec = MAX_POLL_TIME;

#if DELAY_POOLS
    /* We have delayed fds in queue? */
    if (n_slow_fds)
	msec = 0;
#endif

    statCounter.select_loops++;

    /* Check for disk io callbacks */
    storeDirCallback();

    /* Check timeouts once per second */
    if (last_timeout + 0.999 < current_dtime) {
	last_timeout = current_dtime;
	checkTimeouts();
    } else {
	int max_timeout = (last_timeout + 1.0 - current_dtime) * 1000;
	if (max_timeout < msec)
	    msec = max_timeout;
    }
    comm_select_handled = 0;

    rc = do_comm_select(msec);

#if DELAY_POOLS
    comm_call_slowfds();
#endif
    getCurrentTime();
    statCounter.select_time += (current_dtime - start);

    if (rc == COMM_TIMEOUT)
	debug(5, 8) ("comm_select: time out\n");

    return rc;
}
Exemple #4
0
void *openspy_mod_run(modLoadOptions *options)
{
#ifdef _WIN32
	WSADATA wsdata;
	WSAStartup(MAKEWORD(1,0),&wsdata);
#endif
  fd_set  rset;
  memcpy(&servoptions,options,sizeof(modLoadOptions));
  int num_instances = options->getConfInt(options->moduleArray,"numinstances");
  char buff[1024];
  int len;
  struct timeval timeout;
  memset(&timeout,0,sizeof(struct timeval));
  if(num_instances < 1) return NULL;
  int *sockets = (int *)malloc(num_instances * sizeof(int));
  struct sockaddr_in *si_me = (struct sockaddr_in *)malloc(num_instances * sizeof(struct sockaddr_in));
  for(int i=0;i<num_instances;i++) {
	if((sockets[i] = socket(AF_INET,SOCK_DGRAM, IPPROTO_UDP)) == -1) {
		servoptions.logMessageProc(moduleInfo.name,LOGLEVEL_ERROR,"Error creating socket for instance %d\n",i+1);
		return NULL;
	}
	si_me[i].sin_family = AF_INET;
	si_me[i].sin_port = htons(MATCHUP_PORT);
	si_me[i].sin_addr.s_addr = getIP(i);
	if(bind(sockets[i],(struct sockaddr *)&si_me[i],sizeof(struct sockaddr)) == -1) {
		servoptions.logMessageProc(moduleInfo.name,LOGLEVEL_ERROR,"Error binding address for socket instance %d\n",i+1);
		return NULL;
	}
  }
  for(;;) {
	struct sockaddr_in si_other;
	socklen_t slen = sizeof(struct sockaddr_in);
	int rsock = getnfds(&rset,sockets,num_instances);
	timeout.tv_sec = 5;
	timeout.tv_usec = 0;
	if(select(rsock+1, &rset, NULL, NULL, &timeout) < 0) continue;
	for(int i=0;i<num_instances;i++) {
		if(FD_ISSET(sockets[i],&rset)) {
			len = recvfrom(sockets[i],(char *)&buff,sizeof(buff),0,(struct sockaddr *)&si_other,&slen);
			handleConnection(sockets[i], (struct sockaddr_in *)&si_other, i+1, (char *)&buff, len);	
		}
	}
	checkTimeouts();
  }
  return NULL;
}
Exemple #5
0
void *openspy_mod_run(modLoadOptions *options) {
  int sd;
  int len;
  char buf[MAX_DATA_SIZE + 1];
  #ifndef _WIN32
  gi = GeoIP_new(GEOIP_STANDARD);
  #else
  WSADATA ws;
  WSAStartup(MAKEWORD(1,0),&ws);
  #endif
  struct sockaddr si_other;
  memcpy(&servoptions,options,sizeof(modLoadOptions));
  socklen_t slen = sizeof(si_other);
  struct sockaddr_in si_me;
  if((sd=socket(AF_INET,SOCK_DGRAM, IPPROTO_UDP)) == -1)
   return NULL;
  memset((char *)&si_me, 0, sizeof(si_me));
  si_me.sin_family = AF_INET;
  si_me.sin_port = htons(QRPORT);
  si_me.sin_addr.s_addr = options->bindIP;
  if(bind(sd,(struct sockaddr *)&si_me,sizeof(si_me)) == -1)
   return NULL;
	struct timeval tv;
  for(;;) {
    memset((char *)&buf,0, sizeof(buf));
    tv.tv_sec = 60;
    tv.tv_usec = 0;
    setsockopt(sd, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv,  sizeof tv);
    len = recvfrom(sd,(char *)&buf,sizeof(buf), 0, (struct sockaddr *)&si_other, &slen);
    if(len < 0) { //timeout, do keep alives and delete clients who have expired
	checkTimeouts();
	continue;
    }
    buf[len] = 0;
//  makeStringSafe((char *)&buf, sizeof(buf));
    handleClient(sd,(struct sockaddr_in *)&si_other,(char *)&buf,len);
  }
	return NULL;
}
void BlockLocalPositionEstimator::update()
{
	// wait for a sensor update, check for exit condition every 100 ms
	int ret = px4_poll(_polls, 3, 100);

	if (ret < 0) {
		return;
	}

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// set dt for all child blocks
	setDt(dt);

	// auto-detect connected rangefinders while not armed
	bool armedState = _sub_armed.get().armed;

	if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr)) {
		// detect distance sensors
		for (int i = 0; i < N_DIST_SUBS; i++) {
			uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];

			if (s == _sub_lidar || s == _sub_sonar) { continue; }

			if (s->updated()) {
				s->update();

				if (s->get().timestamp == 0) { continue; }

				if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
				    s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
				    _sub_lidar == nullptr) {
					_sub_lidar = s;
					mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %i", msg_label, i);

				} else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
					   s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
					   _sub_sonar == nullptr) {
					_sub_sonar = s;
					mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %i", msg_label, i);
				}
			}
		}
	}

	// reset pos, vel, and terrain on arming

	// XXX this will be re-enabled for indoor use cases using a
	// selection param, but is really not helping outdoors
	// right now.

	// if (!_lastArmedState && armedState) {

	//      // we just armed, we are at origin on the ground
	//      _x(X_x) = 0;
	//      _x(X_y) = 0;
	//      // reset Z or not? _x(X_z) = 0;

	//      // we aren't moving, all velocities are zero
	//      _x(X_vx) = 0;
	//      _x(X_vy) = 0;
	//      _x(X_vz) = 0;

	//      // assume we are on the ground, so terrain alt is local alt
	//      _x(X_tz) = _x(X_z);

	//      // reset lowpass filter as well
	//      _xLowPass.setState(_x);
	//      _aglLowPass.setState(0);
	// }

	_lastArmedState = armedState;

	// see which updates are available
	bool paramsUpdated = _sub_param_update.updated();
	_baroUpdated = false;

	if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated()) {
		int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative;

		if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID) {
			uint64_t baro_timestamp = _sub_sensor.get().timestamp +	\
						  _sub_sensor.get().baro_timestamp_relative;

			if (baro_timestamp != _timeStampLastBaro) {
				_baroUpdated = true;
				_timeStampLastBaro = baro_timestamp;
			}
		}
	}

	_flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
	_gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
	_visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated();
	_mocapUpdated = _sub_mocap.updated();
	_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
	_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
	_landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate
	bool targetPositionUpdated = _sub_landing_target_pose.updated();

	// get new data
	updateSubscriptions();

	// update parameters
	if (paramsUpdated) {
		updateParams();
		updateSSParams();
	}

	// is xy valid?
	bool vxy_stddev_ok = false;

	if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get() * _vxy_pub_thresh.get()) {
		vxy_stddev_ok = true;
	}

	if (_estimatorInitialized & EST_XY) {
		// if valid and gps has timed out, set to not valid
		if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS)) {
			_estimatorInitialized &= ~EST_XY;
		}

	} else {
		if (vxy_stddev_ok) {
			if (!(_sensorTimeout & SENSOR_GPS)
			    || !(_sensorTimeout & SENSOR_FLOW)
			    || !(_sensorTimeout & SENSOR_VISION)
			    || !(_sensorTimeout & SENSOR_MOCAP)
			    || !(_sensorTimeout & SENSOR_LAND)
			    || !(_sensorTimeout & SENSOR_LAND_TARGET)
			   ) {
				_estimatorInitialized |= EST_XY;
			}
		}
	}

	// is z valid?
	bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();

	if (_estimatorInitialized & EST_Z) {
		// if valid and baro has timed out, set to not valid
		if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO)) {
			_estimatorInitialized &= ~EST_Z;
		}

	} else {
		if (z_stddev_ok) {
			_estimatorInitialized |= EST_Z;
		}
	}

	// is terrain valid?
	bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();

	if (_estimatorInitialized & EST_TZ) {
		if (!tz_stddev_ok) {
			_estimatorInitialized &= ~EST_TZ;
		}

	} else {
		if (tz_stddev_ok) {
			_estimatorInitialized |= EST_TZ;
		}
	}

	// check timeouts
	checkTimeouts();

	// if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters
	if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _fake_origin.get()) {
		map_projection_init(&_map_ref,
				    _init_origin_lat.get(),
				    _init_origin_lon.get());

		// set timestamp when origin was set to current time
		_time_origin = _timeStamp;

		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
					     double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
	}

	// reinitialize x if necessary
	bool reinit_x = false;

	for (int i = 0; i < n_x; i++) {
		// should we do a reinit
		// of sensors here?
		// don't want it to take too long
		if (!PX4_ISFINITE(_x(i))) {
			reinit_x = true;
			mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%d) not finite", msg_label, i);
			break;
		}
	}

	if (reinit_x) {
		for (int i = 0; i < n_x; i++) {
			_x(i) = 0;
		}

		mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
	}

	// force P symmetry and reinitialize P if necessary
	bool reinit_P = false;

	for (int i = 0; i < n_x; i++) {
		for (int j = 0; j <= i; j++) {
			if (!PX4_ISFINITE(_P(i, j))) {
				mavlink_and_console_log_info(&mavlink_log_pub,
							     "%sreinit P (%d, %d) not finite", msg_label, i, j);
				reinit_P = true;
			}

			if (i == j) {
				// make sure diagonal elements are positive
				if (_P(i, i) <= 0) {
					mavlink_and_console_log_info(&mavlink_log_pub,
								     "%sreinit P (%d, %d) negative", msg_label, i, j);
					reinit_P = true;
				}

			} else {
				// copy elememnt from upper triangle to force
				// symmetry
				_P(j, i) = _P(i, j);
			}

			if (reinit_P) { break; }
		}

		if (reinit_P) { break; }
	}

	if (reinit_P) {
		initP();
	}

	// do prediction
	predict();

	// sensor corrections/ initializations
	if (_gpsUpdated) {
		if (_sensorTimeout & SENSOR_GPS) {
			gpsInit();

		} else {
			gpsCorrect();
		}
	}

	if (_baroUpdated) {
		if (_sensorTimeout & SENSOR_BARO) {
			baroInit();

		} else {
			baroCorrect();
		}
	}

	if (_lidarUpdated) {
		if (_sensorTimeout & SENSOR_LIDAR) {
			lidarInit();

		} else {
			lidarCorrect();
		}
	}

	if (_sonarUpdated) {
		if (_sensorTimeout & SENSOR_SONAR) {
			sonarInit();

		} else {
			sonarCorrect();
		}
	}

	if (_flowUpdated) {
		if (_sensorTimeout & SENSOR_FLOW) {
			flowInit();

		} else {
			flowCorrect();
		}
	}

	if (_visionUpdated) {
		if (_sensorTimeout & SENSOR_VISION) {
			visionInit();

		} else {
			visionCorrect();
		}
	}

	if (_mocapUpdated) {
		if (_sensorTimeout & SENSOR_MOCAP) {
			mocapInit();

		} else {
			mocapCorrect();
		}
	}

	if (_landUpdated) {
		if (_sensorTimeout & SENSOR_LAND) {
			landInit();

		} else {
			landCorrect();
		}
	}

	if (targetPositionUpdated) {
		if (_sensorTimeout & SENSOR_LAND_TARGET) {
			landingTargetInit();

		} else {
			landingTargetCorrect();
		}
	}

	if (_altOriginInitialized) {
		// update all publications if possible
		publishLocalPos();
		publishEstimatorStatus();
		_pub_innov.get().timestamp = _timeStamp;
		_pub_innov.update();

		if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get())) {
			publishGlobalPos();
		}
	}

	// propagate delayed state, no matter what
	// if state is frozen, delayed state still
	// needs to be propagated with frozen state
	float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);

	if (_time_last_hist == 0 ||
	    (dt_hist > HIST_STEP)) {
		_tDelay.update(Scalar<uint64_t>(_timeStamp));
		_xDelay.update(_x);
		_time_last_hist = _timeStamp;
	}
}
void BlockLocalPositionEstimator::update()
{

	// wait for a sensor update, check for exit condition every 100 ms
	int ret = px4_poll(_polls, 3, 100);

	if (ret < 0) {
		/* poll error, count it in perf */
		perf_count(_err_perf);
		return;
	}

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// set dt for all child blocks
	setDt(dt);

	// auto-detect connected rangefinders while not armed
	bool armedState = _sub_armed.get().armed;

	if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
		detectDistanceSensors();
	}

	// reset pos, vel, and terrain on arming
	if (!_lastArmedState && armedState) {

		// we just armed, we are at home position on the ground
		_x(X_x) = 0;
		_x(X_y) = 0;

		// the pressure altitude of home may have drifted, so we don't
		// reset z to zero

		// reset flow integral
		_flowX = 0;
		_flowY = 0;

		// we aren't moving, all velocities are zero
		_x(X_vx) = 0;
		_x(X_vy) = 0;
		_x(X_vz) = 0;

		// assume we are on the ground, so terrain alt is local alt
		_x(X_tz) = _x(X_z);

		// reset lowpass filter as well
		_xLowPass.setState(_x);
	}

	_lastArmedState = armedState;

	// see which updates are available
	bool flowUpdated = _sub_flow.updated();
	bool paramsUpdated = _sub_param_update.updated();
	bool baroUpdated = _sub_sensor.updated();
	bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
	bool homeUpdated = _sub_home.updated();
	bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
	bool mocapUpdated = _sub_mocap.updated();
	bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
	bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();

	// get new data
	updateSubscriptions();

	// update parameters
	if (paramsUpdated) {
		updateParams();
		updateSSParams();
	}

	// update home position projection
	if (homeUpdated) {
		updateHome();
	}

	// is xy valid?
	bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();

	if (_validXY) {
		// if valid and gps has timed out, set to not valid
		if (!xy_stddev_ok && !_gpsInitialized) {
			_validXY = false;
		}

	} else {
		if (xy_stddev_ok) {
			_validXY = true;
		}
	}

	// is z valid?
	bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();

	if (_validZ) {
		// if valid and baro has timed out, set to not valid
		if (!z_stddev_ok && !_baroInitialized) {
			_validZ = false;
		}

	} else {
		if (z_stddev_ok) {
			_validZ = true;
		}
	}

	// is terrain valid?
	bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();

	if (_validTZ) {
		if (!tz_stddev_ok) {
			_validTZ = false;
		}

	} else {
		if (tz_stddev_ok) {
			_validTZ = true;
		}
	}

	// timeouts
	if (_validXY) {
		_time_last_xy = _timeStamp;
	}

	if (_validZ) {
		_time_last_z = _timeStamp;
	}

	if (_validTZ) {
		_time_last_tz = _timeStamp;
	}

	// check timeouts
	checkTimeouts();

	// if we have no lat, lon initialize projection at 0,0
	if (_validXY && !_map_ref.init_done) {
		map_projection_init(&_map_ref,
				    _init_home_lat.get(),
				    _init_home_lon.get());
	}

	// reinitialize x if necessary
	bool reinit_x = false;

	for (int i = 0; i < n_x; i++) {
		// should we do a reinit
		// of sensors here?
		// don't want it to take too long
		if (!PX4_ISFINITE(_x(i))) {
			reinit_x = true;
			break;
		}
	}

	if (reinit_x) {
		for (int i = 0; i < n_x; i++) {
			_x(i) = 0;
		}

		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
	}

	// reinitialize P if necessary
	bool reinit_P = false;

	for (int i = 0; i < n_x; i++) {
		for (int j = 0; j < n_x; j++) {
			if (!PX4_ISFINITE(_P(i, j))) {
				reinit_P = true;
				break;
			}
		}

		if (reinit_P) { break; }
	}

	if (reinit_P) {
		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P");
		initP();
	}

	// do prediction
	predict();

	// sensor corrections/ initializations
	if (gpsUpdated) {
		if (!_gpsInitialized) {
			gpsInit();

		} else {
			gpsCorrect();
		}
	}

	if (baroUpdated) {
		if (!_baroInitialized) {
			baroInit();

		} else {
			baroCorrect();
		}
	}

	if (lidarUpdated) {
		if (!_lidarInitialized) {
			lidarInit();

		} else {
			lidarCorrect();
		}
	}

	if (sonarUpdated) {
		if (!_sonarInitialized) {
			sonarInit();

		} else {
			sonarCorrect();
		}
	}

	if (flowUpdated) {
		if (!_flowInitialized) {
			flowInit();

		} else {
			perf_begin(_loop_perf);// TODO
			flowCorrect();
			//perf_count(_interval_perf);
			perf_end(_loop_perf);
		}
	}

	if (visionUpdated) {
		if (!_visionInitialized) {
			visionInit();

		} else {
			visionCorrect();
		}
	}

	if (mocapUpdated) {
		if (!_mocapInitialized) {
			mocapInit();

		} else {
			mocapCorrect();
		}
	}

	if (_altHomeInitialized) {
		// update all publications if possible
		publishLocalPos();
		publishEstimatorStatus();

		if (_validXY) {
			publishGlobalPos();
		}
	}

	// propagate delayed state, no matter what
	// if state is frozen, delayed state still
	// needs to be propagated with frozen state
	float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);

	if (_time_last_hist == 0 ||
	    (dt_hist > HIST_STEP)) {
		_tDelay.update(Scalar<uint64_t>(_timeStamp));
		_xDelay.update(_x);
		_time_last_hist = _timeStamp;
	}
}
void BlockLocalPositionEstimator::update()
{

	// wait for a sensor update, check for exit condition every 100 ms
	int ret = px4_poll(_polls, 3, 100);

	if (ret < 0) {
		/* poll error, count it in perf */
		perf_count(_err_perf);
		return;
	}

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// set dt for all child blocks
	setDt(dt);

	// auto-detect connected rangefinders while not armed
	bool armedState = _sub_armed.get().armed;

	if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
		detectDistanceSensors();
	}

	// reset pos, vel, and terrain on arming

	// XXX this will be re-enabled for indoor use cases using a
	// selection param, but is really not helping outdoors
	// right now.

	// if (!_lastArmedState && armedState) {

	// 	// we just armed, we are at origin on the ground
	// 	_x(X_x) = 0;
	// 	_x(X_y) = 0;
	// 	// reset Z or not? _x(X_z) = 0;

	// 	// we aren't moving, all velocities are zero
	// 	_x(X_vx) = 0;
	// 	_x(X_vy) = 0;
	// 	_x(X_vz) = 0;

	// 	// assume we are on the ground, so terrain alt is local alt
	// 	_x(X_tz) = _x(X_z);

	// 	// reset lowpass filter as well
	// 	_xLowPass.setState(_x);
	// 	_aglLowPass.setState(0);
	// }

	_lastArmedState = armedState;

	// see which updates are available
	bool flowUpdated = _sub_flow.updated();
	bool paramsUpdated = _sub_param_update.updated();
	bool baroUpdated = _sub_sensor.updated();
	bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
	bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
	bool mocapUpdated = _sub_mocap.updated();
	bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
	bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();
	bool landUpdated = (
				   (_sub_land.get().landed ||
				    ((!_sub_armed.get().armed) && (!_sub_land.get().freefall)))
				   && (!(_lidarInitialized || _mocapInitialized || _visionInitialized || _sonarInitialized))
				   && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE));

	// get new data
	updateSubscriptions();

	// update parameters
	if (paramsUpdated) {
		updateParams();
		updateSSParams();
	}

	// is xy valid?
	bool vxy_stddev_ok = false;

	if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get()*_vxy_pub_thresh.get()) {
		vxy_stddev_ok = true;
	}

	if (_validXY) {
		// if valid and gps has timed out, set to not valid
		if (!vxy_stddev_ok && !_gpsInitialized) {
			_validXY = false;
		}

	} else {
		if (vxy_stddev_ok) {
			if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) {
				_validXY = true;
			}
		}
	}

	// is z valid?
	bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();

	if (_validZ) {
		// if valid and baro has timed out, set to not valid
		if (!z_stddev_ok && !_baroInitialized) {
			_validZ = false;
		}

	} else {
		if (z_stddev_ok) {
			_validZ = true;
		}
	}

	// is terrain valid?
	bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();

	if (_validTZ) {
		if (!tz_stddev_ok) {
			_validTZ = false;
		}

	} else {
		if (tz_stddev_ok) {
			_validTZ = true;
		}
	}

	// timeouts
	if (_validXY) {
		_time_last_xy = _timeStamp;
	}

	if (_validZ) {
		_time_last_z = _timeStamp;
	}

	if (_validTZ) {
		_time_last_tz = _timeStamp;
	}

	// check timeouts
	checkTimeouts();

	// if we have no lat, lon initialize projection at 0,0
	if (_validXY && !_map_ref.init_done) {
		map_projection_init(&_map_ref,
				    _init_origin_lat.get(),
				    _init_origin_lon.get());
	}

	// reinitialize x if necessary
	bool reinit_x = false;

	for (int i = 0; i < n_x; i++) {
		// should we do a reinit
		// of sensors here?
		// don't want it to take too long
		if (!PX4_ISFINITE(_x(i))) {
			reinit_x = true;
			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x, x(%d) not finite", i);
			break;
		}
	}

	if (reinit_x) {
		for (int i = 0; i < n_x; i++) {
			_x(i) = 0;
		}

		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
	}

	// force P symmetry and reinitialize P if necessary
	bool reinit_P = false;

	for (int i = 0; i < n_x; i++) {
		for (int j = 0; j <= i; j++) {
			if (!PX4_ISFINITE(_P(i, j))) {
				reinit_P = true;
			}

			if (i == j) {
				// make sure diagonal elements are positive
				if (_P(i, i) <= 0) {
					reinit_P = true;
				}

			} else {
				// copy elememnt from upper triangle to force
				// symmetry
				_P(j, i) = _P(i, j);
			}

			if (reinit_P) { break; }
		}

		if (reinit_P) { break; }
	}

	if (reinit_P) {
		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P");
		initP();
	}

	// do prediction
	predict();

	// sensor corrections/ initializations
	if (gpsUpdated) {
		if (!_gpsInitialized) {
			gpsInit();

		} else {
			gpsCorrect();
		}
	}

	if (baroUpdated) {
		if (!_baroInitialized) {
			baroInit();

		} else {
			baroCorrect();
		}
	}

	if (lidarUpdated) {
		if (!_lidarInitialized) {
			lidarInit();

		} else {
			lidarCorrect();
		}
	}

	if (sonarUpdated) {
		if (!_sonarInitialized) {
			sonarInit();

		} else {
			sonarCorrect();
		}
	}

	if (flowUpdated) {
		if (!_flowInitialized) {
			flowInit();

		} else {
			perf_begin(_loop_perf);// TODO
			flowCorrect();
			//perf_count(_interval_perf);
			perf_end(_loop_perf);
		}
	}

	if (visionUpdated) {
		if (!_visionInitialized) {
			visionInit();

		} else {
			visionCorrect();
		}
	}

	if (mocapUpdated) {
		if (!_mocapInitialized) {
			mocapInit();

		} else {
			mocapCorrect();
		}
	}

	if (landUpdated) {
		if (!_landInitialized) {
			landInit();

		} else {
			landCorrect();
		}
	}

	if (_altOriginInitialized) {
		// update all publications if possible
		publishLocalPos();
		publishEstimatorStatus();
		_pub_innov.update();

		if (_validXY) {
			publishGlobalPos();
		}
	}

	// propagate delayed state, no matter what
	// if state is frozen, delayed state still
	// needs to be propagated with frozen state
	float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);

	if (_time_last_hist == 0 ||
	    (dt_hist > HIST_STEP)) {
		_tDelay.update(Scalar<uint64_t>(_timeStamp));
		_xDelay.update(_x);
		_time_last_hist = _timeStamp;
	}
}
Exemple #9
0
/* Select on all sockets; call handlers for those that are ready. */
int
comm_select(int msec)
{
    fd_set readfds;
    fd_set pendingfds;
    fd_set writefds;
#if DELAY_POOLS
    BIT_ARRAY slowfds;
#endif
    PF *hdl = NULL;
    int fd;
    int maxfd;
    int num;
    int pending;
    int callicp = 0, callhttp = 0;
    int calldns = 0;
    int maxindex;
    int k;
    int j;
#if DEBUG_FDBITS
    int i;
#endif
    fd_mask *fdsp;
    fd_mask *pfdsp;
    fd_mask tmask;
    static time_t last_timeout = 0;
    struct timeval poll_time;
    double timeout = current_dtime + (msec / 1000.0);
    fde *F;
#if DELAY_POOLS
    BA_INIT(slowfds,SQUID_NUMFD);
#endif
    do {
#if !ALARM_UPDATES_TIME
	double start;
	getCurrentTime();
	start = current_dtime;
#endif
#if DELAY_POOLS
	BA_ZERO(slowfds);
#endif
	/* Handle any fs callbacks that need doing */
	storeDirCallback();
	if (commCheckICPIncoming)
	    comm_select_icp_incoming();
	if (commCheckDNSIncoming)
	    comm_select_dns_incoming();
	if (commCheckHTTPIncoming)
	    comm_select_http_incoming();
	callicp = calldns = callhttp = 0;
	maxfd = Biggest_FD + 1;
        xmemcpy(&readfds, &global_readfds,
            howmany(maxfd, FD_MASK_BITS) * FD_MASK_BYTES);
        xmemcpy(&writefds, &global_writefds,
            howmany(maxfd, FD_MASK_BITS) * FD_MASK_BYTES);
	/* remove stalled FDs, and deal with pending descriptors */
	pending = 0;
	FD_ZERO(&pendingfds);
	maxindex = howmany(maxfd, FD_MASK_BITS);
	fdsp = (fd_mask *) & readfds;
	for (j = 0; j < maxindex; j++) {
	    if ((tmask = fdsp[j]) == 0)
		continue;	/* no bits here */
	    for (k = 0; k < FD_MASK_BITS; k++) {
		if (!EBIT_TEST(tmask, k))
		    continue;
		/* Found a set bit */
		fd = (j * FD_MASK_BITS) + k;
		switch (commDeferRead(fd)) {
		case 0:
		    break;
		case 1:
		    FD_CLR(fd, &readfds);
		    break;
#if DELAY_POOLS
		case -1:
		    BA_SET(fd, slowfds);
		    break;
#endif
		default:
		    fatalf("bad return value from commDeferRead(FD %d)\n", fd);
		}
		if (FD_ISSET(fd, &readfds) && fd_table[fd].flags.read_pending) {
		    FD_SET(fd, &pendingfds);
		    pending++;
		}
	    }
	}
#if DEBUG_FDBITS
	for (i = 0; i < maxfd; i++) {
	    /* Check each open socket for a handler. */
#if DELAY_POOLS
	    if (fd_table[i].read_handler && commDeferRead(i) != 1) {
#else
	    if (fd_table[i].read_handler && !commDeferRead(i)) {
#endif
		assert(FD_ISSET(i, &readfds));
	    }
	    if (fd_table[i].write_handler) {
		assert(FD_ISSET(i, &writefds));
	    }
	}
#endif
	if (nreadfds + nwritefds == 0) {
	    assert(shutting_down);
#if DELAY_POOLS
            BA_FREE(slowfds);
#endif
	    return COMM_SHUTDOWN;
	}
	if (msec > MAX_POLL_TIME)
	    msec = MAX_POLL_TIME;
	if (pending)
	    msec = 0;
	for (;;) {
	    poll_time.tv_sec = msec / 1000;
	    poll_time.tv_usec = (msec % 1000) * 1000;
	    statCounter.syscalls.selects++;
	    num = select(maxfd, &readfds, &writefds, NULL, &poll_time);
	    statCounter.select_loops++;
	    if (num >= 0 || pending > 0)
		break;
	    if (ignoreErrno(errno))
		break;
	    debug(50, 0) ("comm_select: select failure: %s\n",
		xstrerror());
	    examine_select(&readfds, &writefds);
#if DELAY_POOLS
            BA_FREE(slowfds);
#endif
	    return COMM_ERROR;
	    /* NOTREACHED */
	}
	if (num < 0 && !pending)
	    continue;
	debug(5, num ? 5 : 8) ("comm_select: %d+%d FDs ready at %d\n",
	    num, pending, (int) squid_curtime);
	statHistCount(&statCounter.select_fds_hist, num);
	/* Check lifetime and timeout handlers ONCE each second.
	 * Replaces brain-dead check every time through the loop! */
	if (squid_curtime > last_timeout) {
	    last_timeout = squid_curtime;
	    checkTimeouts();
	}
	if (num == 0 && pending == 0)
	    continue;
	/* Scan return fd masks for ready descriptors */
	fdsp = (fd_mask *) & readfds;
	pfdsp = (fd_mask *) & pendingfds;
	maxindex = howmany(maxfd, FD_MASK_BITS);
	for (j = 0; j < maxindex; j++) {
	    if ((tmask = (fdsp[j] | pfdsp[j])) == 0)
		continue;	/* no bits here */
	    for (k = 0; k < FD_MASK_BITS; k++) {
		if (tmask == 0)
		    break;	/* no more bits left */
		if (!EBIT_TEST(tmask, k))
		    continue;
		/* Found a set bit */
		fd = (j * FD_MASK_BITS) + k;
		EBIT_CLR(tmask, k);	/* this will be done */
#if DEBUG_FDBITS
		debug(5, 9) ("FD %d bit set for reading\n", fd);
		assert(FD_ISSET(fd, &readfds));
#endif
		if (fdIsIcp(fd)) {
		    callicp = 1;
		    continue;
		}
		if (fdIsDns(fd)) {
		    calldns = 1;
		    continue;
		}
		if (fdIsHttp(fd)) {
		    callhttp = 1;
		    continue;
		}
		F = &fd_table[fd];
		debug(5, 6) ("comm_select: FD %d ready for reading\n", fd);
		if (NULL == (hdl = F->read_handler))
		    (void) 0;
#if DELAY_POOLS
		else if (BA_ISSET(fd, slowfds))
		    commAddSlowFd(fd);
#endif
		else {
		    F->read_handler = NULL;
		    commUpdateReadBits(fd, NULL);
		    hdl(fd, F->read_data);
		    statCounter.select_fds++;
		    if (commCheckICPIncoming)
			comm_select_icp_incoming();
		    if (commCheckDNSIncoming)
			comm_select_dns_incoming();
		    if (commCheckHTTPIncoming)
			comm_select_http_incoming();
		}
	    }
	}
	fdsp = (fd_mask *) & writefds;
	for (j = 0; j < maxindex; j++) {
	    if ((tmask = fdsp[j]) == 0)
		continue;	/* no bits here */
	    for (k = 0; k < FD_MASK_BITS; k++) {
		if (tmask == 0)
		    break;	/* no more bits left */
		if (!EBIT_TEST(tmask, k))
		    continue;
		/* Found a set bit */
		fd = (j * FD_MASK_BITS) + k;
		EBIT_CLR(tmask, k);	/* this will be done */
#if DEBUG_FDBITS
		debug(5, 9) ("FD %d bit set for writing\n", fd);
		assert(FD_ISSET(fd, &writefds));
#endif
		if (fdIsIcp(fd)) {
		    callicp = 1;
		    continue;
		}
		if (fdIsDns(fd)) {
		    calldns = 1;
		    continue;
		}
		if (fdIsHttp(fd)) {
		    callhttp = 1;
		    continue;
		}
		F = &fd_table[fd];
		debug(5, 5) ("comm_select: FD %d ready for writing\n", fd);
		if ((hdl = F->write_handler)) {
		    F->write_handler = NULL;
		    commUpdateWriteBits(fd, NULL);
		    hdl(fd, F->write_data);
		    statCounter.select_fds++;
		    if (commCheckICPIncoming)
			comm_select_icp_incoming();
		    if (commCheckDNSIncoming)
			comm_select_dns_incoming();
		    if (commCheckHTTPIncoming)
			comm_select_http_incoming();
		}
	    }
	}
	if (callicp)
	    comm_select_icp_incoming();
	if (calldns)
	    comm_select_dns_incoming();
	if (callhttp)
	    comm_select_http_incoming();
#if DELAY_POOLS
	while ((fd = commGetSlowFd()) != -1) {
	    F = &fd_table[fd];
	    debug(5, 6) ("comm_select: slow FD %d selected for reading\n", fd);
	    if ((hdl = F->read_handler)) {
		F->read_handler = NULL;
		commUpdateReadBits(fd, NULL);
		hdl(fd, F->read_data);
		statCounter.select_fds++;
		if (commCheckICPIncoming)
		    comm_select_icp_incoming();
		if (commCheckDNSIncoming)
		    comm_select_dns_incoming();
		if (commCheckHTTPIncoming)
		    comm_select_http_incoming();
	    }
	}
#endif
#if !ALARM_UPDATES_TIME
	getCurrentTime();
	statCounter.select_time += (current_dtime - start);
#endif
#if DELAY_POOLS
        BA_FREE(slowfds);
#endif
	return COMM_OK;
    }
    while (timeout > current_dtime);
    debug(5, 8) ("comm_select: time out: %d\n", (int) squid_curtime);
#if DELAY_POOLS
    BA_FREE(slowfds);
#endif
    return COMM_TIMEOUT;
}
#endif

static void
#if HAVE_POLL
comm_poll_dns_incoming(void)
#else
comm_select_dns_incoming(void)
#endif
{
    int nfds = 0;
    int fds[2];
    int nevents;
    dns_io_events = 0;
    if (DnsSocket < 0)
	return;
    fds[nfds++] = DnsSocket;
#if HAVE_POLL
    nevents = comm_check_incoming_poll_handlers(nfds, fds);
#else
    nevents = comm_check_incoming_select_handlers(nfds, fds);
#endif
    if (nevents < 0)
	return;
    incoming_dns_interval += Config.comm_incoming.dns_average - nevents;
    if (incoming_dns_interval < Config.comm_incoming.dns_min_poll)
	incoming_dns_interval = Config.comm_incoming.dns_min_poll;
    if (incoming_dns_interval > MAX_INCOMING_INTERVAL)
	incoming_dns_interval = MAX_INCOMING_INTERVAL;
    if (nevents > INCOMING_DNS_MAX)
	nevents = INCOMING_DNS_MAX;
    statHistCount(&statCounter.comm_dns_incoming, nevents);
}
Exemple #10
0
/* poll all sockets; call handlers for those that are ready. */
int
comm_poll(int msec)
{
#if FD_CONFIG
    struct pollfd *pfds = NULL;
#else
    struct pollfd pfds[SQUID_NUMFD];
#endif
#if DELAY_POOLS
    BIT_ARRAY slowfds;
#endif
    PF *hdl = NULL;
    int fd;
    unsigned int i;
    unsigned int maxfd;
    unsigned int nfds;
    unsigned int npending;
    int num;
    int callicp = 0, callhttp = 0;
    int calldns = 0;
    static time_t last_timeout = 0;
    double timeout = current_dtime + (msec / 1000.0);
#if FD_CONFIG
    pfds = xmalloc(sizeof(pfds[0])*SQUID_NUMFD);
#endif
#if DELAY_POOLS
    BA_INIT(slowfds,SQUID_NUMFD);
#endif
    do {
#if !ALARM_UPDATES_TIME
	double start;
	getCurrentTime();
	start = current_dtime;
#endif
	/* Handle any fs callbacks that need doing */
	storeDirCallback();
#if DELAY_POOLS
	BA_ZERO(slowfds);
#endif
	if (commCheckICPIncoming)
	    comm_poll_icp_incoming();
	if (commCheckDNSIncoming)
	    comm_poll_dns_incoming();
	if (commCheckHTTPIncoming)
	    comm_poll_http_incoming();
	callicp = calldns = callhttp = 0;
	nfds = 0;
	npending = 0;
	maxfd = Biggest_FD + 1;
	for (i = 0; i < maxfd; i++) {
	    int events;
	    events = 0;
	    /* Check each open socket for a handler. */
	    if (fd_table[i].read_handler) {
		switch (commDeferRead(i)) {
		case 0:
		    events |= POLLRDNORM;
		    break;
		case 1:
		    break;
#if DELAY_POOLS
		case -1:
		    events |= POLLRDNORM;
		    BA_SET(i, slowfds);
		    break;
#endif
		default:
		    fatalf("bad return value from commDeferRead(FD %d)\n", i);
		}
	    }
	    if (fd_table[i].write_handler)
		events |= POLLWRNORM;
	    if (events) {
		pfds[nfds].fd = i;
		pfds[nfds].events = events;
		pfds[nfds].revents = 0;
		nfds++;
		if ((events & POLLRDNORM) && fd_table[i].flags.read_pending)
		    npending++;
	    }
	}
	if (nfds == 0) {
	    assert(shutting_down);
#if DELAY_POOLS
            BA_FREE(slowfds);
#endif
#if FD_CONFIG
            xfree(pfds);
#endif
	    return COMM_SHUTDOWN;
	}
	if (npending)
	    msec = 0;
	if (msec > MAX_POLL_TIME)
	    msec = MAX_POLL_TIME;
	for (;;) {
	    statCounter.syscalls.polls++;
	    num = poll(pfds, nfds, msec);
	    statCounter.select_loops++;
	    if (num >= 0 || npending > 0)
		break;
	    if (ignoreErrno(errno))
		continue;
	    debug(5, 0) ("comm_poll: poll failure: %s\n", xstrerror());
	    assert(errno != EINVAL);
#if DELAY_POOLS
            BA_FREE(slowfds);
#endif
#if FD_CONFIG
            xfree(pfds);
#endif
	    return COMM_ERROR;
	    /* NOTREACHED */
	}
	debug(5, num ? 5 : 8) ("comm_poll: %d+%u FDs ready\n", num, npending);
	statHistCount(&statCounter.select_fds_hist, num);
	/* Check timeout handlers ONCE each second. */
	if (squid_curtime > last_timeout) {
	    last_timeout = squid_curtime;
	    checkTimeouts();
	}
	if (num <= 0 && npending == 0)
	    continue;
	/* scan each socket but the accept socket. Poll this 
	 * more frequently to minimize losses due to the 5 connect 
	 * limit in SunOS */
	for (i = 0; i < nfds; i++) {
	    fde *F;
	    int revents = pfds[i].revents;
	    fd = pfds[i].fd;
	    if (fd == -1)
		continue;
	    if (fd_table[fd].flags.read_pending)
		revents |= POLLIN;
	    if (revents == 0)
		continue;
	    if (fdIsIcp(fd)) {
		callicp = 1;
		continue;
	    }
	    if (fdIsDns(fd)) {
		calldns = 1;
		continue;
	    }
	    if (fdIsHttp(fd)) {
		callhttp = 1;
		continue;
	    }
	    F = &fd_table[fd];
	    if (revents & (POLLRDNORM | POLLIN | POLLHUP | POLLERR)) {
		debug(5, 6) ("comm_poll: FD %d ready for reading\n", fd);
		if (NULL == (hdl = F->read_handler))
		    (void) 0;
#if DELAY_POOLS
		else if (BA_ISSET(fd, slowfds))
		    commAddSlowFd(fd);
#endif
		else {
		    F->read_handler = NULL;
		    hdl(fd, F->read_data);
		    statCounter.select_fds++;
		    if (commCheckICPIncoming)
			comm_poll_icp_incoming();
		    if (commCheckDNSIncoming)
			comm_poll_dns_incoming();
		    if (commCheckHTTPIncoming)
			comm_poll_http_incoming();
		}
	    }
	    if (revents & (POLLWRNORM | POLLOUT | POLLHUP | POLLERR)) {
		debug(5, 5) ("comm_poll: FD %d ready for writing\n", fd);
		if ((hdl = F->write_handler)) {
		    F->write_handler = NULL;
		    hdl(fd, F->write_data);
		    statCounter.select_fds++;
		    if (commCheckICPIncoming)
			comm_poll_icp_incoming();
		    if (commCheckDNSIncoming)
			comm_poll_dns_incoming();
		    if (commCheckHTTPIncoming)
			comm_poll_http_incoming();
		}
	    }
	    if (revents & POLLNVAL) {
		close_handler *ch;
		debug(5, 0) ("WARNING: FD %d has handlers, but it's invalid.\n", fd);
		debug(5, 0) ("FD %d is a %s\n", fd, fdTypeStr[F->type]);
		debug(5, 0) ("--> %s\n", F->desc);
		debug(5, 0) ("tmout:%p read:%p write:%p\n",
		    F->timeout_handler,
		    F->read_handler,
		    F->write_handler);
		for (ch = F->close_handler; ch; ch = ch->next)
		    debug(5, 0) (" close handler: %p\n", ch->handler);
		if (F->close_handler) {
		    commCallCloseHandlers(fd);
		} else if (F->timeout_handler) {
		    debug(5, 0) ("comm_poll: Calling Timeout Handler\n");
		    F->timeout_handler(fd, F->timeout_data);
		}
		F->close_handler = NULL;
		F->timeout_handler = NULL;
		F->read_handler = NULL;
		F->write_handler = NULL;
		if (F->flags.open)
		    fd_close(fd);
	    }
	}
	if (callicp)
	    comm_poll_icp_incoming();
	if (calldns)
	    comm_poll_dns_incoming();
	if (callhttp)
	    comm_poll_http_incoming();
#if DELAY_POOLS
	while ((fd = commGetSlowFd()) != -1) {
	    fde *F = &fd_table[fd];
	    debug(5, 6) ("comm_select: slow FD %d selected for reading\n", fd);
	    if ((hdl = F->read_handler)) {
		F->read_handler = NULL;
		hdl(fd, F->read_data);
		statCounter.select_fds++;
		if (commCheckICPIncoming)
		    comm_poll_icp_incoming();
		if (commCheckDNSIncoming)
		    comm_poll_dns_incoming();
		if (commCheckHTTPIncoming)
		    comm_poll_http_incoming();
	    }
	}
#endif
#if !ALARM_UPDATES_TIME
	getCurrentTime();
	statCounter.select_time += (current_dtime - start);
#endif
#if DELAY_POOLS
        BA_FREE(slowfds);
#endif
#if FD_CONFIG
        xfree(pfds);
#endif
	return COMM_OK;
    }
    while (timeout > current_dtime);
    debug(5, 8) ("comm_poll: time out: %ld.\n", (long int) squid_curtime);
#if DELAY_POOLS
    BA_FREE(slowfds);
#endif
#if FD_CONFIG
    xfree(pfds);
#endif
    return COMM_TIMEOUT;
}