dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink_mission: fixed "clear mission" mechanism

This commit is contained in:
Igor Mišić 2021-03-29 11:04:56 +02:00
parent 11f0c5a3e6
commit f95c899d18
4 changed files with 49 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/mission.msg
View File

@ -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

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

@ -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;

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

@ -159,7 +159,7 @@ private:
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);

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

@ -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 {