From 08f5ece306650814a72cf5302dfecb50271f49b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Aug 2016 11:13:17 +0200 Subject: [PATCH] Remove on arming reset, be less verbose in normal conditions output --- .../BlockLocalPositionEstimator.cpp | 40 +++++++++++-------- .../local_position_estimator/sensors/baro.cpp | 4 +- .../local_position_estimator/sensors/flow.cpp | 2 +- .../local_position_estimator/sensors/gps.cpp | 12 +++--- .../sensors/sonar.cpp | 2 +- .../sensors/vision.cpp | 2 +- 6 files changed, 36 insertions(+), 26 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index bdabd56b79..ede4309e58 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -254,28 +254,34 @@ void BlockLocalPositionEstimator::update() } // reset pos, vel, and terrain on arming - if (!_lastArmedState && armedState) { - // we just armed, we are at origin on the ground - _x(X_x) = 0; - _x(X_y) = 0; + // XXX this will be re-enabled for indoor use cases using a + // selection param, but is really not helping outdoors + // right now. - // reset flow integral - _flowX = 0; - _flowY = 0; + // if (!_lastArmedState && armedState) { - // we aren't moving, all velocities are zero - _x(X_vx) = 0; - _x(X_vy) = 0; - _x(X_vz) = 0; + // // 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; - // assume we are on the ground, so terrain alt is local alt - _x(X_tz) = _x(X_z); + // // reset flow integral + // _flowX = 0; + // _flowY = 0; - // reset lowpass filter as well - _xLowPass.setState(_x); - _aglLowPass.setState(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; diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index 1d0624a8c8..1f30fe98aa 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -76,8 +76,10 @@ void BlockLocalPositionEstimator::baroCorrect() if (beta > BETA_TABLE[n_y_baro]) { if (_baroFault < FAULT_MINOR) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f", + if (beta > 2.0f * BETA_TABLE[n_y_baro]) { + mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f", double(r(0)), double(beta)); + } _baroFault = FAULT_MINOR; } diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index 13b646d9e0..bdfcb3fb7e 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -180,7 +180,7 @@ void BlockLocalPositionEstimator::flowCheckTimeout() if (_flowInitialized) { _flowInitialized = false; _flowQStats.reset(); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow timeout "); + mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] flow timeout "); } } } diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index ffe3db2aab..4056c16bff 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -46,7 +46,7 @@ void BlockLocalPositionEstimator::gpsInit() } _gpsAltOrigin = _gpsStats.getMean()(2); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps init " + PX4_INFO("[lpe] gps init " "lat %6.2f lon %6.2f alt %5.1f m", gpsLatOrigin, gpsLonOrigin, @@ -175,9 +175,11 @@ void BlockLocalPositionEstimator::gpsCorrect() if (beta > BETA_TABLE[n_y_gps]) { if (_gpsFault < FAULT_MINOR) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g", - double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)), - double(r(3)*r(3) / S_I(3, 3)), double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5))); + if (beta > 2.0f * BETA_TABLE[n_y_gps]) { + mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g", + double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)), + double(r(3)*r(3) / S_I(3, 3)), double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5))); + } _gpsFault = FAULT_MINOR; } @@ -202,7 +204,7 @@ void BlockLocalPositionEstimator::gpsCheckTimeout() if (_gpsInitialized) { _gpsInitialized = false; _gpsStats.reset(); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS timeout "); + mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] GPS timeout "); } } } diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp index d2ea5be9f5..fdb8d8f0a5 100644 --- a/src/modules/local_position_estimator/sensors/sonar.cpp +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -34,7 +34,7 @@ void BlockLocalPositionEstimator::sonarInit() _sonarStats.reset(); } else { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init " + PX4_INFO("[lpe] sonar init " "mean %d cm std %d cm", int(100 * _sonarStats.getMean()(0)), int(100 * _sonarStats.getStdDev()(0))); diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index f0d9f8c0db..0832b22473 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -109,7 +109,7 @@ void BlockLocalPositionEstimator::visionCheckTimeout() if (_visionInitialized) { _visionInitialized = false; _visionStats.reset(); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position timeout "); + mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] vision position timeout "); } } }