GCS_MAVLink: move telemetry_delayed up into base class

This commit is contained in:
Peter Barker 2016-05-30 09:54:11 +10:00 committed by Andrew Tridgell
parent 944541b287
commit 1e146256cc
3 changed files with 24 additions and 0 deletions

View File

@ -211,6 +211,10 @@ public:
// return current packet overhead for a channel
static uint8_t packet_overhead_chan(mavlink_channel_t chan);
// FIXME: move this to be private/protected once possible
bool telemetry_delayed(mavlink_channel_t chan);
virtual uint32_t telem_delay() const = 0;
protected:
bool waypoint_receiving; // currently receiving

View File

@ -1628,3 +1628,21 @@ float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num)
return 1.0f;
}
// are we still delaying telemetry to try to avoid Xbee bricking?
bool GCS_MAVLINK::telemetry_delayed(mavlink_channel_t _chan)
{
uint32_t tnow = AP_HAL::millis() >> 10;
if (tnow > 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;
}

View File

@ -19,6 +19,8 @@ public:
protected:
uint32_t telem_delay() const override { return 0; };
private:
void handleMessage(mavlink_message_t * msg) { }