[canbus-dev] finished testin, seems pretty stable, still needs test with multiple stm

src/shared/drivers/canbus/CanProtocol.h

@@ -56,8 +56,8 @@ namespace Canbus
  */
 struct CanData
 {
-    uint32_t canId =
-        0;  // the id of the can packet without the last 7 bits (sequence bit)
+    int32_t canId =
+        -1;  // the id of the can packet without the last 7 bits (sequence bit)
     uint8_t len;
     uint8_t nRec = 0;
     uint64_t payload[32];
@@ -107,6 +107,17 @@ public:
 
     void waitEmpty() { buffer.waitUntilNotEmpty(); }
 
+    uint8_t byteForInt(uint64_t number)
+    {
+        uint8_t i;
+        for (i = 1; i <= 8; i++)
+        {
+            number = number >> 8;
+            if (number == 0)
+                return i;
+        }
+        return i;
+    }
     /**
      * @brief Takes a canData, it splits it into single canpacket with the
      * correct sequential id
@@ -116,32 +127,28 @@ public:
     void sendCan(CanData toSend)  //@requires toSen to not be empty
     {
         CanPacket packet;
-        uint32_t tempLen = toSend.len - 1;
+        uint8_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
+        packet.id       = (tempId << 7) | idMask.firstPacket |
+                    (63 - (tempLen & idMask.leftToSend));
+        packet.length = byteForInt(toSend.payload[0]);
         for (int k = 0; k < packet.length; k++)
         {
             packet.data[k] = toSend.payload[0] >> (8 * k);
         }
         tempLen--;
-
         can->send(packet);
-
         for (int i = 1; i < toSend.len; i++)
         {
             tempId    = toSend.canId;
             packet.id = (tempId << 7) | !(idMask.firstPacket) |
-                        (tempLen & idMask.leftToSend);
-            packet.length = (toSend.payload[i] + 8) / 8;
+                        (63 - (tempLen & idMask.leftToSend));
+            packet.length = byteForInt(toSend.payload[i]);
             for (int k = 0; k < packet.length; k++)
             {
-                packet.data[k] = toSend.payload[i] << (8 * k);
+                packet.data[k] = toSend.payload[i] >> (8 * k);
             }
-
             can->send(packet);
             tempLen--;
         }
@@ -168,22 +175,21 @@ protected:
             {
 
                 packet   = can->getRXBuffer().pop().packet;
-
                 sourceId = packet.id & idMask.source;
-                if (data[sourceId].canId == 0 ||
+                if (data[sourceId].canId == -1 ||
                     (data[sourceId].canId & idMask.source) == sourceId)
                 {
+                    uint32_t leftToSend = 63 - (packet.id & idMask.leftToSend);
                     if (packet.id & idMask.firstPacket)  // it is a first
                                                          // packet of a data;
                     {
-                        data[sourceId].len =
-                            (packet.id & idMask.leftToSend) + 1;
+                        data[sourceId].len = leftToSend + 1;
                         data[sourceId].canId =
                             packet.id >> 7;  // discard the sequence number
                     }
 
                     if ((data[sourceId].len - (data[sourceId].nRec + 1)) ==
-                        (packet.id & idMask.leftToSend))
+                        leftToSend)
                     {
 
                         uint64_t tempPayload = 0;
@@ -194,8 +200,7 @@ protected:
                         }
 
                         data[sourceId]
-                            .payload[data[sourceId].len -
-                                     (packet.id & idMask.leftToSend) - 1] =
+                            .payload[data[sourceId].len - leftToSend - 1] =
                             tempPayload;
                         data[sourceId].nRec++;
                     }
@@ -206,7 +211,7 @@ protected:
                         mutex.lock();
                         buffer.put(data[sourceId]);
                         // empties the struct
-                        data[sourceId].canId = 0;
+                        data[sourceId].canId = -1;
                         data[sourceId].nRec  = 0;
                         data[sourceId].len   = 0;
                         mutex.unlock();

src/tests/drivers/canbus/test-can-protocol.cpp

@@ -48,7 +48,7 @@ using CanRX = Gpio<GPIOA_BASE, 11>;
 using CanTX = Gpio<GPIOA_BASE, 12>;
 #endif
 
-#define SLP 500
+#define SLP 5000
 
 void sendData(CanProtocol* protocol, CanData* toSend)
 {
@@ -89,28 +89,28 @@ int main()
         CanTX::alternateFunction(9);
 #endif
     }
-    TRACE("start \n");
     CanbusDriver::CanbusConfig cfg{};
     CanbusDriver::AutoBitTiming bt;
     bt.baudRate     = BAUD_RATE;
     bt.samplePoint  = SAMPLE_POINT;
-    TRACE("start \n");
     CanbusDriver* c = new CanbusDriver(CAN1, cfg, bt);
-    TRACE("start \n");
     CanProtocol protocol(c);
     // Allow every message
-    TRACE("start \n");
     Mask32FilterBank f2(0, 0, 0, 0, 0, 0, 0);
-    TRACE("start \n");
     c->addFilter(f2);
     c->init();
     protocol.start();
     CanData toSend1;
     toSend1.canId      = 0x01;
-    toSend1.len        = 3;
-    toSend1.payload[0] = 1;  // 0xffffffff;
+    toSend1.len        = 8;
+    toSend1.payload[0] = 0xffffffffffffffff;
     toSend1.payload[1] = 2;
-    toSend1.payload[2] = 3;  // 78022;
+    toSend1.payload[2] = 78022;
+    toSend1.payload[3] = 0xfffffffffffff;
+    toSend1.payload[4] = 23;
+    toSend1.payload[5] = 3234;
+    toSend1.payload[6] = 12;
+    toSend1.payload[7] = 0;
     std::thread firstSend(sendData, &protocol, &toSend1);
     CanData toSend2;
     toSend2.canId      = 0x100;
@@ -119,8 +119,7 @@ int main()
     toSend2.payload[1] = 2;
     toSend2.payload[2] = 0x123ff;
     toSend2.payload[3] = 1;
-    TRACE("start \n");
-    std::thread secondSend(sendData, &protocol, &toSend2);
+    // std::thread secondSend(sendData, &protocol, &toSend2);
     TRACE("start \n");
     for (;;)
     {
@@ -131,11 +130,12 @@ int main()
         if (!equal(&temp, &toSend1))
         {
             TRACE("Error\n");
-            TRACE("Received  %lu\n", temp.canId);
-            TRACE("Received %d\n", temp.len);
+            TRACE("Received  %lu, expected %lu\n", temp.canId, toSend1.canId);
+            TRACE("Received %d, expected %d\n", temp.len, toSend1.len);
             for (int i = 0; i < temp.len; i++)
             {
-                TRACE("Received payload %d:  %llu\n", i, temp.payload[i]);
+                TRACE("Received payload %d:  %llu, expected %llu\n", i,
+                      temp.payload[i], toSend1.payload[i]);
             }
         }
         else