aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: move try_send_message mission handling up

Browse Source
This commit is contained in:
Peter Barker 2017-07-18 18:55:06 +10:00 committed by Francisco Ferreira
parent dbdfce570a
commit 3b472ccc17
2 changed files with 41 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -289,6 +289,7 @@ protected:
// message sending functions:
bool try_send_compass_message(enum ap_message id);
bool try_send_mission_message(enum ap_message id);
private:

40
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -2054,6 +2054,38 @@ bool GCS_MAVLINK::try_send_compass_message(const enum ap_message id)
return ret;
}
bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id)
{
AP_Mission *mission = get_mission();
if (mission == nullptr) {
return true;
}
bool ret = true;
switch (id) {
case MSG_CURRENT_WAYPOINT:
CHECK_PAYLOAD_SIZE(MISSION_CURRENT);
mavlink_msg_mission_current_send(chan, mission->get_current_nav_index());
ret = true;
break;
case MSG_MISSION_ITEM_REACHED:
CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED);
mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
ret = true;
break;
case MSG_NEXT_WAYPOINT:
CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
queued_waypoint_send();
ret = true;
break;
default:
ret = true;
break;
}
return ret;
}
bool GCS_MAVLINK::try_send_message(const enum ap_message id)
{
if (telemetry_delayed()) {
@ -2064,6 +2096,14 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
switch(id) {
case MSG_CURRENT_WAYPOINT:
/* fall through */
case MSG_MISSION_ITEM_REACHED:
/* fall through */
case MSG_NEXT_WAYPOINT:
ret = try_send_mission_message(id);
break;
case MSG_MAG_CAL_PROGRESS:
/* fall through */
case MSG_MAG_CAL_REPORT: