bool MulticopterLandDetector::update()
{
// first poll for new data from our subscriptions
updateSubscriptions();
// only trigger flight conditions if we are armed
if (!_arming.armed) {
return true;
}
const uint64_t now = hrt_absolute_time();
// check if we are moving vertically
bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate;
// check if we are moving horizontally
bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity;
// next look if all rotation angles are not moving
bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] +
_sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] +
_sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation;
// check if thrust output is minimal (about half of default)
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
// sensed movement, so reset the land detector
_landTimer = now;
return false;
}
return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME;
}
bool FixedwingLandDetector::update()
{
// First poll for new data from our subscriptions
updateSubscriptions();
const uint64_t now = hrt_absolute_time();
bool landDetected = false;
// TODO: reset filtered values on arming?
_velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
_velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
// crude land detector for fixedwing
if (_velocity_xy_filtered < _params.maxVelocity
&& _velocity_z_filtered < _params.maxClimbRate
&& _airspeed_filtered < _params.maxAirSpeed) {
// these conditions need to be stable for a period of time before we trust them
if (now > _landDetectTrigger) {
landDetected = true;
}
} else {
// reset land detect trigger
_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME;
}
return landDetected;
}
bool VtolLandDetector::update()
{
updateSubscriptions();
updateParameterCache(false);
// this is returned from the mutlicopter land detector
bool landed = get_landed_state();
// for vtol we additionally consider airspeed
if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
} else {
// if airspeed does not update, set it to zero and rely on multicopter land detector
_airspeed_filtered = 0.0f;
}
// only consider airspeed if we have been in air before to avoid false
// detections in the case of wind on the ground
if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) {
landed = false;
}
_was_in_air = !landed;
return landed;
}
bool MulticopterLandDetector::update()
{
// first poll for new data from our subscriptions
updateSubscriptions();
// only trigger flight conditions if we are armed
if (!_arming.armed) {
_arming_time = 0;
return true;
} else if (_arming_time == 0) {
_arming_time = hrt_absolute_time();
}
// return status based on armed state if no position lock is available
if (_vehicleGlobalPosition.timestamp == 0 ||
hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) {
// no position lock - not landed if armed
return !_arming.armed;
}
const uint64_t now = hrt_absolute_time();
float armThresholdFactor = 1.0f;
// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives
if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
armThresholdFactor = 2.5f;
}
// check if we are moving vertically - this might see a spike after arming due to
// throttle-up vibration. If accelerating fast the throttle thresholds will still give
// an accurate in-air indication
bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate * armThresholdFactor;
// check if we are moving horizontally
bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity
&& _vehicleStatus.condition_global_position_valid;
// next look if all rotation angles are not moving
float maxRotationScaled = _params.maxRotation * armThresholdFactor;
bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
// check if thrust output is minimal (about half of default)
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
// sensed movement, so reset the land detector
_landTimer = now;
return false;
}
return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME;
}
bool MulticopterLandDetector::update()
{
// first poll for new data from our subscriptions
updateSubscriptions();
updateParameterCache(false);
return get_landed_state();
}
void BlockSegwayController::update() {
// wait for a sensor update, check for exit condition every 100 ms
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// check for sane values of dt
// to prevent large control responses
if (dt > 1.0f || dt < 0) return;
// set dt for all child blocks
setDt(dt);
// check for new updates
if (_param_update.updated()) updateParams();
// get new information from subscriptions
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
if (_status.main_state == MAIN_STATE_AUTO) {
// update guidance
}
// compute speed command
float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
// handle autopilot modes
if (_status.main_state == MAIN_STATE_AUTO ||
_status.main_state == MAIN_STATE_ALTCTL ||
_status.main_state == MAIN_STATE_POSCTL) {
_actuators.control[0] = spdCmd;
_actuators.control[1] = spdCmd;
} else if (_status.main_state == MAIN_STATE_MANUAL) {
if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {
_actuators.control[CH_LEFT] = _manual.throttle;
_actuators.control[CH_RIGHT] = _manual.pitch;
} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {
_actuators.control[0] = spdCmd;
_actuators.control[1] = spdCmd;
}
}
// update all publications
updatePublications();
}
// Constructor
ContactSet::ContactSet(ResourceCached* resource,
UtlString& uri) :
mResource(resource),
mUri(uri)
{
OsSysLog::add(FAC_RLS, PRI_DEBUG,
"ContactSet:: this = %p, resource = %p, mUri = '%s'",
this, mResource, mUri.data());
// Set up the subscriptions.
// Until we have any information from our SUBSCRIBE for "reg" events,
// there will be one subscription to mUri.
updateSubscriptions();
// Start the subscription.
UtlBoolean ret;
UtlString mUriNameAddr = "<" + mUri + ">";
OsSysLog::add(FAC_RLS, PRI_DEBUG,
"SubscriptionSet:: mUri = '%s', mUriNameAddr = '%s'",
mUri.data(), mUriNameAddr.data());
ret = getResourceListServer()->getSubscribeClient().
addSubscription(mUri.data(),
REG_EVENT_TYPE,
REG_EVENT_CONTENT_TYPE,
getResourceListServer()->getClientFromURI(),
mUriNameAddr.data(),
getResourceListServer()->getClientContactURI(),
getResourceListServer()->getResubscribeInterval(),
getResourceListSet(),
ResourceListSet::subscriptionEventCallbackAsync,
ResourceListSet::notifyEventCallbackAsync,
mSubscriptionEarlyDialogHandle);
if (ret)
{
OsSysLog::add(FAC_RLS, PRI_DEBUG,
"ContactSet:: addSubscription succeeded mUri = '%s', mSubscriptionEarlyDialogHandle = '%s'",
mUri.data(),
mSubscriptionEarlyDialogHandle.data());
// Add this ContactSet to mSubscribeMap.
getResourceListSet()->addSubscribeMapping(&mSubscriptionEarlyDialogHandle,
this);
}
else
{
OsSysLog::add(FAC_RLS, PRI_WARNING,
"ContactSet:: addSubscription failed mUri = '%s', mSubscriptionEarlyDialogHandle = '%s'",
mUri.data(),
mSubscriptionEarlyDialogHandle.data());
}
}
bool MulticopterLandDetector::update()
{
// first poll for new data from our subscriptions
updateSubscriptions();
// only trigger flight conditions if we are armed
if (!_arming.armed) {
return true;
}
// return status based on armed state if no position lock is available
if (_vehicleGlobalPosition.timestamp == 0 ||
hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) {
// no position lock - not landed if armed
return !_arming.armed;
}
const uint64_t now = hrt_absolute_time();
// check if we are moving vertically
bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate;
// check if we are moving horizontally
bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity
&& _vehicleStatus.condition_global_position_valid;
// next look if all rotation angles are not moving
bool rotating = fabsf(_vehicleAttitude.rollspeed) > _params.maxRotation ||
fabsf(_vehicleAttitude.pitchspeed) > _params.maxRotation ||
fabsf(_vehicleAttitude.yawspeed) > _params.maxRotation;
// check if thrust output is minimal (about half of default)
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
// sensed movement, so reset the land detector
_landTimer = now;
return false;
}
return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME;
}
LandDetectionResult MulticopterLandDetector::update()
{
// first poll for new data from our subscriptions
updateSubscriptions();
updateParameterCache(false);
if (get_freefall_state()) {
_state = LANDDETECTION_RES_FREEFALL;
} else if (get_landed_state()) {
_state = LANDDETECTION_RES_LANDED;
} else {
_state = LANDDETECTION_RES_FLYING;
}
return _state;
}
bool FixedwingLandDetector::update()
{
// First poll for new data from our subscriptions
updateSubscriptions();
const uint64_t now = hrt_absolute_time();
bool landDetected = false;
if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx *
_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
if (PX4_ISFINITE(val)) {
_velocity_xy_filtered = val;
}
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz);
if (PX4_ISFINITE(val)) {
_velocity_z_filtered = val;
}
}
if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
}
// crude land detector for fixedwing
if (_velocity_xy_filtered < _params.maxVelocity
&& _velocity_z_filtered < _params.maxClimbRate
&& _airspeed_filtered < _params.maxAirSpeed) {
// these conditions need to be stable for a period of time before we trust them
if (now > _landDetectTrigger) {
landDetected = true;
}
} else {
// reset land detect trigger
_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME;
}
return landDetected;
}
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;
}
}
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 BlockMultiModeBacksideAutopilot::update()
{
// wait for a sensor update, check for exit condition every 100 ms
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// check for sane values of dt
// to prevent large control responses
if (dt > 1.0f || dt < 0) return;
// set dt for all child blocks
setDt(dt);
// store old position command before update if new command sent
if (_posCmd.updated()) {
_lastPosCmd = _posCmd.getData();
}
// check for new updates
if (_param_update.updated()) updateParams();
// get new information from subscriptions
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
if (_status.state_machine == SYSTEM_STATE_AUTO) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
}
// XXX handle STABILIZED (loiter on spot) as well
// once the system switches from manual or auto to stabilized
// the setpoint should update to loitering around this position
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_AUTO ||
_status.state_machine == SYSTEM_STATE_STABILIZED) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// altitude hold
float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
// heading hold
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
float pCmd = _phi2P.update(phiCmd - _att.roll);
// velocity hold
// negative sign because nose over to increase speed
float thetaCmd = _theLimit.update(-_v2Theta.update(
_vLimit.update(_vCmd.get()) - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd
float rCmd = 0;
// stabilization
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
_actuators.control[CH_THR] = dThrottle + _trimThr.get();
// XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once
// a first binary release can be targeted.
// This is not a hack, but a design choice.
/* do not limit in HIL */
if (!_status.flag_hil_enabled) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
// from +1 -> -1 m/s for rate of climb
//float dThrottle = _roc2Thr.update(
//_rocMax.get()*_manual.pitch - _pos.vz);
// roll channel -> bank angle
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
float pCmd = _phi2P.update(phiCmd - _att.roll);
// throttle channel -> velocity
// negative sign because nose over to increase speed
float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd
float rCmd = 0;
// stabilization
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
// currenlty using manual throttle
// XXX if you enable this watch out, vz might be very noisy
//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
_actuators.control[CH_THR] = _manual.throttle;
// XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once
// a first binary release can be targeted.
// This is not a hack, but a design choice.
/* do not limit in HIL */
if (!_status.flag_hil_enabled) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
}
// body rates controller, disabled for now
else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
}
}
// update all publications
updatePublications();
}
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 BlockMultiModeBacksideAutopilot::update()
{
// wait for a sensor update, check for exit condition every 100 ms
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// check for sane values of dt
// to prevent large control responses
if (dt > 1.0f || dt < 0) return;
// set dt for all child blocks
setDt(dt);
// store old position command before update if new command sent
if (_posCmd.updated()) {
_lastPosCmd = _posCmd.getData();
}
// check for new updates
if (_param_update.updated()) updateParams();
// get new information from subscriptions
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_STABILIZED) {
_stabilization.update(
_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.state_machine == SYSTEM_STATE_AUTO) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// commands
float rCmd = 0;
_backsideAutopilot.update(
_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
_pos.alt, v,
_att.roll, _att.pitch, _att.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed
);
_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
}
// update all publications
updatePublications();
}
void AppearanceGroup::subscriptionEventCallback(
const UtlString* earlyDialogHandle,
const UtlString* dialogHandle,
SipSubscribeClient::SubscriptionState newState,
const UtlString* subscriptionState)
{
Os::Logger::instance().log(FAC_SAA, PRI_DEBUG,
"AppearanceGroup::subscriptionEventCallback mUri = '%s', newState = %d, earlyDialogHandle = '%s', dialogHandle = '%s', subscriptionState = '%s'",
mSharedUser.data(),
newState, mSubscriptionEarlyDialogHandle.data(),
dialogHandle->data(), subscriptionState->data());
switch (newState)
{
case SipSubscribeClient::SUBSCRIPTION_INITIATED:
break;
case SipSubscribeClient::SUBSCRIPTION_SETUP:
{
// Check that we don't have too many subscriptions.
if (mSubscriptions.entries() <
getAppearanceAgent()->getMaxRegSubscInGroup())
{
// Add this AppearanceGroup to mNotifyMap for the subscription.
getAppearanceAgent()->getAppearanceGroupSet().addNotifyMapping(dialogHandle, this);
// Remember it in mSubscriptions, if there isn't already an entry.
if (!mSubscriptions.find(dialogHandle))
{
mSubscriptions.insertKeyAndValue(new UtlString(*dialogHandle), new UtlHashMap);
Os::Logger::instance().log(FAC_SAA, PRI_INFO,
"AppearanceGroup::subscriptionEventCallback "
"subscription setup for AppearanceGroup = '%s', dialogHandle = '%s'",
mSharedUser.data(), dialogHandle->data());
}
else
{
Os::Logger::instance().log(FAC_SAA, PRI_DEBUG,
"AppearanceGroup::subscriptionEventCallback "
"mSubscriptions element already exists for this dialog handle mUri = '%s', dialogHandle = '%s'",
mSharedUser.data(),
dialogHandle->data());
}
}
else
{
Os::Logger::instance().log(FAC_SAA, PRI_ERR,
"AppearanceGroup::subscriptionEventCallback "
"cannot add reg subscription with dialog handle '%s', already %zu in AppearanceGroup '%s'",
dialogHandle->data(), mSubscriptions.entries(),
mSharedUser.data());
}
}
break;
case SipSubscribeClient::SUBSCRIPTION_TERMINATED:
{
// Remove this dialogHandle from mNotifyMap for the subscription.
// A new one will be added later if necessary.
mAppearanceGroupSet->deleteNotifyMapping(dialogHandle);
// Delete this subscription from mSubscriptions.
mSubscriptions.destroy(dialogHandle);
// Delete this AppearanceGroup from mSubscribeMap (for the "reg" subscription).
mAppearanceGroupSet->deleteSubscribeMapping(&mSubscriptionEarlyDialogHandle);
// End the terminated "reg" subscription.
UtlBoolean ret;
ret = getAppearanceAgent()->getSubscribeClient().
endSubscriptionGroup(mSubscriptionEarlyDialogHandle);
Os::Logger::instance().log(FAC_SAA,
ret ? PRI_INFO : PRI_WARNING,
"AppearanceGroup::subscriptionEventCallback "
"endSubscriptionGroup %s mSharedUser = '******', mSubscriptionEarlyDialogHandle = '%s'",
ret ? "succeeded" : "failed",
mSharedUser.data(),
mSubscriptionEarlyDialogHandle.data());
OsTask::delay(getAppearanceAgent()->getChangeDelay());
// Check to see if there is supposed to be a group with this name.
if (mAppearanceGroupSet->findAppearanceGroup(mSharedUser))
{
Os::Logger::instance().log(FAC_SAA, PRI_WARNING,
"AppearanceGroup::subscriptionEventCallback "
"subscription terminated for AppearanceGroup '%s', but should exist. Retrying subscription",
mSharedUser.data());
startSubscription();
}
else
{
Os::Logger::instance().log(FAC_SAA, PRI_INFO,
"AppearanceGroup::subscriptionEventCallback "
"subscription terminated for AppearanceGroup = '%s', dialogHandle = '%s'",
mSharedUser.data(), dialogHandle->data());
}
// Update the subscriptions.
updateSubscriptions();
}
break;
}
}
// Process a notify event callback.
// This involves parsing the content of the callback and revising our record
// of the state for that subscription. Then, we must regenerate the list
// of contacts and update the set of subscriptions to match the current
// contacts.
void ContactSet::notifyEventCallback(const UtlString* dialogHandle,
const UtlString* content)
{
OsSysLog::add(FAC_RLS, PRI_DEBUG,
"ContactSet::notifyEventCallback mUri = '%s', dialogHandle = '%s', content = '%s'",
mUri.data(), dialogHandle->data(), content->data());
// Parse the XML and update the contact status.
// Find the UtlHashMap for this subscription.
UtlHashMap* state_from_this_subscr =
dynamic_cast <UtlHashMap*> (mSubscriptions.findValue(dialogHandle));
if (!state_from_this_subscr)
{
// No state for this dialogHandle, so we need to add one.
OsSysLog::add(FAC_RLS, PRI_WARNING,
"ContactSet::notifyEventCallback mSubscriptions element does not exist for this dialog handle mUri = '%s', dialogHandle = '%s'",
mUri.data(),
dialogHandle->data());
// Check that we don't have too many subscriptions.
if (mSubscriptions.entries() <
getResourceListServer()->getMaxRegSubscInResource())
{
state_from_this_subscr = new UtlHashMap;
mSubscriptions.insertKeyAndValue(new UtlString(*dialogHandle),
state_from_this_subscr);
}
else
{
OsSysLog::add(FAC_RLS, PRI_ERR,
"ContactSet::notifyEventCallback cannot add reg subscription with dialog handle '%s', already %zu in ContactSet '%s'",
dialogHandle->data(), mSubscriptions.entries(),
mUri.data());
}
}
// Perform the remainder of the processing if we obtained a hash map
// from the above processing.
if (state_from_this_subscr)
{
// Initialize Tiny XML document object.
TiXmlDocument document;
TiXmlNode* reginfo_node;
if (
// Load the XML into it.
document.Parse(content->data()) &&
// Find the top element, which should be a <reginfo>.
(reginfo_node = document.FirstChild("reginfo")) != NULL &&
reginfo_node->Type() == TiXmlNode::ELEMENT)
{
// Check the state attribute.
const char* p = reginfo_node->ToElement()->Attribute("state");
if (p && strcmp(p, "full") == 0)
{
// If the state is "full", delete the current state.
state_from_this_subscr->destroyAll();
OsSysLog::add(FAC_RLS, PRI_DEBUG,
"ContactSet::notifyEventCallback clearing state");
}
// Find all the <registration> elements for this URI.
for (TiXmlNode* registration_node = 0;
(registration_node =
reginfo_node->IterateChildren("registration",
registration_node));
)
{
// Do not test the aor attribute of <registration> elements
// because the reg event server may be operating with the real
// AOR for this URI, whereas we may have been told of an alias.
// Find all the <contact> elements.
for (TiXmlNode* contact_node = 0;
(contact_node =
registration_node->IterateChildren("contact",
contact_node));
)
{
TiXmlElement* contact_element = contact_node->ToElement();
// Get the state attribute.
const char* state = contact_element->Attribute("state");
// Get the id attribute
const char* id = contact_element->Attribute("id");
// Get the Contact URI for the phone. If a GRUU address is present we should
// use that as the Contact URI. Otherwise, use the contact URI present in the
// "uri" element, and append any path headers that are present to the ROUTE
// header parameter in the URI. This will ensure proper routing in HA systems.
// Please refer to XECS-1694 for more details.
UtlString* uri_allocated = new UtlString;
UtlBoolean check_uri = TRUE;
TiXmlNode* pub_gruu_node = contact_element->FirstChild("gr:pub-gruu");
if (pub_gruu_node)
{
TiXmlElement* pub_gruu_element = pub_gruu_node->ToElement();
UtlString pub_gruu_uri(pub_gruu_element->Attribute("uri"));
if (!pub_gruu_uri.isNull())
{
// Check the URI Scheme. Only accept the GRUU address if it is of either
// a sip or sips scheme
Url tmp(pub_gruu_uri, TRUE);
Url::Scheme uriScheme = tmp.getScheme();
if(Url::SipUrlScheme == uriScheme ||
Url::SipsUrlScheme == uriScheme)
{
tmp.removeAngleBrackets();
tmp.getUri(*uri_allocated);
check_uri = FALSE;
}
}
}
// If we did not find a GRUU address, then use the address in the "uri" element as the
// contact URI, and check for path headers.
if (check_uri)
{
TiXmlNode* u = contact_element->FirstChild("uri");
if (u)
{
textContentShallow(*uri_allocated, u);
// Iterate through all the path header elements. Path headers are stored in the
// "unknown-param" elements that have a "name" attribute value of "path".
for (TiXmlNode* unknown_param_node = 0;
(unknown_param_node = contact_node->IterateChildren("unknown-param",
unknown_param_node));
)
{
TiXmlElement* unknown_param_element = unknown_param_node->ToElement();
UtlString path(unknown_param_element->Attribute("name"));
if(0 == path.compareTo("path"))
{
UtlString pathVector;
textContentShallow(pathVector, unknown_param_node);
if(!pathVector.isNull())
{
Url contact_uri(*uri_allocated, TRUE);
// there is already a Route header parameter in the contact; append it to the
// Route derived from the Path vector.
UtlString existingRouteValue;
if ( contact_uri.getHeaderParameter(SIP_ROUTE_FIELD, existingRouteValue))
{
pathVector.append(SIP_MULTIFIELD_SEPARATOR);
pathVector.append(existingRouteValue);
}
contact_uri.setHeaderParameter(SIP_ROUTE_FIELD, pathVector);
contact_uri.removeAngleBrackets();
contact_uri.getUri(*uri_allocated);
}
}
}
}
}
// Only process <contact> elements that have the needed values.
if (state && state[0] != '\0' &&
id && id[0] != '\0' &&
!uri_allocated->isNull())
{
UtlString* id_allocated = new UtlString(id);
if (strcmp(state, "active") == 0)
{
// Add the contact if it is not already present.
if (!state_from_this_subscr->find(id_allocated))
{
// Prepend the registration Call-Id and ';' to *uri_allocated..
uri_allocated->insert(0, ';');
const char* call_id = contact_element->Attribute("callid");
if (call_id)
{
uri_allocated->insert(0, call_id);
}
// Check that we don't have too many contacts.
if (state_from_this_subscr->entries() <
getResourceListServer()->getMaxContInRegSubsc())
{
// Insert the registration record.
if (state_from_this_subscr->insertKeyAndValue(id_allocated,
uri_allocated))
{
OsSysLog::add(FAC_RLS, PRI_DEBUG,
"ContactSet::notifyEventCallback adding id = '%s' Call-Id;URI = '%s'",
id, uri_allocated->data());
id_allocated = NULL;
uri_allocated = NULL;
}
else
{
OsSysLog::add(FAC_RLS, PRI_ERR,
"ContactSet::notifyEventCallback adding id = '%s' Call-Id;URI = '%s' failed",
id_allocated->data(), uri_allocated->data());
OsSysLog::add(FAC_RLS, PRI_DEBUG,
"ContactSet::notifyEventCallback *state_from_this_subscr is:");
UtlHashMapIterator itor(*state_from_this_subscr);
UtlContainable* k;
while ((k = itor()))
{
UtlContainable* v = itor.value();
OsSysLog::add(FAC_RLS, PRI_DEBUG,
"ContactSet::notifyEventCallback (*state_from_this_subscr)['%s'] = '%s'",
(dynamic_cast <UtlString*> (k))->data(),
(dynamic_cast <UtlString*> (v))->data());
}
}
}
else
{
OsSysLog::add(FAC_RLS, PRI_ERR,
"ContactSet::notifyEventCallback cannot add Call-Id;RUI '%s', already %zu in ContactSet '%s' subscription '%s'",
uri_allocated->data(),
state_from_this_subscr->entries(),
mUri.data(), dialogHandle->data());
}
}
}
else if (strcmp(state, "terminated") == 0)
{
// Delete it from the contact state.
state_from_this_subscr->destroy(id_allocated);
OsSysLog::add(FAC_RLS, PRI_DEBUG,
"ContactSet::notifyEventCallback deleting id = '%s'",
id);
}
// Free id_allocated, if it is not pointed to by a data
// structure, which is indicated by setting it to NULL.
if (id_allocated)
{
delete id_allocated;
}
}
else
{
OsSysLog::add(FAC_RLS, PRI_ERR,
"ContactSet::notifyEventCallback <contact> element with id = '%s' is missing id, state, and/or URI",
id ? id : "(missing)");
}
// Free uri_allocated, if it is not pointed to by a data
// structure, which is indicated by setting it to NULL.
if (uri_allocated)
{
delete uri_allocated;
}
}
}
}
else
{
// Error parsing the contents.
OsSysLog::add(FAC_RLS, PRI_ERR,
"ContactSet::notifyEventCallback malformed reg event content for mUri = '%s'",
mUri.data());
}
// Update the subscriptions we maintain to agree with the new state.
updateSubscriptions();
}
}
void ContactSet::subscriptionEventCallback(
const UtlString* earlyDialogHandle,
const UtlString* dialogHandle,
SipSubscribeClient::SubscriptionState newState,
const UtlString* subscriptionState)
{
OsSysLog::add(FAC_RLS, PRI_DEBUG,
"ContactSet::subscriptionEventCallback mUri = '%s', newState = %d, earlyDialogHandle = '%s', dialogHandle = '%s', subscriptionState = '%s'",
mUri.data(),
newState, mSubscriptionEarlyDialogHandle.data(),
dialogHandle->data(), subscriptionState->data());
switch (newState)
{
case SipSubscribeClient::SUBSCRIPTION_INITIATED:
break;
case SipSubscribeClient::SUBSCRIPTION_SETUP:
{
// Remember it in mSubscriptions, if there isn't already an entry.
if (!mSubscriptions.find(dialogHandle))
{
// Check that we don't have too many subscriptions.
if (mSubscriptions.entries() <
getResourceListServer()->getMaxRegSubscInResource())
{
// Add this ContactSet to mNotifyMap for the subscription.
// (::addNotifyMapping() copies *dialogHandle.)
getResourceListSet()->addNotifyMapping(*dialogHandle, this);
// Remember to make a copy of *dialogHandle.
mSubscriptions.insertKeyAndValue(new UtlString(*dialogHandle),
new UtlHashMap);
}
else
{
OsSysLog::add(FAC_RLS, PRI_ERR,
"ContactSet::subscriptionEventCallback cannot add reg subscription with dialog handle '%s', already %zu in ContactSet '%s'",
dialogHandle->data(), mSubscriptions.entries(),
mUri.data());
}
}
else
{
OsSysLog::add(FAC_RLS, PRI_DEBUG,
"ContactSet::subscriptionEventCallback mSubscriptions element already exists for this dialog handle mUri = '%s', dialogHandle = '%s'",
mUri.data(),
dialogHandle->data());
}
}
break;
case SipSubscribeClient::SUBSCRIPTION_TERMINATED:
{
// Remove this ContactSet from mNotifyMap for the subscription.
getResourceListSet()->deleteNotifyMapping(dialogHandle);
// Delete this subscription from mSubscriptions.
mSubscriptions.destroy(dialogHandle);
// Update the subscriptions.
updateSubscriptions();
}
break;
}
}
