added support to reset vehicle position based on external position (#22444)

via command MAV_CMD_EXTERNAL_POSITION_ESTIMATE

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/ekf2/EKF/common.h

@@ -655,6 +655,7 @@ union information_event_status_u {
 		bool reset_hgt_to_gps           : 1; ///< 14 - true when the vertical position state is reset to the gps measurement
 		bool reset_hgt_to_rng           : 1; ///< 15 - true when the vertical position state is reset to the rng measurement
 		bool reset_hgt_to_ev            : 1; ///< 16 - true when the vertical position state is reset to the ev measurement
+		bool reset_pos_to_ext_obs       : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/ekf.cpp

@@ -341,6 +341,22 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
 }
 
+void
+Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation) {
+
+   if (!_pos_ref.isInitialized()) {
+       return;
+   }
+
+   // apply a first order correction using velocity at the delated time horizon and the delta time
+   timestamp_observation = math::min(_time_latest_us, timestamp_observation);
+   const float dt = _time_delayed_us > timestamp_observation ? static_cast<float>(_time_delayed_us - timestamp_observation) * 1e-6f : -static_cast<float>(timestamp_observation - _time_delayed_us) * 1e-6f;
+
+   Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt;
+
+    resetHorizontalPositionToExternal(pos_corrected, math::max(accuracy, FLT_EPSILON));
+}
+
 void Ekf::updateParameters()
 {
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)

src/modules/ekf2/EKF/ekf.h

@@ -502,6 +502,8 @@ public:
 		return is_healthy;
 	}
 
+    void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
+
 	void updateParameters();
 
 	friend class AuxGlobalPosition;
@@ -812,6 +814,7 @@ private:
 
 	void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
 	void resetHorizontalPositionToLastKnown();
+    void resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy);
 
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); }

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -171,6 +171,12 @@ bool Ekf::isHeightResetRequired() const
 	return (continuous_bad_accel_hgt || hgt_fusion_timeout);
 }
 
+void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) {
+    _information_events.flags.reset_pos_to_ext_obs = true;
+    ECL_INFO("reset position to external observation");
+    resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy));
+}
+
 void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
 {
 	const float old_vert_pos = _state.pos(2);

src/modules/ekf2/EKF2.cpp

@@ -40,6 +40,9 @@ using matrix::Eulerf;
 using matrix::Quatf;
 using matrix::Vector3f;
 
+static constexpr float kDefaultExternalPosAccuracy = 50.0f; // [m]
+static constexpr float kMaxDelaySecondsExternalPosMeasurement = 15.0f; // [s]
+
 pthread_mutex_t ekf2_module_mutex = PTHREAD_MUTEX_INITIALIZER;
 static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
 #if defined(CONFIG_EKF2_MULTI_INSTANCE)
@@ -554,6 +557,26 @@ void EKF2::Run()
 						_instance, latitude, longitude, static_cast<double>(altitude));
 				}
 			}
+
+			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) {
+				if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning) &&
+				    PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)) {
+
+					const float measurement_delay_seconds = math::constrain(vehicle_command.param2, 0.0f,
+										kMaxDelaySecondsExternalPosMeasurement);
+					const uint64_t timestamp_observation = vehicle_command.timestamp - measurement_delay_seconds * 1_s;
+
+					float accuracy = kDefaultExternalPosAccuracy;
+
+					if (PX4_ISFINITE(vehicle_command.param3) && vehicle_command.param3 > FLT_EPSILON) {
+						accuracy = vehicle_command.param3;
+					}
+
+					// TODO add check for lat and long validity
+					_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6,
+							accuracy, timestamp_observation);
+				}
+			}
 		}
 	}