From 2a26611cf5be85d8475ac08e387530b847e93fbf Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 15 Jun 2016 12:37:52 -0400 Subject: [PATCH] Make LPE sonar reading more robust. (#4806) --- .../BlockLocalPositionEstimator.cpp | 1 + .../BlockLocalPositionEstimator.hpp | 1 + src/modules/local_position_estimator/params.c | 10 +++--- .../local_position_estimator/sensors/flow.cpp | 15 ++------ .../sensors/sonar.cpp | 34 +++++++++++++------ 5 files changed, 34 insertions(+), 27 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 6bbcb284f6..12360a18a2 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -113,6 +113,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _time_last_gps(0), _time_last_lidar(0), _time_last_sonar(0), + _time_init_sonar(0), _time_last_vision_p(0), _time_last_mocap(0), diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index c32db40066..eba9d61355 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -318,6 +318,7 @@ private: uint64_t _time_last_gps; uint64_t _time_last_lidar; uint64_t _time_last_sonar; + uint64_t _time_init_sonar; uint64_t _time_last_vision_p; uint64_t _time_last_mocap; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 071aabc9fe..4b85db9447 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -126,7 +126,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f); * @max 3 * @decimal 2 */ -PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f); +PARAM_DEFINE_FLOAT(LPE_BAR_Z, 3.0f); /** * Enables GPS data, also forces alt init with GPS @@ -264,7 +264,7 @@ PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f); * @max 1 * @decimal 8 */ -PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f); +PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f); /** * Velocity propagation noise density @@ -275,7 +275,7 @@ PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f); * @max 1 * @decimal 8 */ -PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f); +PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f); /** * Accel bias propagation noise density @@ -289,7 +289,7 @@ PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f); PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f); /** - * Terrain random walk noise density + * Terrain random walk noise density, hilly/outdoor (1e-1), flat/Indoor (1e-3) * * @group Local Position Estimator * @unit m/s/sqrt(Hz) @@ -297,7 +297,7 @@ PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f); * @max 1 * @decimal 3 */ -PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-3f); +PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-1f); /** * Flow gyro high pass filter cut off frequency diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index 331123ab2f..31f29a1487 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -49,21 +49,12 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &y) } // calculate range to center of image for flow - float d = 0; - - if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) { - d = _sub_lidar->get().current_distance - + (_lidar_z_offset.get() - _flow_z_offset.get()); - - } else if (_sonarInitialized && (_sonarFault < fault_lvl_disable)) { - d = _sub_sonar->get().current_distance - + (_sonar_z_offset.get() - _flow_z_offset.get()); - - } else { - // no valid distance data + if (!_canEstimateT) { return -1; } + float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch); + // check for global accuracy if (_gpsInitialized) { double lat = _sub_gps.get().lat * 1.0e-7; diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp index 5e6349b671..d95ad5e346 100644 --- a/src/modules/local_position_estimator/sensors/sonar.cpp +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -7,33 +7,47 @@ extern orb_advert_t mavlink_log_pub; // required number of samples for sensor // to initialize static const int REQ_SONAR_INIT_COUNT = 10; -static const uint32_t SONAR_TIMEOUT = 1000000; // 1.0 s +static const uint32_t SONAR_TIMEOUT = 5000000; // 2.0 s +static const float SONAR_MAX_INIT_STD = 0.3f; // meters void BlockLocalPositionEstimator::sonarInit() { // measure Vector y; + if (_sonarStats.getCount() == 0) { + _time_init_sonar = _timeStamp; + } + if (sonarMeasure(y) != OK) { - _sonarStats.reset(); return; } // if finished if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init " - "mean %d cm std %d cm", - int(100 * _sonarStats.getMean()(0)), - int(100 * _sonarStats.getStdDev()(0))); - _sonarInitialized = true; - _sonarFault = FAULT_NONE; + if (_sonarStats.getStdDev()(0) > SONAR_MAX_INIT_STD) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init std > min"); + _sonarStats.reset(); + + } else if ((_timeStamp - _time_init_sonar) > SONAR_TIMEOUT) { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init timeout "); + _sonarStats.reset(); + + } else { + mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init " + "mean %d cm std %d cm", + int(100 * _sonarStats.getMean()(0)), + int(100 * _sonarStats.getStdDev()(0))); + _sonarInitialized = true; + _sonarFault = FAULT_NONE; + } } } int BlockLocalPositionEstimator::sonarMeasure(Vector &y) { // measure - float d = _sub_sonar->get().current_distance + _sonar_z_offset.get(); + float d = _sub_sonar->get().current_distance; float eps = 0.01f; float min_dist = _sub_sonar->get().min_distance + eps; float max_dist = _sub_sonar->get().max_distance - eps; @@ -47,7 +61,7 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector &y) _sonarStats.update(Scalarf(d)); _time_last_sonar = _timeStamp; y.setZero(); - y(0) = d * + y(0) = (d + _sonar_z_offset.get()) * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch); return OK;