From 79a21ae57c1aacb600ca8e1af2f2c27a7bf020f0 Mon Sep 17 00:00:00 2001
From: Federico Mandelli <federicomandelli@skywarder.eu>
Date: Wed, 26 Jul 2023 13:26:47 +0200
Subject: [PATCH] [BusLoadEstimation] Corrected how we count for sent bit and
created a test for canProtocol
---
CMakeLists.txt | 7 +-
.../canbus/CanDriver/BusLoadEstimation.h | 16 +-
.../canbus/CanProtocol/CanProtocol.cpp | 4 +
.../drivers/canbus/CanProtocol/CanProtocol.h | 9 +-
.../canbus/CanDriver/test-can-2way.cpp | 5 +-
.../canbus/CanDriver/test-can-loopback.cpp | 4 +-
.../CanProtocol/test-can-protocol-2way.cpp | 222 ++++++++++++++++++
...col.cpp => test-can-protocol-loopback.cpp} | 0
8 files changed, 250 insertions(+), 17 deletions(-)
create mode 100644 src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp
rename src/tests/drivers/canbus/CanProtocol/{test-can-protocol.cpp => test-can-protocol-loopback.cpp} (100%)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5352d725d..29e422e7e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -200,8 +200,11 @@ sbs_target(test-can-filters stm32f429zi_skyward_pyxis_auxiliary)
add_executable(test-can-loopback src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp)
sbs_target(test-can-loopback stm32f429zi_skyward_death_stack_x)
-add_executable(test-can-protocol src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp)
-sbs_target(test-can-protocol stm32f429zi_skyward_death_stack_x)
+add_executable(test-can-protocol-loopback src/tests/drivers/canbus/CanProtocol/test-can-protocol-loopback.cpp)
+sbs_target(test-can-protocol-loopback stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-can-protocol-2way src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp)
+sbs_target(test-can-protocol-2way stm32f429zi_skyward_pyxis_auxiliary)
add_executable(test-dsgamma src/tests/drivers/test-dsgamma.cpp)
sbs_target(test-dsgamma stm32f429zi_stm32f4discovery)
diff --git a/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
index 9b76306eb..a68da6aeb 100644
--- a/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
+++ b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
@@ -38,18 +38,17 @@ namespace Boardcore
namespace Canbus
{
+struct BusLoadInfo
+{
+ float payloadBitRate;
+ float totalBitRate;
+ float loadPercent;
+};
class BusLoadEstimation
{
static constexpr uint16_t BUFFER_LEN = 20;
public:
- struct BusLoadInfo
- {
- float payloadBitRate;
- float totalBitRate;
- float loadPercent;
- };
-
BusLoadEstimation(uint32_t baudRate) : baudRate(baudRate) {}
void addPacket(CanPacket p)
@@ -75,8 +74,9 @@ public:
for (size_t i = 0; i < c.count(); ++i)
{
sizePayload += c.get(i).dataLength * 8;
+ sizeFrames += ((54 + c.get(i).dataLength * 8 - 1) / 4);
}
- sizeFrames = sizePayload + (64 + 8) * c.count();
+ sizeFrames += sizePayload + 64 * c.count();
return BusLoadInfo{(sizePayload / dt), (sizeFrames / dt),
((sizeFrames / dt) / baudRate) * 100};
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
index 69f660aa3..473a1f2c4 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
@@ -206,6 +206,10 @@ bool CanProtocol::enqueueSimplePacket(uint8_t priority, uint8_t primaryType,
return enqueueMsg(msg);
}
+BusLoadInfo CanProtocol::getLoad(){
+ return loadEstimator->getLoadInfo();
+}
+
void CanProtocol::runReceiver()
{
CanMessage msg;
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
index ac1998497..1c2942a45 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
@@ -137,12 +137,17 @@ public:
bool enqueueData(uint8_t priority, uint8_t primaryType, uint8_t source,
uint8_t destination, uint8_t secondaryType, const T& t);
+ /**
+ * @brief Return can load.
+ */
+ BusLoadInfo getLoad();
+
private:
/**
* @brief Blocking send function, puts the CanMessage object on the bus.
*
- * Takes a CanMessage object, splits it into multiple CanPackets with the
- * correct sequential id.
+ * Takes a CanMessage object, splits it into multiple CanPackets with
+ * the correct sequential id.
*
* @param msg Contains the id and the data of the packet to send.
*/
diff --git a/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp b/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
index 3fd1440dd..7d75b8b7e 100644
--- a/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
+++ b/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
@@ -174,9 +174,8 @@ public:
totalPackets, missedDeadline, lostPackets, res.mean,
res.maxValue, res.minValue, bufferFull);
- Canbus::BusLoadEstimation::BusLoadInfo info
- __attribute__((unused)) =
- canManager->getLoadSensor().getLoadInfo();
+ Canbus::BusLoadInfo info __attribute__((unused)) =
+ canManager->getLoadSensor().getLoadInfo();
printf(
"Payload rate: %.2f kbps, Frame rate: %.2f kbps, Load: "
"%.2f %%\n",
diff --git a/src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp b/src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp
index 06559020e..19d4f942b 100644
--- a/src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp
+++ b/src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp
@@ -64,8 +64,8 @@ public:
{
while (!shouldStop())
{
- long long start = miosix::getTick();
- BusLoadEstimation::BusLoadInfo info = ble.getLoadInfo();
+ long long start = miosix::getTick();
+ BusLoadInfo info = ble.getLoadInfo();
LOG_INFO(l,
"payload rate: {:.2f} kbps, total rate: {:.2f} kbps, "
"utilization: {:.2f}%",
diff --git a/src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp b/src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp
new file mode 100644
index 000000000..53ff65c4a
--- /dev/null
+++ b/src/tests/drivers/canbus/CanProtocol/test-can-protocol-2way.cpp
@@ -0,0 +1,222 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Luca Erbetta
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+// Sends packets on the canbus and waits for a response, measuring the response
+// time. If a response takes more than MSG_LOST_DEADLINE, the packet is
+// considered lost. It may also receive requests from other canbus devices, to
+// which it will respond
+
+#include <ActiveObject.h>
+#include <diagnostic/PrintLogger.h>
+#include <drivers/canbus/CanDriver/BusLoadEstimation.h>
+#include <drivers/canbus/CanDriver/CanDriver.h>
+#include <drivers/canbus/CanProtocol/CanProtocol.h>
+#include <miosix.h>
+#include <utils/Debug.h>
+#include <utils/Stats/Stats.h>
+#include <utils/collections/CircularBuffer.h>
+
+#include <functional>
+#include <string>
+
+constexpr uint32_t BAUD_RATE = 500 * 1000;
+constexpr float SAMPLE_POINT = 87.5f / 100.0f;
+constexpr uint32_t MSG_DEADLINE = 100; // ms
+constexpr uint32_t MSG_LOST_DEADLINE = 400; // ms
+
+using namespace std;
+using namespace placeholders;
+using namespace Boardcore;
+using namespace Canbus;
+using namespace miosix;
+
+CanProtocol* protocol;
+
+struct CanMsg
+{
+ uint32_t id;
+ uint32_t ts;
+};
+
+CircularBuffer<CanMsg, 64> msgs;
+FastMutex mutexMsgs;
+
+void handleCanMessage(CanMessage packet)
+{
+
+ if (packet.getPriority() == 0x00) // This is a request
+ {
+ PressureData temp = pressureDataFromCanMessage(packet);
+ // we resend the same packet
+ protocol->enqueueData(0x01, packet.getPrimaryType(), packet.getSource(),
+ packet.getDestination(),
+ packet.getSecondaryType(), temp);
+ }
+ else if (packet.getPriority() == 0x1) // This is a response to a request
+ {
+ Lock<FastMutex> l(mutexMsgs);
+
+ for (size_t i = 0; i < msgs.count(); ++i)
+ {
+ CanMsg& msg = msgs.get(i);
+ if (msg.id == packet.getPrimaryType())
+ {
+ msg.id = 0;
+ msg.ts = getTick() - msg.ts;
+ break;
+ }
+ }
+ }
+}
+
+uint8_t seq = 1;
+void sendNewRequest()
+{
+ CanMessage message;
+ uint8_t id;
+ if (seq >= 0x3F)
+ {
+ seq = 1;
+ }
+ id = seq;
+ seq++;
+
+ if (id % 2 == 0)
+ {
+ id = 0x3F - id;
+ }
+
+ {
+ Lock<FastMutex> l(mutexMsgs);
+ msgs.put({id, (uint32_t)getTick()});
+ }
+ PressureData f;
+ f.pressureTimestamp = 0x12345678;
+ f.pressure = 9876;
+ protocol->enqueueData(0x00, id, 0x01, 0x00, 0x00, f);
+}
+
+class MessageCollector : public ActiveObject
+{
+public:
+ void run() override
+ {
+ unsigned int c = 0;
+ while (!shouldStop())
+ {
+ uint32_t tick = (uint32_t)getTick();
+ {
+ Lock<FastMutex> l(mutexMsgs);
+ if (msgs.isFull())
+ {
+ ++bufferFull;
+ }
+ while (!msgs.isEmpty())
+ {
+ CanMsg msg = msgs.get();
+ if (msg.id == 0 || tick - msg.ts > MSG_LOST_DEADLINE)
+ {
+ msgs.pop();
+
+ ++totalPackets;
+ if (msg.id == 0)
+ {
+ msgstats.add(msg.ts * 1.0f);
+ if (msg.ts > MSG_DEADLINE)
+ {
+ ++missedDeadline;
+ }
+ }
+ else
+ {
+ // No response
+ ++lostPackets;
+ }
+ }
+ else
+ {
+ break;
+ }
+ }
+ }
+ if (c % 10 == 0)
+ {
+ StatsResult res __attribute__((unused)) = msgstats.getStats();
+
+ printf(
+ "Total packets: %u, Missed deadlines: %u, Lost packets: "
+ "%u, Mean ping: %.2f, "
+ "Max ping: %.0f, Min ping: %.0f, Buffer full: %u\n",
+ totalPackets, missedDeadline, lostPackets, res.mean,
+ res.maxValue, res.minValue, bufferFull);
+
+ Canbus::BusLoadInfo info __attribute__((unused)) =
+ protocol->getLoad();
+ printf(
+ "Payload rate: %.2f kbps, Frame rate: %.2f kbps, Load: "
+ "%.2f %%\n",
+ info.payloadBitRate / 1000.0f, info.totalBitRate / 1000.0f,
+ info.loadPercent);
+ }
+ ++c;
+ Thread::sleepUntil(tick + MSG_DEADLINE);
+ }
+ }
+
+private:
+ unsigned int totalPackets = 0;
+ unsigned int lostPackets = 0;
+ unsigned int missedDeadline = 0;
+ unsigned int bufferFull = 0;
+ Stats msgstats;
+};
+
+MessageCollector mc;
+
+int main()
+{
+ Logging::startAsyncLogger();
+
+ CanbusDriver::CanbusConfig cfg{};
+ cfg.nart = 0;
+ CanbusDriver::AutoBitTiming bt;
+ bt.baudRate = BAUD_RATE;
+ bt.samplePoint = SAMPLE_POINT;
+
+ CanbusDriver* c1 = new CanbusDriver(CAN1, cfg, bt);
+ CanbusDriver* c = new CanbusDriver(CAN2, cfg, bt);
+ protocol = new CanProtocol(c, bind(&handleCanMessage, _1), BAUD_RATE);
+
+ // Allow every message _1
+ Mask32FilterBank f2(0, 0, 0, 0, 0, 0, 0);
+ c->addFilter(f2);
+ c->init();
+ protocol->start();
+
+ mc.start();
+ const int slp = 20;
+ for (;;)
+ {
+ sendNewRequest();
+ Thread::sleep(slp);
+ }
+}
diff --git a/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp b/src/tests/drivers/canbus/CanProtocol/test-can-protocol-loopback.cpp
similarity index 100%
rename from src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
rename to src/tests/drivers/canbus/CanProtocol/test-can-protocol-loopback.cpp
--
GitLab