From ce301a17ea98430770923650c2b862a7d19002ca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Thu, 23 Sep 2021 14:17:40 +0200 Subject: [PATCH] mission: don't validate old mission if the vehicle is armed --- msg/mission.msg | 1 + src/modules/navigator/mission.cpp | 22 ++++++++++++++++++---- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/msg/mission.msg b/msg/mission.msg index bc19b70dec..b4f4d27187 100644 --- a/msg/mission.msg +++ b/msg/mission.msg @@ -4,4 +4,5 @@ uint8 dataman_id # default 0, there are two offboard storage places in the datam uint16 count # count of the missions stored in the dataman int32 current_seq # default -1, start at the one changed latest +bool valid # true if mission passed feasibility checks bool clear_mission # true if mission is cleared in dataman \ No newline at end of file diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ba49534c90..6184718afa 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -500,6 +500,7 @@ Mission::update_mission() _mission.count = 0; _mission.current_seq = 0; _current_mission_index = 0; + _mission.valid = false; } else { @@ -581,11 +582,23 @@ Mission::restore_old_mission() _mission = _old_mission; - /* check if valid. It could be invalid if there is no previous mission. */ - check_mission_valid(true); - bool valid = _navigator->get_mission_result()->valid; + if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) && + _mission.valid) { - if (valid) { + /* if armed and valid don't check old mission validity again but update navigator */ + _navigator->get_mission_result()->valid = true; + _navigator->get_mission_result()->seq_total = _mission.count; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + _home_inited = _navigator->home_position_valid(); + find_mission_land_start(); + + } else { + /* if not armed check validity again in case we changed feasibility parameters */ + check_mission_valid(true); + } + + if (_mission.valid) { _mission_changed = true; _navigator->get_mission_result()->failure = false; @@ -1785,6 +1798,7 @@ Mission::check_mission_valid(bool force) _param_mis_dist_wps.get(), _navigator->mission_landing_required()); + _mission.valid = _navigator->get_mission_result()->valid; _navigator->get_mission_result()->seq_total = _mission.count; _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated();