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;
}
