From 0b2cbbb677fe208b5fa6d9444d3c83b374d68ba1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 30 Aug 2012 09:03:01 +1000
Subject: [PATCH] ACM: added TELEM_DELAY to ArduCopter

---
 ArduCopter/GCS_Mavlink.pde | 33 +++++++++++++++++++++++++++------
 ArduCopter/Parameters.h    |  3 ++-
 ArduCopter/Parameters.pde  |  9 +++++++++
 ArduCopter/config.h        |  6 ------
 4 files changed, 38 insertions(+), 13 deletions(-)

diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 51dd1799bf..0871659823 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -464,15 +464,38 @@ static void NOINLINE send_statustext(mavlink_channel_t chan)
         pending_status.text);
 }
 
+// are we still delaying telemetry to try to avoid Xbee bricking?
+static bool telemetry_delayed(mavlink_channel_t chan)
+{
+    uint32_t tnow = millis() >> 10;
+    if (tnow > g.telem_delay) {
+        return false;
+    }
+#if USB_MUX_PIN > 0
+    if (chan == MAVLINK_COMM_0 && usb_connected) {
+        // this is an APM2 with USB telemetry
+        return false;
+    }
+    // we're either on the 2nd UART, or no USB cable is connected
+    // we need to delay telemetry
+    return true;
+#else
+    if (chan == MAVLINK_COMM_0) {
+        // we're on the USB port
+        return false;
+    }
+    // don't send telemetry yet
+    return true;
+#endif
+}
+
 
 // try to send a message, return false if it won't fit in the serial tx buffer
 static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
 {
     int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
 
-    if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
-        // defer any messages on the telemetry port for 1 second after
-        // bootup, to try to prevent bricking of Xbees
+    if (telemetry_delayed(chan)) {
         return false;
     }
 
@@ -676,9 +699,7 @@ static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uin
 
 void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
 {
-    if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
-        // don't send status MAVLink messages for 2 seconds after
-        // bootup, to try to prevent Xbee bricking
+    if (telemetry_delayed(chan)) {
         return;
     }
 
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index 8cd9e15489..793f7145ea 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -94,6 +94,7 @@ public:
         k_param_sysid_this_mav,
         k_param_sysid_my_gcs,
         k_param_serial3_baud,
+        k_param_telem_delay,
 
         //
         // 140: Sensor parameters
@@ -218,7 +219,7 @@ public:
     AP_Int16        sysid_this_mav;
     AP_Int16        sysid_my_gcs;
     AP_Int8         serial3_baud;
-
+    AP_Int8         telem_delay;
 
     AP_Int16        RTL_altitude;
     AP_Int8         sonar_enabled;
diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde
index c34be68ad2..6b5bc6fd05 100644
--- a/ArduCopter/Parameters.pde
+++ b/ArduCopter/Parameters.pde
@@ -27,6 +27,15 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(serial3_baud,   "SERIAL3_BAUD",     SERIAL3_BAUD/1000),
 
+    // @Param: TELEM_DELAY
+    // @DisplayName: Telemetry startup delay 
+    // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up
+    // @User: Standard
+    // @Units: seconds
+    // @Range: 0 10
+    // @Increment: 1
+    GSCALAR(telem_delay,            "TELEM_DELAY",     0),
+
     // @Param: ALT_HOLD_RTL
     // @DisplayName: RTL Altitude
     // @Description: This is the altitude the model will move to before Returning to Launch.  Set to zero to return at current altitude.
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 8538ad14b3..9062d286cd 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -1056,12 +1056,6 @@
  # define CLI_SLIDER_ENABLED DISABLED
 #endif
 
-// delay to prevent Xbee bricking, in milliseconds
-#ifndef MAVLINK_TELEMETRY_PORT_DELAY
- # define MAVLINK_TELEMETRY_PORT_DELAY 6000
-#endif
-
-
 // experimental mpu6000 DMP code
 #ifndef DMP_ENABLED
  # define DMP_ENABLED DISABLED