Remove on arming reset, be less verbose in normal conditions output

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;
 

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;
 		}
 

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 ");
 		}
 	}
 }

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 ");
 		}
 	}
 }

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)));

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 ");
 		}
 	}
 }