diff --git a/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h index d635553acec07638bffa7aa20876123429fbe2de..9b76306ebda9b1a0de93ec1fcd42f16266c8dab7 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 c2714aedf1c103ccf7343572468b60ad4a1fbc99..69f660aa3936989c89e05754d75ca93284a8c44c 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 3df511ecd13d5e11a4387707a5cd0ec1c779a70a..8554c31767a172a0d27469b27e16a284735335bb 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"); };