fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 16:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator/mavlink: fixes for mission item reached message (#5486)

Browse Source 
* navigator/mavlink: always send last reached item

Since we can't rely on mavlink that every message arrives, it makes
sense to continuously send the last reached waypoint.

* navigator: don't report reached for takeoff

If a takeoff waypoint has been inserted, we should not report that we
reached a mavlink mission item because we actually have not.
This commit is contained in:
Julian Oes
2016-09-13 21:52:28 +02:00
committed by Lorenz Meier
parent d2626d725d
commit 4453e4201b
4 changed files with 24 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+15 -2
View File
@@ -204,7 +204,12 @@ Mission::on_active()
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
set_mission_item_reached();
/* If we just completed a takeoff which was inserted before the right waypoint,
there is no need to report that we reached it because we didn't. */
if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
set_mission_item_reached();
}
if (_mission_item.autocontinue) {
/* switch to next waypoint if 'autocontinue' flag set */
@@ -266,6 +271,11 @@ Mission::update_onboard_mission()
_navigator->get_mission_result()->valid = true;
/* reset mission failure if we have an updated valid mission */
_navigator->get_mission_result()->mission_failure = false;
/* reset reached info as well */
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->seq_reached = 0;
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
@@ -310,6 +320,10 @@ Mission::update_offboard_mission()
if (!failed) {
/* reset mission failure if we have an updated valid mission */
_navigator->get_mission_result()->mission_failure = false;
/* reset reached info as well */
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->seq_reached = 0;
}
} else {
@@ -1206,7 +1220,6 @@ Mission::set_mission_item_reached()
void
Mission::set_current_offboard_mission_item()
{
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
_navigator->set_mission_result_updated();