From 87a7907a6326c62995aaf8441abe667fbf1e956b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 30 May 2016 10:08:46 +1000 Subject: [PATCH] Rover: move telemetry_delayed up into base class --- APMrover2/GCS_Mavlink.cpp | 18 +++--------------- APMrover2/GCS_Mavlink.h | 2 ++ 2 files changed, 5 insertions(+), 15 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 0af2b5d9e0..ab28e4006a 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -397,27 +397,15 @@ void Rover::send_current_waypoint(mavlink_channel_t chan) mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); } -// are we still delaying telemetry to try to avoid Xbee bricking? -bool Rover::telemetry_delayed(mavlink_channel_t chan) +uint32_t GCS_MAVLINK_Rover::telem_delay() const { - uint32_t tnow = millis() >> 10; - if (tnow > (uint32_t)g.telem_delay) { - return false; - } - if (chan == MAVLINK_COMM_0 && hal.gpio->usb_connected()) { - // this is USB telemetry, so won't be an Xbee - return false; - } - // we're either on the 2nd UART, or no USB cable is connected - // we need to delay telemetry by the TELEM_DELAY time - return true; + return (uint32_t)(rover.g.telem_delay); } - // try to send a message, return false if it won't fit in the serial tx buffer bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) { - if (rover.telemetry_delayed(chan)) { + if (telemetry_delayed(chan)) { return false; } diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h index 986bcb7198..ac3eb34158 100644 --- a/APMrover2/GCS_Mavlink.h +++ b/APMrover2/GCS_Mavlink.h @@ -11,6 +11,8 @@ public: protected: + uint32_t telem_delay() const override; + private: void handleMessage(mavlink_message_t * msg) override;