From 99d55b0e3ef177dae70f6e665150424f5d833bcb Mon Sep 17 00:00:00 2001
From: Federico Mandelli <federicomandelli@skywarder.eu>
Date: Wed, 26 Jul 2023 11:14:51 +0200
Subject: [PATCH] [CanProtocol] Added BusLoadEstimation to CanProtocol

---
 .../canbus/CanDriver/BusLoadEstimation.h      |  3 ++-
 .../canbus/CanProtocol/CanProtocol.cpp        | 12 +++++++++++
 .../drivers/canbus/CanProtocol/CanProtocol.h  | 21 ++++++++++++++++++-
 3 files changed, 34 insertions(+), 2 deletions(-)

diff --git a/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
index d635553ac..9b76306eb 100644
--- a/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
+++ b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
@@ -40,7 +40,7 @@ namespace Canbus
 
 class BusLoadEstimation
 {
-    static constexpr uint16_t BUFFER_LEN = 100;
+    static constexpr uint16_t BUFFER_LEN = 20;
 
 public:
     struct BusLoadInfo
@@ -60,6 +60,7 @@ public:
 
     BusLoadInfo getLoadInfo()
     {
+        // implements a simple "moving avg"
         Lock<FastMutex> l(mutex);
         if (c.count() < 2)
         {
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
index c2714aedf..69f660aa3 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
@@ -31,8 +31,16 @@ namespace Canbus
 {
 
 CanProtocol::CanProtocol(CanbusDriver* can, MsgHandler onReceive)
+{
+    // We assume the bus to be configured at its max velocity
+    CanProtocol(can, onReceive, 500 * 1000);
+}
+
+CanProtocol::CanProtocol(CanbusDriver* can, MsgHandler onReceive,
+                         uint32_t baudRate)
     : can(can), onReceive(onReceive)
 {
+    loadEstimator = new BusLoadEstimation(baudRate);
 }
 
 bool CanProtocol::start()
@@ -138,6 +146,8 @@ void CanProtocol::sendMessage(const CanMessage& msg)
 
     // Send the first packet
     can->send(packet);
+    // Updates the loadEstimator
+    loadEstimator->addPacket(packet);
     leftToSend--;
 
     // Prepare the remaining packets
@@ -155,6 +165,8 @@ void CanProtocol::sendMessage(const CanMessage& msg)
             packet.data[k] = msg.payload[i] >> (8 * k);
 
         can->send(packet);
+        // Updates the loadEstimator
+        loadEstimator->addPacket(packet);
         leftToSend--;
     }
 }
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
index 3df511ecd..8554c3176 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
@@ -23,6 +23,7 @@
 #pragma once
 
 #include <diagnostic/PrintLogger.h>
+#include <drivers/canbus/CanDriver/BusLoadEstimation.h>
 #include <drivers/canbus/CanDriver/CanDriver.h>
 #include <utils/Debug.h>
 #include <utils/collections/SyncCircularBuffer.h>
@@ -54,8 +55,24 @@ public:
      * @brief Construct a new CanProtocol object.
      *
      * @param can Pointer to a CanbusDriver object.
+     *
+     * @param onReceive function to be called when a new message is received.
+     */
+
+    CanProtocol::CanProtocol(CanbusDriver* can, MsgHandler onReceive);
+
+    /**
+     * @brief Construct a new CanProtocol object.
+     *
+     * @param can Pointer to a CanbusDriver object.
+     *
+     * @param onReceive function to be called when a new message is received.
+     *
+     * @param baudRate used to calculate bus usage.
      */
-    CanProtocol(CanbusDriver* can, MsgHandler onReceive);
+
+    CanProtocol::CanProtocol(CanbusDriver* can, MsgHandler onReceive,
+                             uint32_t baudRate);
 
     /**
      * @brief Start the receiving and sending threads.
@@ -183,6 +200,8 @@ private:
 
     SyncCircularBuffer<CanMessage, 10> outQueue;
 
+    BusLoadEstimation* loadEstimator;
+
     PrintLogger logger = Logging::getLogger("canprotocol");
 };
 
-- 
GitLab