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(); }
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; }
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; }
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; } }
/* 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); }
/* 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; }