diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index 1f30fe98aa..c41bfed60c 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -78,8 +78,9 @@ void BlockLocalPositionEstimator::baroCorrect() if (_baroFault < FAULT_MINOR) { 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)); + double(r(0)), double(beta)); } + _baroFault = FAULT_MINOR; } diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index 4056c16bff..ff331072be 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -47,10 +47,10 @@ void BlockLocalPositionEstimator::gpsInit() _gpsAltOrigin = _gpsStats.getMean()(2); PX4_INFO("[lpe] gps init " - "lat %6.2f lon %6.2f alt %5.1f m", - gpsLatOrigin, - gpsLonOrigin, - double(_gpsAltOrigin)); + "lat %6.2f lon %6.2f alt %5.1f m", + gpsLatOrigin, + gpsLonOrigin, + double(_gpsAltOrigin)); _gpsInitialized = true; _gpsFault = FAULT_NONE; _gpsStats.reset(); @@ -177,9 +177,10 @@ void BlockLocalPositionEstimator::gpsCorrect() if (_gpsFault < FAULT_MINOR) { 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))); + 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; } diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp index fdb8d8f0a5..c03015a53a 100644 --- a/src/modules/local_position_estimator/sensors/sonar.cpp +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -35,9 +35,9 @@ void BlockLocalPositionEstimator::sonarInit() } else { PX4_INFO("[lpe] sonar init " - "mean %d cm std %d cm", - int(100 * _sonarStats.getMean()(0)), - int(100 * _sonarStats.getStdDev()(0))); + "mean %d cm std %d cm", + int(100 * _sonarStats.getMean()(0)), + int(100 * _sonarStats.getStdDev()(0))); _sonarInitialized = true; _sonarFault = FAULT_NONE; }