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

mission_base: fix to set the end of mission item always, if the mission can't be properly loaded or started

Browse Source
This commit is contained in:
Konrad 2024-04-10 10:32:42 +02:00 committed by Silvan Fuhrer
parent cc11e1fbbf
commit 6a7555c005
2 changed files with 23 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -356,7 +356,7 @@ MissionBase::on_active()
void MissionBase::update_mission()
{
if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !_navigator->get_mission_result()->valid) {
if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !isMissionValid()) {
if (_land_detected_sub.get().landed) {
/* landed, refusing to take off without a mission */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff\t");
@ -441,24 +441,33 @@ MissionBase::advance_mission()
void
MissionBase::set_mission_items()
{
if (_is_current_planned_mission_item_valid) {
/* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */
loadCurrentMissionItem();
bool set_end_of_mission{false};
/* force vtol land */
if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
if (_is_current_planned_mission_item_valid && _mission_type == MissionType::MISSION_TYPE_MISSION && isMissionValid()) {
/* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */
if (loadCurrentMissionItem()) {
/* force vtol land */
if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
}
setActiveMissionItems();
} else {
set_end_of_mission = true;
}
setActiveMissionItems();
} else {
set_end_of_mission = true;
}
if (set_end_of_mission) {
setEndOfMissionItems();
_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
}
}
void MissionBase::loadCurrentMissionItem()
bool MissionBase::loadCurrentMissionItem()
{
const dm_item_t dm_item = static_cast<dm_item_t>(_mission.mission_dataman_id);
bool success = _dataman_cache.loadWait(dm_item, _mission.current_seq, reinterpret_cast<uint8_t *>(&_mission_item),
@ -469,6 +478,8 @@ void MissionBase::loadCurrentMissionItem()
events::send(events::ID("mission_item_set_failed"), events::Log::Error,
"Mission item could not be set");
}
return success;
}
void MissionBase::setEndOfMissionItems()

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

@ -250,8 +250,9 @@ protected:
* @brief Load current mission item
*
* Load current mission item from dataman cache.
* @return true, if the mission item could be loaded, false otherwise
*/
void loadCurrentMissionItem();
bool loadCurrentMissionItem();
/**
* Set the mission result