fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(navigator): fix alt setpoint after home-alt reset (#26662)

Browse Source 
fix(navigator): skip mission alt update for non-position items
This commit is contained in:
Marco Hauswirth 2026-03-09 15:39:21 +01:00 committed by GitHub
parent 3aa499dfce
commit 3e396f65e5
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 17 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/modules/navigator/mission_base.cpp
View File

@ -1460,25 +1460,26 @@ bool MissionBase::canRunMissionFeasibility()
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;
if (item_contains_position(_mission_item)) {
const float new_alt = get_absolute_altitude_for_item(_mission_item);
const 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 += altitude_diff;
}
_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 += altitude_diff;
}
_navigator->set_position_setpoint_triplet_updated();
}
_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;
}
}