[CanProtocol] Added BusLoadEstimation to CanProtocol

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)
         {

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--;
     }
 }

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");
 };