Make LPE sonar reading more robust. (#4806)

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

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;
 

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

src/modules/local_position_estimator/sensors/flow.cpp

@@ -49,21 +49,12 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &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;

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<float, n_y_sonar> 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<float, n_y_sonar> &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<float, n_y_sonar> &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;