From e7bd6a46cad3a264f2a375245f77b24834fc4d28 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Fri, 24 Jul 2015 00:31:30 -0700
Subject: [PATCH] Rover: queue MISSION_ITEM_REACHED

clean up unreachable code
---
 APMrover2/GCS_Mavlink.cpp    | 24 ++++++++++++++++++------
 APMrover2/Rover.h            |  1 +
 APMrover2/commands_logic.cpp |  8 +++++++-
 3 files changed, 26 insertions(+), 7 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 0209ab34c0..8b0e21d1d8 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -5,10 +5,6 @@
 // default sensors are present and healthy: gyro, accelerometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
 #define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
 
-// 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 (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_## id ##_LEN) return false
-
 void Rover::send_heartbeat(mavlink_channel_t chan)
 {
     uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
@@ -416,8 +412,6 @@ bool Rover::telemetry_delayed(mavlink_channel_t chan)
 // try to send a message, return false if it won't fit in the serial tx buffer
 bool GCS_MAVLINK::try_send_message(enum ap_message id)
 {
-    uint16_t txspace = comm_get_txspace(chan);
-
     if (rover.telemetry_delayed(chan)) {
         return false;
     }
@@ -590,6 +584,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         rover.send_pid_tuning(chan);
         break;
 
+    case MSG_MISSION_ITEM_REACHED:
+        CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED);
+        mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
+        break;
+
     case MSG_RETRY_DEFERRED:
     case MSG_TERRAIN:
     case MSG_OPTICAL_FLOW:
@@ -1321,6 +1320,19 @@ void Rover::gcs_send_message(enum ap_message id)
     }
 }
 
+/*
+ *  send a mission item reached message and load the index before the send attempt in case it may get delayed
+ */
+void Rover::gcs_send_mission_item_reached_message(uint16_t mission_index)
+{
+    for (uint8_t i=0; i<num_gcs; i++) {
+        if (gcs[i].initialised) {
+            gcs[i].mission_item_reached_index = mission_index;
+            gcs[i].send_message(MSG_MISSION_ITEM_REACHED);
+        }
+    }
+}
+
 /*
  *  send data streams in the given rate range on both links
  */
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 71eabdbc2f..d0a2e5fb7c 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -396,6 +396,7 @@ private:
     void send_statustext(mavlink_channel_t chan);
     bool telemetry_delayed(mavlink_channel_t chan);
     void gcs_send_message(enum ap_message id);
+    void gcs_send_mission_item_reached_message(uint16_t mission_index);
     void gcs_data_stream_send(void);
     void gcs_update(void);
     void gcs_send_text_P(gcs_severity severity, const prog_char_t *str);
diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp
index 5f910508d5..dc2a162e7b 100644
--- a/APMrover2/commands_logic.cpp
+++ b/APMrover2/commands_logic.cpp
@@ -124,10 +124,16 @@ void Rover::exit_mission()
 
 // verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
 //      we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
-bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
+bool Rover::verify_command_callback(const AP_Mission::Mission_Command& cmd)
 {
     if (control_mode == AUTO) {
         bool cmd_complete = verify_command(cmd);
+
+        // send message to GCS
+        if (cmd_complete) {
+            gcs_send_mission_item_reached_message(cmd.index);
+        }
+
         return cmd_complete;
     }
     return false;