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