From 37caddedbb8e8fd871ebbe942f03c257ae6a0012 Mon Sep 17 00:00:00 2001
From: Drone-Lab <2334088143@qq.com>
Date: Wed, 20 Mar 2024 15:37:19 +0800
Subject: [PATCH] navigator: update mission after changing home position
 (#22834)

---
 src/modules/navigator/mission_base.cpp | 27 ++++++++++++++++++++++++++
 src/modules/navigator/mission_base.h   |  8 ++++++++
 2 files changed, 35 insertions(+)

diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp
index 4a376995a0..026cefaba2 100644
--- a/src/modules/navigator/mission_base.cpp
+++ b/src/modules/navigator/mission_base.cpp
@@ -253,6 +253,7 @@ MissionBase::on_active()
 
 	updateMavlinkMission();
 	updateDatamanCache();
+	updateMissionAltAfterHomeChanged();
 
 	/* Check the mission */
 	if (!_mission_checked && canRunMissionFeasibility()) {
@@ -1375,3 +1376,29 @@ bool MissionBase::canRunMissionFeasibility()
 	       (_geofence_status_sub.get().geofence_id == _mission.geofence_id) &&
 	       (_geofence_status_sub.get().status == geofence_status_s::GF_STATUS_READY);
 }
+
+void MissionBase::updateMissionAltAfterHomeChanged()
+{
+	if (_navigator->get_home_position()->update_count > _home_update_counter) {
+		float new_alt = get_absolute_altitude_for_item(_mission_item);
+		float altitude_diff = new_alt - _navigator->get_position_setpoint_triplet()->current.alt;
+
+		if (_navigator->get_position_setpoint_triplet()->previous.valid
+		    && PX4_ISFINITE(_navigator->get_position_setpoint_triplet()->previous.alt)) {
+			_navigator->get_position_setpoint_triplet()->previous.alt = _navigator->get_position_setpoint_triplet()->previous.alt +
+					altitude_diff;
+		}
+
+		_navigator->get_position_setpoint_triplet()->current.alt = _navigator->get_position_setpoint_triplet()->current.alt +
+				altitude_diff;
+
+		if (_navigator->get_position_setpoint_triplet()->next.valid
+		    && PX4_ISFINITE(_navigator->get_position_setpoint_triplet()->next.alt)) {
+			_navigator->get_position_setpoint_triplet()->next.alt = _navigator->get_position_setpoint_triplet()->next.alt +
+					altitude_diff;
+		}
+
+		_navigator->set_position_setpoint_triplet_updated();
+		_home_update_counter = _navigator->get_home_position()->update_count;
+	}
+}
diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h
index 7f234bb7d2..1b788106f4 100644
--- a/src/modules/navigator/mission_base.h
+++ b/src/modules/navigator/mission_base.h
@@ -448,8 +448,16 @@ private:
 	 */
 	bool checkMissionDataChanged(mission_s new_mission);
 
+	/**
+	 * @brief update current mission altitude after the home position has changed.
+	 */
+
+	void updateMissionAltAfterHomeChanged();
+
 	bool canRunMissionFeasibility();
 
+	uint32_t _home_update_counter = 0; /**< Variable to store the previous value for home change detection.*/
+
 	bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment.
 
 	mission_item_s _last_gimbal_configure_item {};