[CanProtocol] Changed CanProtocol to work more like MavlinkDriver

src/shared/drivers/canbus/CanProtocol.cpp

@@ -30,24 +30,25 @@ 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()
+{
 
-void CanProtocol::sendData(CanData dataToSend)
+    while (true)
     {
+        TXbuffer.waitUntilNotEmpty();
+        if (!TXbuffer.IRQisEmpty())
+        {
+            CanData dataToSend = TXbuffer.pop();
             CanPacket packet   = {};
             uint8_t tempLen    = dataToSend.length - 1;
             uint32_t tempId    = dataToSend.canId;
@@ -55,7 +56,8 @@ void CanProtocol::sendData(CanData dataToSend)
             // Send the first packet
             packet.ext = true;
             // create the id for the first packet
-    packet.id = (tempId << shiftSequentialInfo) | (63 - (tempLen & leftToSend));
+            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++)
@@ -77,6 +79,8 @@ void CanProtocol::sendData(CanData dataToSend)
                 tempLen--;
             }
         }
+    }
+}
 
 uint8_t CanProtocol::byteForInt(uint64_t number)
 {
@@ -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;

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

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);
 }