dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Made LPE var pub threshold a parameter. (#4959)

This commit is contained in:
James Goppert 2016-07-01 13:27:29 -04:00 committed by GitHub
parent a0fdfb0c21
commit cb3120764a
3 changed files with 32 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -14,6 +14,7 @@ static const uint32_t EST_SRC_TIMEOUT = 10000; // 0.01 s
static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m
static const uint32_t EST_STDDEV_Z_VALID = 2.0; // 2.0 m
static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
static const bool integrate = true; // use accel for integrating
// minimum flow altitude
static const float flow_min_agl = 0.3;
@ -55,7 +56,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_map_ref(),
// block parameters
_integrate(this, "INTEGRATE"),
_xy_pub_thresh(this, "XY_PUB"),
_z_pub_thresh(this, "Z_PUB"),
_sonar_z_stddev(this, "SNR_Z"),
_sonar_z_offset(this, "SNR_OFF_Z"),
_lidar_z_stddev(this, "LDR_Z"),
@ -303,7 +305,7 @@ void BlockLocalPositionEstimator::update()
}
// is xy valid?
bool xy_stddev_ok = sqrt(math::max(_P(X_x, X_x), _P(X_y, X_y))) < EST_STDDEV_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
@ -318,7 +320,7 @@ void BlockLocalPositionEstimator::update()
}
// is z valid?
bool z_stddev_ok = sqrt(_P(X_z, X_z)) < EST_STDDEV_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
@ -333,7 +335,7 @@ void BlockLocalPositionEstimator::update()
}
// is terrain valid?
bool tz_stddev_ok = sqrt(_P(X_tz, X_tz)) < EST_STDDEV_TZ_VALID;
bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
if (_validTZ) {
if (!tz_stddev_ok) {
@ -555,7 +557,7 @@ float BlockLocalPositionEstimator::agl()
void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
{
// don't correct bias when rotating rapidly
float ang_speed = sqrt(
float ang_speed = sqrtf(
_sub_att.get().rollspeed * _sub_att.get().rollspeed +
_sub_att.get().pitchspeed * _sub_att.get().pitchspeed +
_sub_att.get().yawspeed * _sub_att.get().yawspeed);
@ -838,7 +840,7 @@ void BlockLocalPositionEstimator::predict()
// state or covariance
if (!_validXY && !_validZ) { return; }
if (_integrate.get() && _sub_att.get().R_valid) {
if (integrate && _sub_att.get().R_valid) {
Matrix3f R_att(_sub_att.get().R);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
_u = R_att * a;

3
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@ -243,7 +243,8 @@ private:
struct map_projection_reference_s _map_ref;
// general parameters
BlockParamInt _integrate;
BlockParamFloat _xy_pub_thresh;
BlockParamFloat _z_pub_thresh;
// sonar parameters
BlockParamFloat _sonar_z_stddev;

31
src/modules/local_position_estimator/params.c
View File

@ -3,14 +3,6 @@
// 16 is max name length
/**
* Accelerometer integration for prediction.
*
* @boolean
* @group Local Position Estimator
*/
PARAM_DEFINE_INT32(LPE_INTEGRATE, 1);
/**
* Optical flow z offset from center
*
@ -346,5 +338,26 @@ PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
* @max 1000
* @decimal 0
*/
PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0);
PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0f);
/**
* Required xy standard deviation to publish position
*
* @group Local Position Estimator
* @unit m
* @min 0.3
* @max 5.0
* @decimal 1
*/
PARAM_DEFINE_FLOAT(LPE_XY_PUB, 1.0f);
/**
* Required z standard deviation to publish altitude/ terrain
*
* @group Local Position Estimator
* @unit m
* @min 0.3
* @max 5.0
* @decimal 1
*/
PARAM_DEFINE_FLOAT(LPE_Z_PUB, 1.0f);