AP_HAL: utility: Add Data Rate Limit helper

libraries/AP_HAL/utility/DataRateLimit.cpp

@@ -0,0 +1,25 @@
+#include "DataRateLimit.h"
+
+#include <AP_HAL/AP_HAL.h>
+
+// Return the max number of bytes that can be sent since the last call given byte/s rate limit
+uint32_t DataRateLimit::max_bytes(const float bytes_per_sec)
+{
+    // Time since last call
+    const uint32_t now_us = AP_HAL::micros();
+    const float dt = (now_us - last_us) * 1.0e-6;
+    last_us = now_us;
+
+    // Maximum number of bytes that could be transferred in that time
+    float max_bytes = bytes_per_sec * dt;
+
+    // Add on the remainder from the last call, this prevents cumulative rounding errors
+    max_bytes += remainder;
+
+    // Get integer number of bytes and store the remainder
+    float max_bytes_int;
+    remainder = modf(max_bytes, &max_bytes_int);
+
+    // Add 0.5 to make sure the float rounds to the correct int
+    return uint32_t(max_bytes_int + 0.5);
+}

libraries/AP_HAL/utility/DataRateLimit.h

@@ -0,0 +1,26 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+
+// Returns the max number of bytes that can be sent since the last call given byte/s rate limit
+class DataRateLimit {
+public:
+    uint32_t max_bytes(const float bytes_per_sec);
+private:
+    uint32_t last_us;
+    float remainder;
+};