ekf2: initialize origin from corrent position when possible

PX4 · Aug 23, 2024 · 73c3bad · 73c3bad
1 parent 8698b84
commit 73c3bad
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 3 deletions.
diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
@@ -113,7 +113,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 
 				} else {
 					// Try to initialize using measurement
-					if (ekf.setEkfGlobalOrigin(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, sample.epv)) {
+					if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
+							sample.epv)) {
 						ekf.enableControlStatusAuxGpos();
 						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
 						_state = State::active;

diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp
@@ -280,8 +280,7 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
 		uint64_t timestamp_observation)
 {
 	if (!_pos_ref.isInitialized()) {
-		ECL_WARN("unable to reset global position, position reference not initialized");
-		return false;
+		return setLatLonOriginFromCurrentPos(lat_deg, lon_deg, accuracy);
 	}
 
 	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);