Add more GPS checks for LPE (#4750)

* Added more GPS checks for LPE.

* Add logic to force GPS alt init.

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -65,6 +65,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_gps_vxy_stddev(this, "GPS_VXY"),
 	_gps_vz_stddev(this, "GPS_VZ"),
 	_gps_eph_max(this, "EPH_MAX"),
+	_gps_epv_max(this, "EPV_MAX"),
 	_vision_xy_stddev(this, "VIS_XY"),
 	_vision_z_stddev(this, "VIS_Z"),
 	_vision_on(this, "VIS_ON"),

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -262,6 +262,7 @@ private:
 	BlockParamFloat  _gps_vxy_stddev;
 	BlockParamFloat  _gps_vz_stddev;
 	BlockParamFloat  _gps_eph_max;
+	BlockParamFloat  _gps_epv_max;
 
 	// vision parameters
 	BlockParamFloat  _vision_xy_stddev;

src/modules/local_position_estimator/params.c

@@ -129,7 +129,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
 PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
 
 /**
- * GPS
+ * Enables GPS data, also forces alt init with GPS
  *
  * @group Local Position Estimator
  * @boolean
@@ -203,6 +203,17 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
  */
 PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
 
+/**
+ * GPS max epv
+ *
+ * @group Local Position Estimator
+ * @unit m
+ * @min 1.0
+ * @max 5.0
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_EPV_MAX, 5.0f);
+
 /**
  * Vision xy standard deviation.
  *

src/modules/local_position_estimator/sensors/baro.cpp

@@ -29,7 +29,8 @@ void BlockLocalPositionEstimator::baroInit()
 		_baroInitialized = true;
 		_baroFault = FAULT_NONE;
 
-		if (!_altHomeInitialized) {
+		// only initialize alt home with baro if gps is disabled
+		if (!_altHomeInitialized && !_gps_on.get()) {
 			_altHomeInitialized = true;
 			_altHome = _baroAltHome;
 		}

src/modules/local_position_estimator/sensors/gps.cpp

@@ -51,8 +51,15 @@ int BlockLocalPositionEstimator::gpsMeasure(Vector<double, n_y_gps> &y)
 	// check for good gps signal
 	uint8_t nSat = _sub_gps.get().satellites_used;
 	float eph = _sub_gps.get().eph;
+	float epv = _sub_gps.get().epv;
+	uint8_t fix_type = _sub_gps.get().fix_type;
 
-	if (nSat < 6 || eph > _gps_eph_max.get()) {
+	if (
+		nSat < 6 ||
+		eph > _gps_eph_max.get() ||
+		epv > _gps_epv_max.get() ||
+		fix_type < 3
+	) {
 		return -1;
 	}