diff --git a/px4_ros2_cpp/include/px4_ros2/navigation/experimental/global_position_measurement_interface.hpp b/px4_ros2_cpp/include/px4_ros2/navigation/experimental/global_position_measurement_interface.hpp index 98ceece..fb06098 100644 --- a/px4_ros2_cpp/include/px4_ros2/navigation/experimental/global_position_measurement_interface.hpp +++ b/px4_ros2_cpp/include/px4_ros2/navigation/experimental/global_position_measurement_interface.hpp @@ -62,6 +62,16 @@ class GlobalPositionMeasurementInterface : public PositionMeasurementInterfaceBa */ void update(const GlobalPositionMeasurement & global_position_measurement) const; +/** + * @brief Notify the FMU that the global position estimate has been reset. + * + * Increments the reset counter for horizontal position (latitude and longitude) to signal the EKF + * of a discontinuity in external position data (e.g., loss of tracking or reinitialization). Future measurement + * updates will contain the incremented counter. This helps the EKF adjust and maintain consistent state estimation, + * and accept new measurements after a gap or inconsistency in updates. + */ + inline void reset() {++_lat_lon_reset_counter;} + private: /** * @brief Check that at least one measurement value is defined. @@ -85,6 +95,7 @@ class GlobalPositionMeasurementInterface : public PositionMeasurementInterfaceBa rclcpp::Publisher::SharedPtr _aux_global_position_pub; + uint8_t _lat_lon_reset_counter{0}; /** Counter for reset events on horizontal position coordinates */ // uint8_t _altitude_frame; }; diff --git a/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp b/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp index b440bb2..c49d790 100644 --- a/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp +++ b/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp @@ -41,6 +41,7 @@ void GlobalPositionMeasurementInterface::update( // Populate aux global position VehicleGlobalPosition aux_global_position; + aux_global_position.lat_lon_reset_counter = _lat_lon_reset_counter; aux_global_position.timestamp_sample = global_position_measurement.timestamp_sample.nanoseconds() *