diff --git a/CMakeLists.txt b/CMakeLists.txt
index 53273bb8481f7b3ac5c2385fd4505fd29a0f90fe..45d913d806bd9ad7d4bba4b9c43f4f020d4927c3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -159,6 +159,8 @@ sbs_target(test-triad-parafoil stm32f429zi_parafoil)
#-----------------------------------------------------------------------------#
# Tests - Drivers #
#-----------------------------------------------------------------------------#
+add_executable(test-can-protocol src/tests/drivers/canbus/test-can-protocol.cpp)
+sbs_target(est-can-protocol stm32f429zi_stm32f4discovery)
add_executable(test-canbus-loopback src/tests/drivers/canbus/test-canbus-loopback.cpp)
sbs_target(test-canbus-loopback stm32f429zi_stm32f4discovery)
diff --git a/src/shared/drivers/canbus/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol.h
index 1c69acf5cc3adfc006ef375b3da96e93ff2575dc..cf78a0c341fd185d846b8fc077c48e8d1497552d 100644
--- a/src/shared/drivers/canbus/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol.h
@@ -23,6 +23,7 @@
#pragma once
#include <ActiveObject.h>
+#include <utils/Debug.h>
#include <utils/collections/IRQCircularBuffer.h>
#include "Canbus.h"
@@ -70,22 +71,42 @@ struct CanData
class CanProtocol : public ActiveObject
{
private:
+ miosix::FastMutex
+ mutex; // todo add mutex and create get data in can protocol
CanbusDriver* can; // the physical can
- IRQCircularBuffer<CanData, NPACKET>*
+ IRQCircularBuffer<CanData, NPACKET>
buffer; // the buffer used to send data from CanProtocol to CanHandler
public:
/**
* @brief Construct a new CanProtocol object
* @param can CanbusDriver pointer.
- * @param The circular buffer
*/
- CanProtocol(CanbusDriver* can, IRQCircularBuffer<CanData, NPACKET>* buffer)
+ CanProtocol(CanbusDriver* can) { this->can = can; }
+
+ CanData
+ getPacket() // return the packet, if buffer is empty return an empty packet
+ {
+ CanData temp;
+ mutex.lock();
+ if (!buffer.isEmpty())
+ {
+ temp = buffer.pop();
+ }
+ mutex.unlock();
+
+ return temp;
+ }
+
+ bool isEmpty()
{
- this->can = can;
- this->buffer = buffer;
+ mutex.lock();
+ return buffer.isEmpty();
+ mutex.unlock();
}
+ void waitEmpty() { buffer.waitUntilNotEmpty(); }
+
/**
* @brief Takes a canData, it splits it into single canpacket with the
* correct sequential id
@@ -95,19 +116,35 @@ public:
void sendCan(CanData toSend) //@requires toSen to not be empty
{
CanPacket packet;
- packet.ext = true;
- packet.length = toSend.len;
- packet.id = (toSend.canId << 7) | idMask.firstPacket |
- (toSend.len & idMask.leftToSend);
- packet.data[0] = toSend.payload[0];
+ uint32_t tempLen = toSend.len - 1;
+ uint32_t tempId = toSend.canId;
+ packet.ext = true;
+ packet.id =
+ (tempId << 7) | idMask.firstPacket | (tempLen & idMask.leftToSend);
+ packet.length = (toSend.payload[0] + 8) /
+ 8; // simple formula for upper approximation
+ for (int k = 0; k < packet.length; k++)
+ {
+ packet.data[k] = toSend.payload[0] >> (8 * k);
+ }
+ tempLen--;
+
can->send(packet);
- uint8_t tempLen = toSend.len - 1;
+ TRACE("tosend len %d\n", toSend.len);
+
for (int i = 1; i < toSend.len; i++)
{
- packet.id =
- packet.id | !idMask.firstPacket | (tempLen & idMask.leftToSend);
+ tempId = toSend.canId;
+ packet.id = (tempId << 7) | !(idMask.firstPacket) |
+ (tempLen & idMask.leftToSend);
+ packet.length = (toSend.payload[i] + 8) / 8;
+ for (int k = 0; k < packet.length; k++)
+ {
+ packet.data[k] = toSend.payload[i] << (8 * k);
+ }
+ TRACE("packetlen %d\n, dato %d\n", packet.length, packet.data[0]);
+ can->send(packet);
tempLen--;
- packet.data[i] = toSend.payload[i];
}
}
@@ -130,45 +167,55 @@ protected:
can->getRXBuffer().waitUntilNotEmpty();
if (!can->getRXBuffer().isEmpty())
{
- packet = can->getRXBuffer().pop().packet;
- sourceId = packet.id & idMask.source;
+ packet = can->getRXBuffer().pop().packet;
+
+ sourceId = packet.id & idMask.source;
if (data[sourceId].canId == 0 ||
(data[sourceId].canId & idMask.source) == sourceId)
{
- if (sourceId & idMask.firstPacket) // it is a first
- // packet of a data;
+ if (packet.id & idMask.firstPacket) // it is a first
+ // packet of a data;
{
- data[sourceId].len = packet.id & idMask.leftToSend;
+ data[sourceId].len =
+ (packet.id & idMask.leftToSend) + 1;
data[sourceId].canId =
packet.id >> 7; // discard the sequence number
- // data[i].len = packet.length;
}
- if ((data[sourceId].len - data[sourceId].nRec) ==
+ TRACE("pakcet %d, nrec %d, left %lu\n", packet.data[0],
+ data[sourceId].nRec, (packet.id & idMask.leftToSend));
+ if ((data[sourceId].len - (data[sourceId].nRec + 1)) ==
(packet.id & idMask.leftToSend))
{
- uint64_t tempPayload;
+
+ uint64_t tempPayload = 0;
for (int f = 0; f < packet.length; f++)
{
- tempPayload =
- tempPayload & (packet.data[f] << (56 - f * 8));
+ uint64_t tempData = packet.data[f];
+ tempPayload = tempPayload | (tempData << (f * 8));
}
data[sourceId]
.payload[data[sourceId].len -
- (packet.id & idMask.leftToSend)] =
+ (packet.id & idMask.leftToSend) - 1] =
tempPayload;
data[sourceId].nRec++;
}
+
if (data[sourceId].nRec == data[sourceId].len)
{
- buffer->put(data[sourceId]);
+ mutex.lock();
+ buffer.put(data[sourceId]);
+ // empties the struct
+ data[sourceId].canId = 0;
+ data[sourceId].nRec = 0;
+ data[sourceId].len = 0;
+ mutex.unlock();
}
- break;
}
}
}
}
};
} // namespace Canbus
-} // namespace Boardcore
\ No newline at end of file
+} // namespace Boardcore
diff --git a/src/tests/drivers/canbus/test-canprotocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp
similarity index 75%
rename from src/tests/drivers/canbus/test-canprotocol.cpp
rename to src/tests/drivers/canbus/test-can-protocol.cpp
index 2ea486a4a473c2a463ca0c42eba396a1638e2ee7..e2126819497d35abdf939256d70a0e4bd8fb3caf 100644
--- a/src/tests/drivers/canbus/test-canprotocol.cpp
+++ b/src/tests/drivers/canbus/test-can-protocol.cpp
@@ -1,3 +1,5 @@
+
+
/* Copyright (c) 2022 Skyward Experimental Rocketry
* Author: Federico Mandelli
*
@@ -21,7 +23,9 @@
*/
#include <drivers/canbus/CanProtocol.h>
-#include <utils/Debug.h>
+#include <inttypes.h>
+
+#include <thread>
#include "drivers/canbus/BusLoadEstimation.h"
#include "drivers/canbus/Canbus.h"
@@ -45,6 +49,18 @@ using CanRX = Gpio<GPIOA_BASE, 11>;
using CanTX = Gpio<GPIOA_BASE, 12>;
#endif
+#define SLP 5000
+
+void sendData(CanProtocol* protocol, CanData toSend)
+{
+ while (true)
+ {
+ TRACE("send\n");
+ (*protocol).sendCan(toSend);
+ Thread::sleep(SLP);
+ }
+}
+
int main()
{
@@ -68,45 +84,48 @@ int main()
bt.baudRate = BAUD_RATE;
bt.samplePoint = SAMPLE_POINT;
- IRQCircularBuffer<CanData, NPACKET>* buffer;
CanbusDriver* c = new CanbusDriver(CAN1, cfg, bt);
- CanProtocol protocol(c, buffer);
-
+ CanProtocol protocol(c);
// Allow every message
Mask32FilterBank f2(0, 0, 0, 0, 0, 0, 0);
c->addFilter(f2);
c->init();
-
protocol.start();
-
- const int slp = 500;
CanData toSend;
toSend.canId = 0x01;
toSend.len = 3;
toSend.payload[0] = 1;
toSend.payload[1] = 2;
toSend.payload[2] = 3;
+ std::thread second(sendData, &protocol, toSend);
for (;;)
{
- protocol.sendCan(toSend);
- (*buffer).waitUntilNotEmpty();
- CanData temp = (*buffer).IRQpop();
+ TRACE("start \n");
+ protocol.waitEmpty();
+ CanData temp = protocol.getPacket();
if (temp.canId != toSend.canId || temp.len != toSend.len ||
temp.payload[0] != toSend.payload[0] ||
temp.payload[1] != toSend.payload[1] ||
temp.payload[2] != toSend.payload[2])
{
TRACE("Error\n");
- TRACE("Expected id %d , received %d\n", toSend.canId, temp.canId);
+ TRACE("Expected id %lu, received %lu\n", toSend.canId, temp.canId);
TRACE("Expected len %d , received %d\n", toSend.len, temp.len);
- TRACE("Expected payload 0 %d , received %d\n", toSend.payload[0],
- temp.payload[0]);
- TRACE("Expected payload 1 %d , received %d\n", toSend.payload[1],
- temp.payload[1]);
- TRACE("Expected payload 2 %d , received %d\n", toSend.payload[2],
- temp.payload[2]);
+ TRACE(
+ "Expected payload 0 %llu , received "
+ "%llu\n",
+ toSend.payload[0], temp.payload[0]);
+ TRACE("Expected payload 1 %llu, received %llu\n",
+ toSend.payload[1], temp.payload[1]);
+ TRACE(
+ "Expected payload 2 %llu, received "
+ "%llu\n",
+ toSend.payload[2], temp.payload[2]);
+ }
+ else
+ {
+ TRACE("OK :)\n");
}
- Thread::sleep(slp);
}
}