From f23bd7e09d9b1811797b2a6523bb856aceeebe75 Mon Sep 17 00:00:00 2001 From: Leandro Pereira Date: Thu, 5 May 2016 17:45:37 -0300 Subject: [PATCH] GCS_MAVLink: Use a single stream_trigger() implementation This has no side effects, but since all implementations were basically the same, move the implementation to GCS_Common and the only part that adjusts the rate based on which which stream to each individual GCS_MAVLINK implementation. --- APMrover2/GCS_Mavlink.cpp | 30 +++------------------------ AntennaTracker/GCS_Mavlink.cpp | 30 ++++----------------------- ArduCopter/GCS_Mavlink.cpp | 31 +++------------------------- ArduPlane/GCS_Mavlink.cpp | 28 +++---------------------- libraries/GCS_MAVLink/GCS.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 27 ++++++++++++++++++++++++ 6 files changed, 42 insertions(+), 106 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index ed79e85b8f..7880d74b91 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -695,38 +695,14 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { AP_GROUPEND }; - -// see if we should send a stream now. Called at 50Hz -bool GCS_MAVLINK::stream_trigger(enum streams stream_num) +float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num) { - if (stream_num >= NUM_STREAMS) { - return false; - } - float rate = (uint8_t)streamRates[stream_num].get(); - - // send at a much lower rate while handling waypoints and - // parameter sends if ((stream_num != STREAM_PARAMS) && (waypoint_receiving || _queued_parameter != NULL)) { - rate *= 0.25f; + return 0.25f; } - if (rate <= 0) { - return false; - } - - if (stream_ticks[stream_num] == 0) { - // we're triggering now, setup the next trigger point - if (rate > 50) { - rate = 50; - } - stream_ticks[stream_num] = (50 / rate) - 1 + stream_slowdown; - return true; - } - - // count down at 50Hz - stream_ticks[stream_num]--; - return false; + return 1.0f; } void diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 3f40c4aafb..1530055f83 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -376,35 +376,13 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { AP_GROUPEND }; -// see if we should send a stream now. Called at 50Hz -bool GCS_MAVLINK::stream_trigger(enum streams stream_num) +float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num) { - if (stream_num >= NUM_STREAMS) { - return false; - } - float rate = (uint8_t)streamRates[stream_num].get(); - - // send at a much lower rate during parameter sends - if (_queued_parameter != NULL) { - rate *= 0.25f; + if (_queued_parameter != nullptr) { + return 0.25f; } - if (rate <= 0) { - return false; - } - - if (stream_ticks[stream_num] == 0) { - // we're triggering now, setup the next trigger point - if (rate > 50) { - rate = 50; - } - stream_ticks[stream_num] = (50 / rate) -1 + stream_slowdown; - return true; - } - - // count down at 50Hz - stream_ticks[stream_num]--; - return false; + return 1.0f; } void diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 98c747a555..58381e911b 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -840,38 +840,13 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { AP_GROUPEND }; - -// see if we should send a stream now. Called at 50Hz -bool GCS_MAVLINK::stream_trigger(enum streams stream_num) +float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num) { - if (stream_num >= NUM_STREAMS) { - return false; - } - float rate = (uint8_t)streamRates[stream_num].get(); - - // send at a much lower rate while handling waypoints and - // parameter sends if ((stream_num != STREAM_PARAMS) && (waypoint_receiving || _queued_parameter != NULL)) { - rate *= 0.25f; + return 0.25f; } - - if (rate <= 0) { - return false; - } - - if (stream_ticks[stream_num] == 0) { - // we're triggering now, setup the next trigger point - if (rate > 50) { - rate = 50; - } - stream_ticks[stream_num] = (50 / rate) - 1 + stream_slowdown; - return true; - } - - // count down at 50Hz - stream_ticks[stream_num]--; - return false; + return 1.0f; } void diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index e662bd1368..d2766e0537 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -978,38 +978,16 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { AP_GROUPEND }; - -// see if we should send a stream now. Called at 50Hz -bool GCS_MAVLINK::stream_trigger(enum streams stream_num) +float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num) { - if (stream_num >= NUM_STREAMS) { - return false; - } - float rate = (uint8_t)streamRates[stream_num].get(); - // send at a much lower rate while handling waypoints and // parameter sends if ((stream_num != STREAM_PARAMS) && (waypoint_receiving || _queued_parameter != NULL)) { - rate *= 0.25f; + return 0.25f; } - if (rate <= 0) { - return false; - } - - if (stream_ticks[stream_num] == 0) { - // we're triggering now, setup the next trigger point - if (rate > 50) { - rate = 50; - } - stream_ticks[stream_num] = (50 / rate) - 1 + stream_slowdown; - return true; - } - - // count down at 50Hz - stream_ticks[stream_num]--; - return false; + return 1.0f; } void diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 8a8b623374..7490e7d199 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -200,6 +200,8 @@ public: } private: + float adjust_rate_for_stream_trigger(enum streams stream_num); + void handleMessage(mavlink_message_t * msg); /// The stream we are communicating over diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 26e5d55077..2caf8e1e0a 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -622,6 +622,33 @@ void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *Data } } +// see if we should send a stream now. Called at 50Hz +bool GCS_MAVLINK::stream_trigger(enum streams stream_num) +{ + if (stream_num >= NUM_STREAMS) { + return false; + } + float rate = (uint8_t)streamRates[stream_num].get(); + + rate *= adjust_rate_for_stream_trigger(stream_num); + + if (rate <= 0) { + return false; + } + + if (stream_ticks[stream_num] == 0) { + // we're triggering now, setup the next trigger point + if (rate > 50) { + rate = 50; + } + stream_ticks[stream_num] = (50 / rate) - 1 + stream_slowdown; + return true; + } + + // count down at 50Hz + stream_ticks[stream_num]--; + return false; +} void GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *str)