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