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

RTL_mission_reverse: start from previous WP if RTL is triggered while in Mission

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2024-05-24 13:40:21 +02:00
parent b9d3b9f211
commit 42bca65cbf
2 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -52,12 +52,27 @@ RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) :
}
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;
}
void RtlMissionFastReverse::on_activation()
{
_home_pos_sub.update();
_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;
// 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) {
_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();
}
if (_land_detected_sub.get().landed) {
// already landed, no need to do anything, invalidate the position mission item.

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

@ -57,6 +57,7 @@ public:
void on_activation() override;
void on_active() override;
void on_inactive() override;
rtl_time_estimate_s calc_rtl_time_estimate() override;
@ -65,5 +66,7 @@ private:
void setActiveMissionItems() override;
void handleLanding(WorkItemType &new_work_item_type);
int _mission_index_prior_rtl{-1};
uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
};