diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index 91a8685ceb..cc165d4533 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -87,7 +87,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) // determine if we should use height aiding const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast(GnssCtrl::VPOS)) && measurement_valid - && _NED_origin_initialised + && _pos_ref.isInitialized() && _gps_checks_passed; const bool starting_conditions_passing = continuing_conditions_passing diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index e3c40dda77..8d3cbd9567 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -57,11 +57,9 @@ void Ekf::collect_gps(const gnssSample &gps) { - if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) { + if (_filter_initialised && !_pos_ref.isInitialized() && _gps_checks_passed) { // If we have good GPS data set the origin's WGS-84 position to the last gps fix - if (!_pos_ref.isInitialized()) { - setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc); - } + setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc); // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin if (!PX4_ISFINITE(_gps_alt_ref)) { @@ -77,7 +75,7 @@ void Ekf::collect_gps(const gnssSample &gps) // a rough 2D fix is sufficient to lookup earth spin rate const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000); - if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) { + if (gps_rough_2d_fix && (_gps_checks_passed || !_pos_ref.isInitialized())) { _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); } } diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 7bfb2e2831..bf3d086c6a 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -109,7 +109,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) const bool continuing_conditions_passing = (gnss_vel_enabled || gnss_pos_enabled) && _control_status.flags.tilt_align && _control_status.flags.yaw_align - && _NED_origin_initialised; + && _pos_ref.isInitialized(); const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed; if (_control_status.flags.gps) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 2255807467..35c564157f 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -188,8 +188,7 @@ public: Vector3f getPositionVariance() const { return getStateVariance(); } // 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; + void getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; bool checkLatLonValidity(double latitude, double longitude); bool checkAltitudeValidity(float altitude); bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN); @@ -223,7 +222,7 @@ public: // and have not started using synthetic position observations to constrain drift bool global_position_is_valid() const { - return (_NED_origin_initialised && local_position_is_valid()); + return (_pos_ref.isInitialized() && local_position_is_valid()); } // return true if the local position estimate is valid diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index a5d1f362a5..26757aacbc 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -63,13 +63,12 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad)); } -bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const +void Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const { origin_time = _pos_ref.getProjectionReferenceTimestamp(); latitude = _pos_ref.getProjectionReferenceLat(); longitude = _pos_ref.getProjectionReferenceLon(); origin_alt = getEkfGlobalOriginAltitude(); - return _NED_origin_initialised; } bool Ekf::checkLatLonValidity(const double latitude, const double longitude) @@ -122,8 +121,6 @@ bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const f _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); @@ -198,8 +195,6 @@ bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double long _gpos_origin_eph = eph; } - _NED_origin_initialised = true; - return true; } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 3fd302df00..a188d2f88e 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -250,7 +250,7 @@ public: // At the next startup, set param.mag_declination_deg to the value saved bool get_mag_decl_deg(float &val) const { - if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) { + if (PX4_ISFINITE(_wmm_declination_rad) && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) { val = math::degrees(_wmm_declination_rad); return true; @@ -261,7 +261,7 @@ public: bool get_mag_inc_deg(float &val) const { - if (_NED_origin_initialised) { + if (PX4_ISFINITE(_wmm_inclination_rad)) { val = math::degrees(_wmm_inclination_rad); return true; @@ -307,7 +307,7 @@ public: const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); } const uint64_t &time_delayed_us() const { return _time_delayed_us; } - const bool &global_origin_valid() const { return _NED_origin_initialised; } + bool global_origin_valid() const { return _pos_ref.isInitialized(); } const MapProjection &global_origin() const { return _pos_ref; } float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } @@ -379,7 +379,6 @@ protected: bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized // Variables used to publish the WGS-84 location of the EKF local NED origin - bool _NED_origin_initialised{false}; MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin float _gps_alt_ref{NAN}; ///< WGS-84 height (m) float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin