From 9d67f67741efbfe4461df9f5244f613c1a701b76 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 30 Aug 2012 09:36:18 +1000
Subject: [PATCH] Rover: added TELEM_DELAY to rover code

---
 APMrover2/GCS_Mavlink.pde | 33 +++++++++++++++++++++++++++------
 APMrover2/Parameters.h    |  2 ++
 APMrover2/Parameters.pde  |  1 +
 APMrover2/config.h        |  5 -----
 4 files changed, 30 insertions(+), 11 deletions(-)

diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index b10c4a0c4f..b0fe69d127 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -487,15 +487,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)
 {
     int 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;
     }
 
@@ -703,9 +726,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/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 953fc7993d..03a34b9258 100644
--- a/APMrover2/Parameters.h
+++ b/APMrover2/Parameters.h
@@ -59,6 +59,7 @@ public:
 	k_param_sysid_this_mav,
 	k_param_sysid_my_gcs,
         k_param_serial3_baud,
+        k_param_telem_delay,
 
         // 120: Fly-by-wire control
         //
@@ -234,6 +235,7 @@ public:
 	AP_Int16    sysid_this_mav;
 	AP_Int16    sysid_my_gcs;
         AP_Int8	    serial3_baud;
+        AP_Int8     telem_delay;
 
         // Feed-forward gains
         //
diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde
index 2eb49ac7d8..c605d73b11 100644
--- a/APMrover2/Parameters.pde
+++ b/APMrover2/Parameters.pde
@@ -19,6 +19,7 @@ const AP_Param::Info var_info[] PROGMEM = {
 	GSCALAR(sysid_this_mav,         "SYSID_THISMAV",    MAV_SYSTEM_ID),
 	GSCALAR(sysid_my_gcs,           "SYSID_MYGCS",      255),
 	GSCALAR(serial3_baud,           "SERIAL3_BAUD",     SERIAL3_BAUD/1000),
+    GSCALAR(telem_delay,            "TELEM_DELAY",     0),
 	GSCALAR(kff_pitch_compensation, "KFF_PTCHCOMP",     PITCH_COMP),
 	GSCALAR(kff_rudder_mix,         "KFF_RDDRMIX",      RUDDER_MIX),
 	GSCALAR(kff_pitch_to_throttle,  "KFF_PTCH2THR",     P_TO_T),
diff --git a/APMrover2/config.h b/APMrover2/config.h
index 29aa181806..349219f8cd 100644
--- a/APMrover2/config.h
+++ b/APMrover2/config.h
@@ -890,11 +890,6 @@
 # define CLI_SLIDER_ENABLED ENABLED
 #endif
 
-// delay to prevent Xbee bricking, in milliseconds
-#ifndef MAVLINK_TELEMETRY_PORT_DELAY
-# define MAVLINK_TELEMETRY_PORT_DELAY 6000
-#endif
-
 // use this to disable gen-fencing
 #ifndef GEOFENCE_ENABLED
 # define GEOFENCE_ENABLED ENABLED