fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 11:43:35 +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/mavlink/mavlink_mission.cpp
+7
View File
@@ -63,6 +63,7 @@ int MavlinkMissionManager::_dataman_id = 0;
bool MavlinkMissionManager::_dataman_init = false;
unsigned MavlinkMissionManager::_count = 0;
int MavlinkMissionManager::_current_seq = 0;
int MavlinkMissionManager::_last_reached = -1;
bool MavlinkMissionManager::_transfer_in_progress = false;
#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
@@ -327,7 +328,10 @@ MavlinkMissionManager::send(const hrt_abstime now)
if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); }
if (mission_result.reached) {
_last_reached = mission_result.seq_reached;
send_mission_item_reached((uint16_t)mission_result.seq_reached);
} else {
_last_reached = -1;
}
send_mission_current(_current_seq);
@@ -341,6 +345,9 @@ MavlinkMissionManager::send(const hrt_abstime now)
} else {
if (_slow_rate_limiter.check(now)) {
send_mission_current(_current_seq);
if (_last_reached >= 0) {
send_mission_item_reached((uint16_t)_last_reached);
}
}
}
src/modules/mavlink/mavlink_mission.h
+2
View File
@@ -118,6 +118,8 @@ private:
static unsigned _count; ///< Count of items in active mission
static int _current_seq; ///< Current item sequence in active mission
static int _last_reached; ///< Last reached waypoint in active mission (-1 means nothing reached)
int _transfer_dataman_id; ///< Dataman storage ID for current transmission
unsigned _transfer_count; ///< Items count in current transmission
unsigned _transfer_seq; ///< Item sequence in current transmission