From 92adcb14e1c8c82170f7d721a93870482b6feb21 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Mon, 1 Apr 2024 21:09:41 +0100
Subject: [PATCH] AP_HAL: utility: Add Data Rate Limit helper

---
 libraries/AP_HAL/utility/DataRateLimit.cpp | 25 +++++++++++++++++++++
 libraries/AP_HAL/utility/DataRateLimit.h   | 26 ++++++++++++++++++++++
 2 files changed, 51 insertions(+)
 create mode 100644 libraries/AP_HAL/utility/DataRateLimit.cpp
 create mode 100644 libraries/AP_HAL/utility/DataRateLimit.h

diff --git a/libraries/AP_HAL/utility/DataRateLimit.cpp b/libraries/AP_HAL/utility/DataRateLimit.cpp
new file mode 100644
index 0000000000..a61259428b
--- /dev/null
+++ b/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);
+}
diff --git a/libraries/AP_HAL/utility/DataRateLimit.h b/libraries/AP_HAL/utility/DataRateLimit.h
new file mode 100644
index 0000000000..ec8f55e37a
--- /dev/null
+++ b/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;
+};