From 819c771da1dda379e0808ff4e933389733b9c5fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Wed, 3 Mar 2021 11:10:42 +0100 Subject: [PATCH 1/7] mission: added restore old mission if a new mission is not feasible --- src/modules/navigator/mission.cpp | 37 +++++++++++++++++++++++++------ src/modules/navigator/mission.h | 6 +++++ 2 files changed, 36 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 389aa795a26b..18304a03a1d1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -489,7 +489,8 @@ Mission::update_mission() * an existing ROI setting from previous missions */ _navigator->reset_vroi(); - const mission_s old_mission = _mission; + _old_mission = _mission; + _old_mission.current_seq = _current_mission_index; if (_mission_sub.copy(&_mission)) { /* determine current index */ @@ -528,8 +529,8 @@ Mission::update_mission() /* check if the mission waypoints changed while the vehicle is in air * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ - if (((_mission.count != old_mission.count) || - (_mission.dataman_id != old_mission.dataman_id)) && + if (((_mission.count != _old_mission.count) || + (_mission.dataman_id != _old_mission.dataman_id)) && !_navigator->get_land_detected()->landed) { _mission_waypoints_changed = true; } @@ -544,10 +545,15 @@ Mission::update_mission() PX4_WARN("mission check failed"); } - // reset the mission - _mission.count = 0; - _mission.current_seq = 0; - _current_mission_index = 0; + bool restored = restore_old_mission(); + + if (!restored) { + + // Mission can't be restored, reset the mission + _mission.count = 0; + _mission.current_seq = 0; + _current_mission_index = 0; + } } // find and store landing start marker (if available) @@ -556,6 +562,23 @@ Mission::update_mission() set_current_mission_item(); } +bool +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 (valid) { + _mission_changed = true; + _navigator->get_mission_result()->failure = false; + _current_mission_index = _old_mission.current_seq; + } + + return valid; +} void Mission::advance_mission() diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 308b2387283a..aa74c52772f3 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -110,6 +110,11 @@ class Mission : public MissionBlock, public ModuleParams */ void update_mission(); + /** + * Restore old mission if update mission was not successful + */ + bool restore_old_mission(); + /** * Move on to next mission item or switch to loiter */ @@ -247,6 +252,7 @@ class Mission : public MissionBlock, public ModuleParams uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */ mission_s _mission {}; + mission_s _old_mission {}; /**< old mission is used as a backup and for comparison with a new mission */ int32_t _current_mission_index{-1}; From 11f0c5a3e680580831d3781b466cef21a0628bc8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Thu, 4 Mar 2021 14:06:06 +0100 Subject: [PATCH 2/7] mavlink_mission: sync transfer_dataman_id with mission.cpp --- src/modules/mavlink/mavlink_mission.cpp | 34 +++++++++++++++++++++++-- src/modules/mavlink/mavlink_mission.h | 9 +++++++ src/modules/navigator/mission.cpp | 28 +++++++++++++++++++- 3 files changed, 68 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index b4b2ab0b73b9..7c8a8f94caef 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -943,8 +943,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) _transfer_partner_sysid = msg->sysid; _transfer_partner_compid = msg->compid; _transfer_count = wpc.count; - _transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission + _transfer_dataman_id = next_transfer_dataman_id(); _transfer_current_seq = -1; if (_mission_type == MAV_MISSION_TYPE_FENCE) { @@ -994,6 +993,37 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) } } +dm_item_t +MavlinkMissionManager::next_transfer_dataman_id() +{ + dm_item_t transfer_dataman_id = _dataman_id; + /* lock MISSION_STATE item */ + int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + + if (dm_lock_ret != 0) { + PX4_ERR("DM_KEY_MISSION_STATE lock failed"); + } + + mission_s mission_state; + int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + + /* unlock MISSION_STATE item */ + if (dm_lock_ret == 0) { + dm_unlock(DM_KEY_MISSION_STATE); + } + + if (ret == sizeof(mission_s)) { + _dataman_id = (dm_item_t)mission_state.dataman_id; + transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission + + } else { + PX4_ERR("Can't read DM_KEY_MISSION_STATE"); + } + + return transfer_dataman_id; +} + void MavlinkMissionManager::switch_to_idle_state() { diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 6740f0c69766..93847b3ff471 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -216,6 +216,15 @@ class MavlinkMissionManager void handle_mission_count(const mavlink_message_t *msg); + /** + * Helper function to determine next transfer dataman id. + * It reads DM_KEY_MISSION_STATE which stores previous valid dataman_id. + * Since mavlink_mission module and mission module communicate over dataman, + * mission module is restoring dataman_id for invalid mission to previous valid mission + * if valid mission exist. + */ + dm_item_t next_transfer_dataman_id(); + void handle_mission_item(const mavlink_message_t *msg); void handle_mission_item_int(const mavlink_message_t *msg); void handle_mission_item_both(const mavlink_message_t *msg); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 18304a03a1d1..485dc63b43a5 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -565,6 +565,8 @@ Mission::update_mission() bool Mission::restore_old_mission() { + bool success = false; + _mission = _old_mission; /* check if valid. It could be invalid if there is no previous mission. */ @@ -575,9 +577,33 @@ Mission::restore_old_mission() _mission_changed = true; _navigator->get_mission_result()->failure = false; _current_mission_index = _old_mission.current_seq; + + /* lock MISSION_STATE item */ + int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + + if (dm_lock_ret != 0) { + PX4_ERR("DM_KEY_MISSION_STATE lock failed"); + } + + /* Since mission is invalid restore DM_KEY_MISSION_STATE with old dataman_id. + * This helps mavlink_mission module to not overwrite the valid mission if two or more invalid missions are sent. + */ + int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &_mission, sizeof(mission_s)); + + /* unlock MISSION_STATE item */ + if (dm_lock_ret == 0) { + dm_unlock(DM_KEY_MISSION_STATE); + } + + if (res == sizeof(mission_s)) { + success = true; + + } else { + PX4_ERR("Can't write DM_KEY_MISSION_STATE. The next invalid mission will not be restored!"); + } } - return valid; + return success; } void From f95c899d18085e559e2ed3aa6327be7196552137 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Mon, 29 Mar 2021 11:04:56 +0200 Subject: [PATCH 3/7] mavlink_mission: fixed "clear mission" mechanism --- msg/mission.msg | 2 + src/modules/mavlink/mavlink_mission.cpp | 7 +-- src/modules/mavlink/mavlink_mission.h | 2 +- src/modules/navigator/mission.cpp | 72 ++++++++++++++----------- 4 files changed, 49 insertions(+), 34 deletions(-) diff --git a/msg/mission.msg b/msg/mission.msg index 70fc68ccf2c1..bc19b70dec9e 100644 --- a/msg/mission.msg +++ b/msg/mission.msg @@ -3,3 +3,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 clear_mission # true if mission is cleared in dataman \ No newline at end of file diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7c8a8f94caef..995b9557dd38 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -148,7 +148,7 @@ MavlinkMissionManager::load_safepoint_stats() * Publish mission topic to notify navigator about changes. */ int -MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq) +MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool clear_mission) { // We want to make sure the whole struct is initialized including padding before getting written by dataman. mission_s mission{}; @@ -156,6 +156,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun mission.dataman_id = dataman_id; mission.count = count; mission.current_seq = seq; + mission.clear_mission = clear_mission; /* update mission state in dataman */ @@ -1303,7 +1304,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) switch (wpca.mission_type) { case MAV_MISSION_TYPE_MISSION: ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, true); break; case MAV_MISSION_TYPE_FENCE: @@ -1316,7 +1317,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) case MAV_MISSION_TYPE_ALL: ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, true); ret = update_geofence_count(0) || ret; ret = update_safepoint_count(0) || ret; break; diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 93847b3ff471..0b6b8a82eafe 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -159,7 +159,7 @@ class MavlinkMissionManager void init_offboard_mission(); - int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq); + int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool clear_mission = false); /** store the geofence count to dataman */ int update_geofence_count(unsigned count); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 485dc63b43a5..c6844c3bff6b 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -482,7 +482,7 @@ void Mission::update_mission() { - bool failed = true; + bool failed = false; /* Reset vehicle_roi * Missions that do not explicitly configure ROI would not override @@ -493,46 +493,58 @@ Mission::update_mission() _old_mission.current_seq = _current_mission_index; if (_mission_sub.copy(&_mission)) { - /* determine current index */ - if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) { - _current_mission_index = _mission.current_seq; + + /* If _mission.clear_mission flag is set to true, mission is deleted from dataman. + * No mission validation needed. */ + if (_mission.clear_mission) { + _mission.count = 0; + _mission.current_seq = 0; + _current_mission_index = 0; } else { - /* if less items available, reset to first item */ - if (_current_mission_index >= (int)_mission.count) { - _current_mission_index = 0; - } else if (_current_mission_index < 0) { - /* if not initialized, set it to 0 */ - _current_mission_index = 0; + /* determine current index */ + if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) { + _current_mission_index = _mission.current_seq; + + } else { + /* if less items available, reset to first item */ + if (_current_mission_index >= (int)_mission.count) { + _current_mission_index = 0; + + } else if (_current_mission_index < 0) { + /* if not initialized, set it to 0 */ + _current_mission_index = 0; + } + + /* otherwise, just leave it */ } - /* otherwise, just leave it */ - } + check_mission_valid(true); - check_mission_valid(true); + failed = !_navigator->get_mission_result()->valid; - failed = !_navigator->get_mission_result()->valid; + if (!failed) { + /* reset mission failure if we have an updated valid mission */ + _navigator->get_mission_result()->failure = false; - if (!failed) { - /* reset mission failure if we have an updated valid mission */ - _navigator->get_mission_result()->failure = false; + /* reset sequence info as well */ + _navigator->get_mission_result()->seq_reached = -1; + _navigator->get_mission_result()->seq_total = _mission.count; - /* reset sequence info as well */ - _navigator->get_mission_result()->seq_reached = -1; - _navigator->get_mission_result()->seq_total = _mission.count; + /* reset work item if new mission has been accepted */ + _work_item_type = WORK_ITEM_TYPE_DEFAULT; + _mission_changed = true; + } - /* reset work item if new mission has been accepted */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - _mission_changed = true; - } + /* check if the mission waypoints changed while the vehicle is in air + * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ + if (((_mission.count != _old_mission.count) || + (_mission.dataman_id != _old_mission.dataman_id)) && + !_navigator->get_land_detected()->landed) { + _mission_waypoints_changed = true; + } - /* check if the mission waypoints changed while the vehicle is in air - * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ - if (((_mission.count != _old_mission.count) || - (_mission.dataman_id != _old_mission.dataman_id)) && - !_navigator->get_land_detected()->landed) { - _mission_waypoints_changed = true; } } else { From bb6bf89913e2cb9fd41c573359e4fd410b0d3437 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Thu, 22 Jul 2021 14:48:25 +0200 Subject: [PATCH 4/7] mavlink: fix for new dataman while reading transfer_dataman_id --- src/modules/mavlink/mavlink_mission.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 995b9557dd38..067b4f4cb273 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1018,8 +1018,13 @@ MavlinkMissionManager::next_transfer_dataman_id() transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission + } else if (ret == 0) { + //dataman is empty (new or formatted SD card) + transfer_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { - PX4_ERR("Can't read DM_KEY_MISSION_STATE"); + PX4_ERR("Dataman can't read DM_KEY_MISSION_STATE. The actual size of readout is %d, expected size is %d.", ret, + static_cast(sizeof(mission_s))); } return transfer_dataman_id; From 2877e74021dfee3e284e1878d39ed09f6befd786 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Thu, 19 Aug 2021 14:06:52 +0200 Subject: [PATCH 5/7] mavlink_mission: restore old sequence number when a new mission is infeasible --- src/modules/mavlink/mavlink_mission.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 067b4f4cb273..939c766f0184 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -487,6 +487,12 @@ MavlinkMissionManager::send() if (_mission_result_sub.update(&mission_result)) { + /* If an infeasible mission is uploaded and the old feasible mission exists item count could be wrong. + * This is restoring item count from the feasible mission. */ + if (_count[MAV_MISSION_TYPE_MISSION] != mission_result.seq_total) { + _count[MAV_MISSION_TYPE_MISSION] = mission_result.seq_total; + } + if (_current_seq != mission_result.seq_current) { _current_seq = mission_result.seq_current; From 37ddd095ad3abc4f530a03586547253d3f1707b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Fri, 20 Aug 2021 13:40:42 +0200 Subject: [PATCH 6/7] mission: reset current sequence to the first item for finished old mission --- src/modules/navigator/mission.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c6844c3bff6b..ba49534c903b 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -588,7 +588,14 @@ Mission::restore_old_mission() if (valid) { _mission_changed = true; _navigator->get_mission_result()->failure = false; - _current_mission_index = _old_mission.current_seq; + + /* For completed old mission reset sequence to the first item. */ + if (_old_mission.current_seq >= _old_mission.count) { + _current_mission_index = 0; + + } else { + _current_mission_index = _old_mission.current_seq; + } /* lock MISSION_STATE item */ int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); 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 7/7] 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 bc19b70dec9e..b4f4d2718758 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 ba49534c903b..6184718afa67 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();