ekf2: send global position if origin is set manually

Even if the aiding is local only, a valid global estimate can be sent as
long as we know the EKF's origin.

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -44,6 +44,7 @@
 #include "python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h"
 
 #include <mathlib/mathlib.h>
+#include <lib/world_magnetic_model/geo_mag_declination.h>
 #include <cstdlib>
 
 void Ekf::resetHorizontalVelocityToZero()
@@ -432,6 +433,24 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
 		_gps_alt_ref = altitude;
 
+		const float mag_declination_gps = get_mag_declination_radians(latitude, longitude);
+		const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude);
+		const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude);
+
+		if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {
+			_mag_declination_gps = mag_declination_gps;
+			_mag_inclination_gps = mag_inclination_gps;
+			_mag_strength_gps = mag_strength_gps;
+
+			_wmm_gps_time_last_set = _time_delayed_us;
+		}
+
+		// We don't know the uncertainty of the origin
+		_gpos_origin_eph = 0.f;
+		_gpos_origin_epv = 0.f;
+
+		_NED_origin_initialised = true;
+
 		// minimum change in position or height that triggers a reset
 		static constexpr float MIN_RESET_DIST_M = 0.01f;
 
@@ -468,7 +487,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	// report absolute accuracy taking into account the uncertainty in location of the origin
 	// If not aiding, return 0 for horizontal position estimate as no estimate is available
 	// TODO - allow for baro drift in vertical position error
-	float hpos_err = sqrtf(P(7, 7) + P(8, 8) + sq(_gps_origin_eph));
+	float hpos_err = sqrtf(P(7, 7) + P(8, 8) + sq(_gpos_origin_eph));
 
 	// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
@@ -486,7 +505,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	}
 
 	*ekf_eph = hpos_err;
-	*ekf_epv = sqrtf(P(9, 9) + sq(_gps_origin_epv));
+	*ekf_epv = sqrtf(P(9, 9) + sq(_gpos_origin_epv));
 }
 
 // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position

src/modules/ekf2/EKF/estimator_interface.h

@@ -375,8 +375,8 @@ protected:
 	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized
 
 	bool _NED_origin_initialised{false};
-	float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin
-	float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin
+	float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin
+	float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin
 	MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
 	MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
 	float _gps_alt_prev{0.0f};	// height from the previous GPS message (m)

src/modules/ekf2/EKF/gps_checks.cpp

@@ -85,8 +85,8 @@ bool Ekf::collect_gps(const gpsMessage &gps)
 		_NED_origin_initialised = true;
 
 		// save the horizontal and vertical position uncertainty of the origin
-		_gps_origin_eph = gps.eph;
-		_gps_origin_epv = gps.epv;
+		_gpos_origin_eph = gps.eph;
+		_gpos_origin_epv = gps.epv;
 
 		_information_events.flags.gps_checks_passed = true;
 		ECL_INFO("GPS checks passed");

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -271,6 +271,8 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
 	EXPECT_DOUBLE_EQ(_longitude, _longitude_new);
 	EXPECT_FLOAT_EQ(_altitude, _altitude_new);
 
+	EXPECT_FALSE(_ekf->global_origin_valid());
+
 	_latitude_new  = 45.0000005;
 	_longitude_new = 111.0000005;
 	_altitude_new  = 1500.0;
@@ -282,6 +284,11 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
 	EXPECT_DOUBLE_EQ(_longitude, _longitude_new);
 	EXPECT_FLOAT_EQ(_altitude, _altitude_new);
 
+	// Global origin has been initialized but since there is no position aiding, the global
+	// position is still not valid
+	EXPECT_TRUE(_ekf->global_origin_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+
 	_sensor_simulator.runSeconds(1);
 
 	float hpos = 0.f;
@@ -299,4 +306,62 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
 	EXPECT_NEAR(vvel, 0.f, 0.02f);
 }
 
+TEST_F(EkfBasicsTest, global_position_from_local_ev)
+{
+	// GIVEN: external vision (local) aiding
+	_ekf_wrapper.enableExternalVisionPositionFusion();
+	_sensor_simulator._vio.setPositionFrameToLocalNED();
+	_sensor_simulator.startExternalVision();
+
+	_sensor_simulator.runSeconds(1);
+
+	// THEN; since there is no origin, only the local position can be valid
+	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_origin_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+
+	_latitude_new  = 45.0000005;
+	_longitude_new = 111.0000005;
+	_altitude_new  = 1500.0;
+
+	// BUT WHEN: the global origin is set (manually)
+	_ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new);
+
+	// THEN: local and global positions are valid
+	EXPECT_TRUE(_ekf->global_origin_valid());
+	EXPECT_TRUE(_ekf->global_position_is_valid());
+	EXPECT_TRUE(_ekf->local_position_is_valid());
+}
+
+TEST_F(EkfBasicsTest, global_position_from_opt_flow)
+{
+	// GIVEN: optical flow aiding
+	const float max_flow_rate = 5.f;
+	const float min_ground_distance = 0.f;
+	const float max_ground_distance = 50.f;
+	_ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance);
+	_sensor_simulator.startFlow();
+	_ekf_wrapper.enableFlowFusion();
+	_sensor_simulator.startRangeFinder();
+
+	_sensor_simulator.runSeconds(1);
+
+	// THEN; since there is no origin, only the local position can be valid
+	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_origin_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+
+	_latitude_new  = 45.0000005;
+	_longitude_new = 111.0000005;
+	_altitude_new  = 1500.0;
+
+	// BUT WHEN: the global origin is set (manually)
+	_ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new);
+
+	// THEN: local and global positions are valid
+	EXPECT_TRUE(_ekf->global_origin_valid());
+	EXPECT_TRUE(_ekf->global_position_is_valid());
+	EXPECT_TRUE(_ekf->local_position_is_valid());
+}
+
 // TODO: Add sampling tests

src/modules/ekf2/test/test_EKF_fusionLogic.cpp

@@ -183,7 +183,6 @@ TEST_F(EkfFusionLogicTest, fallbackFromGpsToFlow)
 	const float max_ground_distance = 50.f;
 	_ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance);
 	_sensor_simulator.startFlow();
-	_sensor_simulator.startFlow();
 	_ekf_wrapper.enableFlowFusion();
 
 	_ekf->set_in_air_status(true);