From 945639ac0faa71c337ac0b3f9bd9ee03a1343878 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Fri, 24 Jul 2015 01:10:04 -0700
Subject: [PATCH] Copter: implement try send mission_item_reached

clean up unreachable code
---
 ArduCopter/Copter.h           |  2 +-
 ArduCopter/GCS_Mavlink.cpp    | 35 ++++++++++++++++++-----------------
 ArduCopter/commands_logic.cpp | 19 ++-----------------
 3 files changed, 21 insertions(+), 35 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 05c73ba70f..eb8767fffa 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -588,10 +588,10 @@ 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_check_input(void);
     void gcs_send_text_P(gcs_severity severity, const prog_char_t *str);
-    void gcs_send_mission_item_reached(uint16_t seq);
     void do_erase_logs(void);
     void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
     void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index 22e7e815a0..4ef8b97e09 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -5,10 +5,6 @@
 // default sensors are present and healthy: gyro, accelerometer, barometer, 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_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | 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 Copter::gcs_send_heartbeat(void)
 {
     gcs_send_message(MSG_HEARTBEAT);
@@ -518,8 +514,6 @@ bool Copter::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 (copter.telemetry_delayed(chan)) {
         return false;
     }
@@ -738,6 +732,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         send_vibration(copter.ins);
         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:
         break; // just here to prevent a warning
     }
@@ -1841,6 +1840,19 @@ void Copter::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 Copter::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
  */
@@ -1900,14 +1912,3 @@ void Copter::gcs_send_text_fmt(const prog_char_t *fmt, ...)
     }
 }
 
-/*
- *  send mission_item_reached message to all GCSs
- */
-void Copter::gcs_send_mission_item_reached(uint16_t seq)
-{
-    for (uint8_t i=0; i<num_gcs; i++) {
-        if (gcs[i].initialised) {
-            gcs[i].send_mission_item_reached(seq);
-        }
-    }
-}
diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp
index c1c9f0f653..5ce1acbfcd 100644
--- a/ArduCopter/commands_logic.cpp
+++ b/ArduCopter/commands_logic.cpp
@@ -168,7 +168,7 @@ bool Copter::verify_command_callback(const AP_Mission::Mission_Command& cmd)
 
         // send message to GCS
         if (cmd_complete) {
-            gcs_send_mission_item_reached(cmd.index);
+            gcs_send_mission_item_reached_message(cmd.index);
         }
 
         return cmd_complete;
@@ -189,40 +189,31 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
     //
     case MAV_CMD_NAV_TAKEOFF:
         return verify_takeoff();
-        break;
 
     case MAV_CMD_NAV_WAYPOINT:
         return verify_nav_wp(cmd);
-        break;
 
     case MAV_CMD_NAV_LAND:
         return verify_land();
-        break;
 
     case MAV_CMD_NAV_LOITER_UNLIM:
         return verify_loiter_unlimited();
-        break;
 
     case MAV_CMD_NAV_LOITER_TURNS:
         return verify_circle(cmd);
-        break;
 
     case MAV_CMD_NAV_LOITER_TIME:
         return verify_loiter_time();
-        break;
 
     case MAV_CMD_NAV_RETURN_TO_LAUNCH:
         return verify_RTL();
-        break;
 
     case MAV_CMD_NAV_SPLINE_WAYPOINT:
         return verify_spline_wp(cmd);
-        break;
 
 #if NAV_GUIDED == ENABLED
     case MAV_CMD_NAV_GUIDED_ENABLE:
         return verify_nav_guided_enable(cmd);
-        break;
 #endif
 
     ///
@@ -230,29 +221,23 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
     ///
     case MAV_CMD_CONDITION_DELAY:
         return verify_wait_delay();
-        break;
 
     case MAV_CMD_CONDITION_DISTANCE:
         return verify_within_distance();
-        break;
 
     case MAV_CMD_CONDITION_CHANGE_ALT:
         return verify_change_alt();
-        break;
 
     case MAV_CMD_CONDITION_YAW:
         return verify_yaw();
-        break;
 
     case MAV_CMD_DO_PARACHUTE:
         // assume parachute was released successfully
         return true;
-        break;
 
     default:
-        // return true if we do not recognise the command so that we move on to the next command
+        // return true if we do not recognize the command so that we move on to the next command
         return true;
-        break;
     }
 }