diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 505a039d22..6bbcb284f6 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/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"), diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 4ccdb329f4..c32db40066 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/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; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 590e726ccb..071aabc9fe 100644 --- a/src/modules/local_position_estimator/params.c +++ b/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. * diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index f3de04856e..351a134696 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/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; } diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index 5bff337a55..c71512deb9 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -51,8 +51,15 @@ int BlockLocalPositionEstimator::gpsMeasure(Vector &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; }