diff --git a/src/shared/drivers/canbus/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol.cpp
index 5f4fb440179ef5de65c8eaca061790985aa9d410..a1c8aade82ccd9ee2b246f69a9f21e7cdd668e18 100644
--- a/src/shared/drivers/canbus/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol.cpp
@@ -30,51 +30,55 @@ namespace Canbus
// For each board contains the packet that we are re-assembling
CanData data[N_BOARDS];
-CanProtocol::CanProtocol(CanbusDriver* can) { this->can = can; }
-
-CanProtocol::~CanProtocol() { (*can).~CanbusDriver(); }
-
-CanData CanProtocol::getPacket()
+CanProtocol::CanProtocol(CanbusDriver* can, MsgHandler callback)
{
- if (!buffer.isEmpty())
- return buffer.pop();
- else
- return {};
+ this->can = can;
+ this->callback = callback;
}
-bool CanProtocol::isBufferEmpty() { return buffer.isEmpty(); }
+CanProtocol::~CanProtocol() { delete can; }
-void CanProtocol::waitBufferNotEmpty() { buffer.waitUntilNotEmpty(); }
+void CanProtocol::send(CanData toSend) { TXbuffer.put(toSend); }
-void CanProtocol::sendData(CanData dataToSend)
+void CanProtocol::sendData()
{
- CanPacket packet = {};
- uint8_t tempLen = dataToSend.length - 1;
- uint32_t tempId = dataToSend.canId;
-
- // Send the first packet
- packet.ext = true;
- // create the id for the first packet
- packet.id = (tempId << shiftSequentialInfo) | (63 - (tempLen & leftToSend));
- packet.length = byteForInt(dataToSend.payload[0]);
- // Splits payload[0] in the right number of uint8_t
- for (int k = 0; k < packet.length; k++)
- packet.data[k] = dataToSend.payload[0] >> (8 * k);
- can->send(packet);
- tempLen--;
-
- for (int i = 1; i < dataToSend.length; i++)
+
+ while (true)
{
- // create the id for the remaining packets
- packet.id = (tempId << shiftSequentialInfo) | firstPacket |
- (63 - (tempLen & leftToSend));
- packet.length = byteForInt(dataToSend.payload[i]);
- // Splits payload[i] in the right number of uint8_t
- for (int k = 0; k < packet.length; k++)
- packet.data[k] = dataToSend.payload[i] >> (8 * k);
-
- can->send(packet);
- tempLen--;
+ TXbuffer.waitUntilNotEmpty();
+ if (!TXbuffer.IRQisEmpty())
+ {
+ CanData dataToSend = TXbuffer.pop();
+ CanPacket packet = {};
+ uint8_t tempLen = dataToSend.length - 1;
+ uint32_t tempId = dataToSend.canId;
+
+ // Send the first packet
+ packet.ext = true;
+ // create the id for the first packet
+ packet.id =
+ (tempId << shiftSequentialInfo) | (63 - (tempLen & leftToSend));
+ packet.length = byteForInt(dataToSend.payload[0]);
+ // Splits payload[0] in the right number of uint8_t
+ for (int k = 0; k < packet.length; k++)
+ packet.data[k] = dataToSend.payload[0] >> (8 * k);
+ can->send(packet);
+ tempLen--;
+
+ for (int i = 1; i < dataToSend.length; i++)
+ {
+ // create the id for the remaining packets
+ packet.id = (tempId << shiftSequentialInfo) | firstPacket |
+ (63 - (tempLen & leftToSend));
+ packet.length = byteForInt(dataToSend.payload[i]);
+ // Splits payload[i] in the right number of uint8_t
+ for (int k = 0; k < packet.length; k++)
+ packet.data[k] = dataToSend.payload[i] >> (8 * k);
+
+ can->send(packet);
+ tempLen--;
+ }
+ }
}
}
@@ -94,11 +98,11 @@ uint8_t CanProtocol::byteForInt(uint64_t number)
void CanProtocol::run()
{
+ std::thread send(std::bind(&CanProtocol::sendData, this));
while (!shouldStop())
{
// Wait for the next packet
can->getRXBuffer().waitUntilNotEmpty();
-
// If the buffer is not empty retrieve the packet
if (!can->getRXBuffer().isEmpty())
{
@@ -169,7 +173,7 @@ void CanProtocol::run()
data[sourceId].nRec != 0)
{
// We put the element of data in buffer
- buffer.put(data[sourceId]);
+ callback(data[sourceId]);
// Empties the struct
data[sourceId].canId = -1;
diff --git a/src/shared/drivers/canbus/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol.h
index ebbc7e05ae0df39ca87c7c3235225eda3ebef22c..d2be1c2efe69dc3fba64068530dd3e6e8b4d5a5e 100644
--- a/src/shared/drivers/canbus/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol.h
@@ -26,6 +26,8 @@
#include <utils/Debug.h>
#include <utils/collections/IRQCircularBuffer.h>
+#include <thread>
+
#include "Canbus.h"
#define N_BOARDS 3 ///< Number of boards on the bus.
@@ -115,14 +117,7 @@ struct CanData
*/
class CanProtocol : public ActiveObject
{
-private:
- // The physical implementation of the CanBus
- CanbusDriver* can;
-
- // The buffer used to store the completed CanData
- IRQCircularBuffer<CanData, N_BOARDS> buffer;
-
- miosix::FastMutex mutex;
+ using MsgHandler = std::function<void(CanData data)>;
public:
/**
@@ -130,22 +125,10 @@ public:
*
* @param can Pointer to a CanbusDriver object.
*/
- explicit CanProtocol(CanbusDriver* can);
+ explicit CanProtocol(CanbusDriver* can, MsgHandler callback);
~CanProtocol();
- /**
- * @brief Returns the first packet in the buffer.
- *
- * If buffer is empty return an empty packet.
- * @warning Should be called only after checking isEmpty()
- */
- CanData getPacket();
-
- bool isBufferEmpty();
-
- void waitBufferNotEmpty();
-
/**
* @brief Sends a CanData object on the bus.
*
@@ -155,7 +138,7 @@ public:
*
* @param data Contains the id and the data of the packet to send.
*/
- void sendData(CanData dataToSend);
+ void send(CanData toSend);
private:
/**
@@ -163,6 +146,8 @@ private:
*/
uint8_t byteForInt(uint64_t number);
+ void sendData();
+
/**
* @brief Keeps listening on the canbus for packets.
*
@@ -175,6 +160,16 @@ private:
* CanHandler.
*/
void run() override;
+
+ // The physical implementation of the CanBus
+ CanbusDriver* can;
+
+ // The buffer used to store the CanData to send
+ IRQCircularBuffer<CanData, N_BOARDS> TXbuffer;
+
+ miosix::FastMutex mutex;
+
+ MsgHandler callback;
};
} // namespace Canbus
diff --git a/src/tests/drivers/canbus/test-can-protocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp
index 598adf32b4e4c9a459fc4c22d54e38235a2d9e06..04b1fae84d928f8b93fb18d67fbea7f78c3a15f6 100644
--- a/src/tests/drivers/canbus/test-can-protocol.cpp
+++ b/src/tests/drivers/canbus/test-can-protocol.cpp
@@ -24,6 +24,7 @@
#include <drivers/canbus/CanProtocol.h>
+#include <functional>
#include <thread>
#include "drivers/canbus/BusLoadEstimation.h"
@@ -46,23 +47,10 @@ using CanRX = Gpio<GPIOA_BASE, 11>;
using CanTX = Gpio<GPIOA_BASE, 12>;
#endif
-#define SLP 5
+#define SLP 100
miosix::FastMutex mutex;
CanData toSend1;
CanData toSend2;
-void sendData(CanProtocol* protocol, CanData* toSend)
-{
- while (true)
- {
-
- // TRACE("send id %d\n", toSend->canId);
- {
- miosix::Lock<miosix::FastMutex> l(mutex);
- (*protocol).sendData(*toSend);
- }
- Thread::sleep(SLP);
- }
-}
bool equal(CanData* first, CanData* second)
{
if ((*first).canId != (*second).canId ||
@@ -78,6 +66,36 @@ bool equal(CanData* first, CanData* second)
return true;
}
+void print(CanData data)
+{
+ if ((!equal(&data, &toSend1) && !equal(&data, &toSend2)))
+ {
+ TRACE("Error\n");
+ TRACE("Received id %lu\n", data.canId);
+ TRACE("length %d\n", data.length);
+ for (int i = 0; i < data.length; i++)
+ {
+ TRACE("Received payload %d: %llu,\n", i, data.payload[i]);
+ }
+ }
+ else
+ {
+ TRACE("OK :) id %lu\n", data.canId);
+ }
+}
+void sendData(CanProtocol* protocol, CanData* toSend)
+{
+ while (true)
+ {
+
+ // TRACE("send id %d\n", toSend->canId);
+ {
+ miosix::Lock<miosix::FastMutex> l(mutex);
+ (*protocol).send(*toSend);
+ }
+ Thread::sleep(SLP);
+ }
+}
int main()
{
{
@@ -99,7 +117,8 @@ int main()
bt.baudRate = BAUD_RATE;
bt.samplePoint = SAMPLE_POINT;
CanbusDriver* c = new CanbusDriver(CAN1, cfg, bt);
- CanProtocol protocol(c);
+ CanProtocol protocol(c,
+ &print); // std::bind(&print, std::placeholders::_1));
// Allow every message
Mask32FilterBank f2(0, 0, 1, 1, 0, 0, 0);
@@ -117,7 +136,7 @@ int main()
toSend1.payload[5] = 3234;
toSend1.payload[6] = 12;
toSend1.payload[7] = 0;
- std::thread firstSend(sendData, &protocol, &toSend1);
+ // std::thread firstSend(sendData, &protocol, &toSend1);
Thread::sleep(10);
toSend2.canId = 0x100;
@@ -127,32 +146,15 @@ int main()
toSend2.payload[2] = 0x123ff;
toSend2.payload[3] = 1;
std::thread secondSend(sendData, &protocol, &toSend2);
+
+ /* code */
+
TRACE("start \n");
- int error = 0;
- for (int f = 0; f < 100000; f++)
+ while (true)
{
- protocol.waitBufferNotEmpty();
- CanData temp = protocol.getPacket();
+ protocol.send(toSend1);
+ Thread::sleep(SLP);
+
// TRACE("received packet \n");
- if ((!equal(&temp, &toSend1) && !equal(&temp, &toSend2)))
- {
- error++;
- TRACE("Error\n");
- TRACE("Received id %lu\n", temp.canId);
- TRACE("length %d\n", temp.length);
- for (int i = 0; i < temp.length; i++)
- {
- TRACE("Received payload %d: %llu,\n", i, temp.payload[i]);
- }
- }
- else
- {
- TRACE("OK :) id %lu\n", temp.canId);
- }
- }
- if (error != 0)
- {
- TRACE("Number of Error %d\n", error);
}
- free(c);
}