void BlockLocalPositionEstimator::publishGlobalPos()
{
// publish global position
double lat = 0;
double lon = 0;
const Vector<float, n_x> &xLP = _xLowPass.getState();
map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
float alt = -xLP(X_z) + _altHome;
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
PX4_ISFINITE(xLP(X_vz))) {
_pub_gpos.get().timestamp = _timeStamp;
_pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec;
_pub_gpos.get().lat = lat;
_pub_gpos.get().lon = lon;
_pub_gpos.get().alt = alt;
_pub_gpos.get().vel_n = xLP(X_vx);
_pub_gpos.get().vel_e = xLP(X_vy);
_pub_gpos.get().vel_d = xLP(X_vz);
_pub_gpos.get().yaw = _sub_att.get().yaw;
_pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
_pub_gpos.get().epv = sqrtf(_P(X_z, X_z));
_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
_pub_gpos.get().terrain_alt_valid = _validTZ;
_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0];
_pub_gpos.update();
}
}
void BlockLocalPositionEstimator::publishGlobalPos()
{
// publish global position
double lat = 0;
double lon = 0;
const Vector<float, n_x> &xLP = _xLowPass.getState();
map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
float alt = -xLP(X_z) + _altOrigin;
// lie about eph/epv to allow visual odometry only navigation when velocity est. good
float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
float epv = sqrtf(_P(X_z, X_z));
float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
if (vxy_stddev < _vxy_pub_thresh.get()) {
if (eph > eph_thresh) {
eph = eph_thresh;
}
if (epv > epv_thresh) {
epv = epv_thresh;
}
}
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
PX4_ISFINITE(xLP(X_vz))) {
_pub_gpos.get().timestamp = _timeStamp;
_pub_gpos.get().lat = lat;
_pub_gpos.get().lon = lon;
_pub_gpos.get().alt = alt;
_pub_gpos.get().vel_n = xLP(X_vx);
_pub_gpos.get().vel_e = xLP(X_vy);
_pub_gpos.get().vel_d = xLP(X_vz);
_pub_gpos.get().yaw = _eul(2);
_pub_gpos.get().eph = eph;
_pub_gpos.get().epv = epv;
_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;
_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
_pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ;
_pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY);
_pub_gpos.update();
}
}
void BlockLocalPositionEstimator::publishLocalPos()
{
const Vector<float, n_x> &xLP = _xLowPass.getState();
// publish local position
if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& PX4_ISFINITE(_x(X_vz))) {
_pub_lpos.get().timestamp = _timeStamp;
_pub_lpos.get().xy_valid = _validXY;
_pub_lpos.get().z_valid = _validZ;
_pub_lpos.get().v_xy_valid = _validXY;
_pub_lpos.get().v_z_valid = _validZ;
_pub_lpos.get().x = xLP(X_x); // north
_pub_lpos.get().y = xLP(X_y); // east
_pub_lpos.get().z = xLP(X_z); // down
_pub_lpos.get().vx = xLP(X_vx); // north
_pub_lpos.get().vy = xLP(X_vy); // east
_pub_lpos.get().vz = xLP(X_vz); // down
_pub_lpos.get().yaw = _sub_att.get().yaw;
_pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference
_pub_lpos.get().z_global = _baroInitialized;
_pub_lpos.get().ref_timestamp = _sub_home.get().timestamp;
_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
_pub_lpos.get().ref_alt = _sub_home.get().alt;
_pub_lpos.get().dist_bottom = agl();
_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
_pub_lpos.get().dist_bottom_valid = _validTZ && _validZ;
_pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
_pub_lpos.get().epv = sqrtf(_P(X_z, X_z));
_pub_lpos.update();
}
}
void BlockLocalPositionEstimator::publishLocalPos()
{
const Vector<float, n_x> &xLP = _xLowPass.getState();
// lie about eph/epv to allow visual odometry only navigation when velocity est. good
float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
float epv = sqrtf(_P(X_z, X_z));
float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
if (vxy_stddev < _vxy_pub_thresh.get()) {
if (eph > eph_thresh) {
eph = eph_thresh;
}
if (epv > epv_thresh) {
epv = epv_thresh;
}
}
// publish local position
if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& PX4_ISFINITE(_x(X_vz))) {
_pub_lpos.get().timestamp = _timeStamp;
_pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY;
_pub_lpos.get().z_valid = _estimatorInitialized & EST_Z;
_pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY;
_pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z;
_pub_lpos.get().x = xLP(X_x); // north
_pub_lpos.get().y = xLP(X_y); // east
if (_fusion.get() & FUSE_PUB_AGL_Z) {
_pub_lpos.get().z = -_aglLowPass.getState(); // agl
} else {
_pub_lpos.get().z = xLP(X_z); // down
}
_pub_lpos.get().vx = xLP(X_vx); // north
_pub_lpos.get().vy = xLP(X_vy); // east
_pub_lpos.get().vz = xLP(X_vz); // down
// this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity
_pub_lpos.get().z_deriv = xLP(X_vz);
_pub_lpos.get().yaw = _eul(2);
_pub_lpos.get().xy_global = _estimatorInitialized & EST_XY;
_pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO) && _altOriginGlobal;
_pub_lpos.get().ref_timestamp = _time_origin;
_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
_pub_lpos.get().ref_alt = _altOrigin;
_pub_lpos.get().dist_bottom = _aglLowPass.getState();
_pub_lpos.get().dist_bottom_rate = -xLP(X_vz);
// we estimate agl even when we don't have terrain info
// if you are in terrain following mode this is important
// so that if terrain estimation fails there isn't a
// sudden altitude jump
_pub_lpos.get().dist_bottom_valid = _estimatorInitialized & EST_Z;
_pub_lpos.get().eph = eph;
_pub_lpos.get().epv = epv;
_pub_lpos.update();
//TODO provide calculated values for these
_pub_lpos.get().evh = 0.0f;
_pub_lpos.get().evv = 0.0f;
_pub_lpos.get().vxy_max = 0.0f;
_pub_lpos.get().limit_hagl = false;
}
}
float BlockLocalPositionEstimator::agl()
{
const Vector<float, n_x> &xLP = _xLowPass.getState();
return xLP(X_tz) - xLP(X_z);
}
void BlockLocalPositionEstimator::publishLocalPos()
{
const Vector<float, n_x> &xLP = _xLowPass.getState();
// lie about eph/epv to allow visual odometry only navigation when velocity est. good
float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
float epv = sqrtf(_P(X_z, X_z));
float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
if (vxy_stddev < _vxy_pub_thresh.get()) {
if (eph > eph_thresh) {
eph = eph_thresh;
}
if (epv > epv_thresh) {
epv = epv_thresh;
}
}
// publish local position
if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& PX4_ISFINITE(_x(X_vz))) {
_pub_lpos.get().timestamp = _timeStamp;
_pub_lpos.get().xy_valid = _validXY;
_pub_lpos.get().z_valid = _validZ;
_pub_lpos.get().v_xy_valid = _validXY;
_pub_lpos.get().v_z_valid = _validZ;
_pub_lpos.get().x = xLP(X_x); // north
_pub_lpos.get().y = xLP(X_y); // east
if (_pub_agl_z.get()) {
_pub_lpos.get().z = -_aglLowPass.getState(); // agl
} else {
_pub_lpos.get().z = xLP(X_z); // down
}
_pub_lpos.get().vx = xLP(X_vx); // north
_pub_lpos.get().vy = xLP(X_vy); // east
_pub_lpos.get().vz = xLP(X_vz); // down
_pub_lpos.get().yaw = _eul(2);
_pub_lpos.get().xy_global = _validXY;
_pub_lpos.get().z_global = _baroInitialized;
_pub_lpos.get().ref_timestamp = _timeStamp;
_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
_pub_lpos.get().ref_alt = _altOrigin;
_pub_lpos.get().dist_bottom = _aglLowPass.getState();
_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
// we estimate agl even when we don't have terrain info
// if you are in terrain following mode this is important
// so that if terrain estimation fails there isn't a
// sudden altitude jump
_pub_lpos.get().dist_bottom_valid = _validZ;
_pub_lpos.get().eph = eph;
_pub_lpos.get().epv = epv;
_pub_lpos.update();
}
}
