From 1dd8b9d3630fcef604cad0a37186d2919f4f2f65 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 24 Jul 2015 00:18:14 -0700 Subject: [PATCH] GCS_MAVLink: add support for try send mission_item_reached also moved most of send_item_reached into common library --- libraries/GCS_MAVLink/GCS.h | 10 +++++++++- libraries/GCS_MAVLink/GCS_Common.cpp | 7 ------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index c74ae09d32..c4ac650ee6 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -18,6 +18,11 @@ #include #include +// check if a message will fit in the payload space available +#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) +#define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false +#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false + // GCS Message ID's /// NOTE: to ensure we never block on sending MAVLink messages /// please keep each MSG_ to a single MAVLink message. If need be @@ -61,6 +66,7 @@ enum ap_message { MSG_PID_TUNING, MSG_VIBRATION, MSG_RPM, + MSG_MISSION_ITEM_REACHED, MSG_RETRY_DEFERRED // this must be last }; @@ -124,6 +130,9 @@ public: // last time we got a non-zero RSSI from RADIO_STATUS static uint32_t last_radio_status_remrssi_ms; + // mission item index to be sent on queued msg, delayed or not + uint16_t mission_item_reached_index = AP_MISSION_CMD_INDEX_NONE; + // common send functions void send_meminfo(void); void send_power_status(void); @@ -142,7 +151,6 @@ public: void send_autopilot_version() const; void send_local_position(const AP_AHRS &ahrs) const; void send_vibration(const AP_InertialSensor &ins) const; - void send_mission_item_reached(uint16_t seq) const; // return a bitmap of active channels. Used by libraries to loop // over active channels to send to all active channels diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index aec4028719..2072bf0d9d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1353,10 +1353,3 @@ void GCS_MAVLINK::send_vibration(const AP_InertialSensor &ins) const #endif } -/* - * send notification that a mission command has been completed - */ -void GCS_MAVLINK::send_mission_item_reached(uint16_t seq) const -{ - mavlink_msg_mission_item_reached_send(chan, seq); -}