Changed LPE distance sensor timeout logic. (#4996)

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -33,19 +33,23 @@ void BlockLocalPositionEstimator::lidarInit()
 int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
 {
 	// measure
-	float d = _sub_lidar->get().current_distance + _lidar_z_offset.get();
-	warnx("d %10.2g, lidar z offset %10.2g\n", double(d), double(_lidar_z_offset.get()));
-	float eps = 0.01f;
+	float d = _sub_lidar->get().current_distance;
+	float eps = 0.01f; // 1 cm
 	float min_dist = _sub_lidar->get().min_distance + eps;
 	float max_dist = _sub_lidar->get().max_distance - eps;
 
+	// prevent driver from setting min dist below eps
+	if (min_dist < eps) {
+		min_dist = eps;
+	}
+
 	// check for bad data
 	if (d > max_dist || d < min_dist) {
 		return -1;
 	}
 
 	// update stats
-	_lidarStats.update(Scalarf(d));
+	_lidarStats.update(Scalarf(d + _lidar_z_offset.get()));
 	_time_last_lidar = _timeStamp;
 	y.setZero();
 	y(0) = d;

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -48,10 +48,15 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
 {
 	// measure
 	float d = _sub_sonar->get().current_distance;
-	float eps = 0.01f;
+	float eps = 0.01f; // 1 cm
 	float min_dist = _sub_sonar->get().min_distance + eps;
 	float max_dist = _sub_sonar->get().max_distance - eps;
 
+	// prevent driver from setting min dist below eps
+	if (min_dist < eps) {
+		min_dist = eps;
+	}
+
 	// check for bad data
 	if (d > max_dist || d < min_dist) {
 		return -1;