diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e9715f3e37..44f635573c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -261,6 +261,8 @@ public: // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; + bool checkLatLonValidity(double latitude, double longitude, float eph = 0.f); + bool checkAltitudeValidity(float altitude, float epv = 0.f); bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f); // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position @@ -765,6 +767,9 @@ private: P.slice(S.idx, S.idx) = cov; } + bool setLatLonOrigin(double latitude, double longitude, float eph = NAN); + bool setAltOrigin(float altitude, float epv = NAN); + // update quaternion states and covariances using an innovation, observation variance and Jacobian vector bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW); void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 01821fd97b..4ba346e1f9 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -72,63 +72,95 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo return _NED_origin_initialised; } +bool Ekf::checkLatLonValidity(const double latitude, const double longitude, const float eph) +{ + const bool lat_valid = (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)); + const bool lon_valid = (PX4_ISFINITE(longitude) && (abs(longitude) <= 180)); + const bool eph_valid = (PX4_ISFINITE(eph) && (eph >= 0.f)); + + return (lat_valid && lon_valid && eph_valid); +} + +bool Ekf::checkAltitudeValidity(const float altitude, const float epv) +{ + // sanity check valid altitude anywhere between the Mariana Trench and edge of Space + const bool alt_valid = (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f))); + const bool epv_valid = (PX4_ISFINITE(epv) && (epv >= 0.f)); + + return alt_valid && epv_valid; +} + bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph, const float epv) { - // sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space - if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90) - && PX4_ISFINITE(longitude) && (abs(longitude) <= 180) - && PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f) - ) { - bool current_pos_available = false; - double current_lat = static_cast(NAN); - double current_lon = static_cast(NAN); - - // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (_pos_ref.isInitialized() && isHorizontalAidingActive()) { - _pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon); - current_pos_available = true; - } - - const float gps_alt_ref_prev = _gps_alt_ref; - - // reinitialize map projection to latitude, longitude, altitude, and reset position - _pos_ref.initReference(latitude, longitude, _time_delayed_us); - _gps_alt_ref = altitude; - - _gpos_origin_eph = eph; - _gpos_origin_epv = epv; - - _NED_origin_initialised = true; - - _earth_rate_NED = calcEarthRateNED(math::radians(static_cast(latitude))); - - if (current_pos_available) { - // reset horizontal position if we already have a global origin - Vector2f position = _pos_ref.project(current_lat, current_lon); - resetHorizontalPositionTo(position); - } - - if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) { - // determine current z - const float z_prev = _state.pos(2); - const float current_alt = -z_prev + gps_alt_ref_prev; -#if defined(CONFIG_EKF2_GNSS) - const float gps_hgt_bias = _gps_hgt_b_est.getBias(); -#endif // CONFIG_EKF2_GNSS - resetVerticalPositionTo(_gps_alt_ref - current_alt); - ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev, - (double)_state.pos(2)); -#if defined(CONFIG_EKF2_GNSS) - // adjust existing GPS height bias - _gps_hgt_b_est.setBias(gps_hgt_bias); -#endif // CONFIG_EKF2_GNSS - } - - return true; + if (!setLatLonOrigin(latitude, longitude, eph)) { + return false; } - return false; + // altitude is optional + setAltOrigin(altitude, epv); + + return true; +} + +bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph) +{ + if (!checkLatLonValidity(latitude, longitude, eph)) { + return false; + } + + bool current_pos_available = false; + double current_lat = static_cast(NAN); + double current_lon = static_cast(NAN); + + // if we are already doing aiding, correct for the change in position since the EKF started navigating + if (_pos_ref.isInitialized() && isHorizontalAidingActive()) { + _pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon); + current_pos_available = true; + } + + // reinitialize map projection to latitude, longitude, altitude, and reset position + _pos_ref.initReference(latitude, longitude, _time_delayed_us); + _gpos_origin_eph = eph; + + _NED_origin_initialised = true; + + if (current_pos_available) { + // reset horizontal position if we already have a global origin + Vector2f position = _pos_ref.project(current_lat, current_lon); + resetHorizontalPositionTo(position); + } + + return true; +} + +bool Ekf::setAltOrigin(const float altitude, const float epv) +{ + if (!checkAltitudeValidity(altitude, epv)) { + return false; + } + + const float gps_alt_ref_prev = _gps_alt_ref; + _gps_alt_ref = altitude; + _gpos_origin_epv = epv; + + if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) { + // determine current z + const float z_prev = _state.pos(2); + const float current_alt = -z_prev + gps_alt_ref_prev; +#if defined(CONFIG_EKF2_GNSS) + const float gps_hgt_bias = _gps_hgt_b_est.getBias(); +#endif // CONFIG_EKF2_GNSS + resetVerticalPositionTo(_gps_alt_ref - current_alt); + ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev, + (double)_state.pos(2)); +#if defined(CONFIG_EKF2_GNSS) + // adjust existing GPS height bias + _gps_hgt_b_est.setBias(gps_hgt_bias); +#endif // CONFIG_EKF2_GNSS + } + + return true; } void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const