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

mision: only run mission feasibility checks when mission updated

Browse Source 
Instead of also when geofence/safe points updated.
This prevents reporting multiple times.
This commit is contained in:
Beat Küng 2023-06-22 13:31:38 +02:00
parent 72955221cb
commit 133aeb10a6
4 changed files with 29 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/Mission.msg
View File

@ -4,5 +4,6 @@ uint8 dataman_id # default 0, there are two offboard storage places in the datam
uint16 count # count of the missions stored in the dataman uint16 count # count of the missions stored in the dataman
int32 current_seq # default -1, start at the one changed latest int32 current_seq # default -1, start at the one changed latest
int16 mission_update_counter # indicates updates to the mission, reload from dataman if increased
int16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased int16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased
int16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased int16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased

7
src/modules/mavlink/mavlink_mission.cpp
View File

@ -62,6 +62,7 @@ uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 };
int32_t MavlinkMissionManager::_current_seq = 0; int32_t MavlinkMissionManager::_current_seq = 0;
bool MavlinkMissionManager::_transfer_in_progress = false; bool MavlinkMissionManager::_transfer_in_progress = false;
constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; constexpr uint16_t MavlinkMissionManager::MAX_COUNT[];
uint16_t MavlinkMissionManager::_mission_update_counter = 0;
uint16_t MavlinkMissionManager::_geofence_update_counter = 0; uint16_t MavlinkMissionManager::_geofence_update_counter = 0;
uint16_t MavlinkMissionManager::_safepoint_update_counter = 0; uint16_t MavlinkMissionManager::_safepoint_update_counter = 0;
@ -146,6 +147,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
mission.dataman_id = dataman_id; mission.dataman_id = dataman_id;
mission.count = count; mission.count = count;
mission.current_seq = seq; mission.current_seq = seq;
mission.mission_update_counter = _mission_update_counter;
mission.geofence_update_counter = _geofence_update_counter; mission.geofence_update_counter = _geofence_update_counter;
mission.safe_points_update_counter = _safepoint_update_counter; mission.safe_points_update_counter = _safepoint_update_counter;
@ -869,6 +871,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
switch (_mission_type) { switch (_mission_type) {
case MAV_MISSION_TYPE_MISSION: case MAV_MISSION_TYPE_MISSION:
++_mission_update_counter;
/* alternate dataman ID anyway to let navigator know about changes */ /* alternate dataman ID anyway to let navigator know about changes */
if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) { if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) {
@ -1155,6 +1159,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
switch (_mission_type) { switch (_mission_type) {
case MAV_MISSION_TYPE_MISSION: case MAV_MISSION_TYPE_MISSION:
++_mission_update_counter;
update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq);
break; break;
@ -1209,6 +1214,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
switch (wpca.mission_type) { switch (wpca.mission_type) {
case MAV_MISSION_TYPE_MISSION: case MAV_MISSION_TYPE_MISSION:
++_mission_update_counter;
update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : 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);
break; break;
@ -1222,6 +1228,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
break; break;
case MAV_MISSION_TYPE_ALL: case MAV_MISSION_TYPE_ALL:
++_mission_update_counter;
update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : 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);
ret = update_geofence_count(0); ret = update_geofence_count(0);

1
src/modules/mavlink/mavlink_mission.h
View File

@ -132,6 +132,7 @@ private:
uORB::Publication<mission_s> _offboard_mission_pub{ORB_ID(mission)}; uORB::Publication<mission_s> _offboard_mission_pub{ORB_ID(mission)};
static uint16_t _mission_update_counter;
static uint16_t _geofence_update_counter; static uint16_t _geofence_update_counter;
static uint16_t _safepoint_update_counter; static uint16_t _safepoint_update_counter;

38
src/modules/navigator/mission.cpp
View File

@ -624,29 +624,31 @@ Mission::update_mission()
/* otherwise, just leave it */ /* otherwise, just leave it */
} }
check_mission_valid(true); if (old_mission.mission_update_counter != _mission.mission_update_counter) {
check_mission_valid(true);
failed = !_navigator->get_mission_result()->valid; failed = !_navigator->get_mission_result()->valid;
if (!failed) { if (!failed) {
/* reset mission failure if we have an updated valid mission */ /* reset mission failure if we have an updated valid mission */
_navigator->get_mission_result()->failure = false; _navigator->get_mission_result()->failure = false;
/* reset sequence info as well */ /* reset sequence info as well */
_navigator->get_mission_result()->seq_reached = -1; _navigator->get_mission_result()->seq_reached = -1;
_navigator->get_mission_result()->seq_total = _mission.count; _navigator->get_mission_result()->seq_total = _mission.count;
/* reset work item if new mission has been accepted */ /* reset work item if new mission has been accepted */
_work_item_type = WORK_ITEM_TYPE_DEFAULT; _work_item_type = WORK_ITEM_TYPE_DEFAULT;
_mission_changed = true; _mission_changed = true;
} }
/* check if the mission waypoints changed while the vehicle is in air /* 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 */ * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
if (((_mission.count != old_mission.count) || if (((_mission.count != old_mission.count) ||
(_mission.dataman_id != old_mission.dataman_id)) && (_mission.dataman_id != old_mission.dataman_id)) &&
!_navigator->get_land_detected()->landed) { !_navigator->get_land_detected()->landed) {
_mission_waypoints_changed = true; _mission_waypoints_changed = true;
}
} }
} else { } else {