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

Navigator: fix RTL_TYP=2 with NAV_CMD_CONDITION_GATE (#25648)

Browse Source 
* rtl_mission_fast: make sure to set a position item on activation

* rtl_mission_fast_reverse: make sure to set a position item on activation

---------

Co-authored-by: Konrad Rudin <konrad@auterion.com>
This commit is contained in:
Silvan Fuhrer 2025-10-24 10:10:33 +02:00 committed by GitHub
parent 8f38a2ddbc
commit 17e96554ec
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
4 changed files with 38 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/modules/navigator/rtl_mission_fast.cpp
View File

@ -57,7 +57,7 @@ void RtlMissionFast::on_inactive()
MissionBase::on_inactive();
_vehicle_status_sub.update();
_mission_index_prior_rtl = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ?
_mission.current_seq : -1;
_mission.current_seq : INT32_C(-1);
}
void RtlMissionFast::on_activation()
@ -65,13 +65,30 @@ void RtlMissionFast::on_activation()
_home_pos_sub.update();
// set mission item to closest item if not already in mission
if (_mission_index_prior_rtl < 0) {
if (_mission_index_prior_rtl < INT32_C(0)) {
_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
} else {
setMissionIndex(_mission_index_prior_rtl);
_is_current_planned_mission_item_valid = isMissionValid();
int32_t next_mission_item_index;
size_t num_found_items{0U};
getNextPositionItems(_mission_index_prior_rtl, &next_mission_item_index, num_found_items, UINT8_C(1));
if (num_found_items > 0U) {
setMissionIndex(next_mission_item_index);
_is_current_planned_mission_item_valid = isMissionValid();
} else {
// No more position items left. Set it to the land item if it exists
if (_mission.land_index > 0) {
setMissionIndex(_mission.land_index);
_is_current_planned_mission_item_valid = isMissionValid();
} else {
// Nothing we can do, set the validity to false to trigger end of mission reaction
_is_current_planned_mission_item_valid = false;
}
}
}
if (_land_detected_sub.get().landed) {

2
src/modules/navigator/rtl_mission_fast.h
View File

@ -64,7 +64,7 @@ private:
bool setNextMissionItem() override;
void setActiveMissionItems() override;
int _mission_index_prior_rtl{-1};
int32_t _mission_index_prior_rtl{INT32_C(-1)};
uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
};

19
src/modules/navigator/rtl_mission_fast_reverse.cpp
View File

@ -57,7 +57,7 @@ void RtlMissionFastReverse::on_inactive()
MissionBase::on_inactive();
_vehicle_status_sub.update();
_mission_index_prior_rtl = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ?
_mission.current_seq : -1;
_mission.current_seq : INT32_C(-1);
}
void RtlMissionFastReverse::on_inactivation()
@ -71,13 +71,24 @@ void RtlMissionFastReverse::on_activation()
_home_pos_sub.update();
// set mission item to closest item if not already in mission. If we are in mission, set to the previous item.
if (_mission_index_prior_rtl < 0) {
if (_mission_index_prior_rtl < INT32_C(0)) {
_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
} else {
setMissionIndex(math::max(_mission_index_prior_rtl - 1, 0));
_is_current_planned_mission_item_valid = isMissionValid();
int32_t previous_mission_item_index;
size_t num_found_items{0U};
getPreviousPositionItems(math::max(_mission_index_prior_rtl - INT32_C(1), INT32_C(0)), &previous_mission_item_index,
num_found_items, UINT8_C(1));
if (num_found_items > 0U) {
setMissionIndex(previous_mission_item_index);
_is_current_planned_mission_item_valid = isMissionValid();
} else {
// No prior position items, so try to go to the first one.
_is_current_planned_mission_item_valid = (goToNextPositionItem(false) == PX4_OK);
}
}
if (_land_detected_sub.get().landed) {

2
src/modules/navigator/rtl_mission_fast_reverse.h
View File

@ -69,7 +69,7 @@ private:
void setActiveMissionItems() override;
void handleLanding(WorkItemType &new_work_item_type);
int _mission_index_prior_rtl{-1};
int32_t _mission_index_prior_rtl{INT32_C(-1)};
bool _in_landing_phase{false};