diff --git a/src/shared/drivers/canbus/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol.h
index de9f52462d0c5efaba73b3e7c4e08a1e7bc23caf..73d751d55c78800e69ed2692eb458e3f3d08da0f 100644
--- a/src/shared/drivers/canbus/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol.h
@@ -88,21 +88,19 @@ public:
getPacket() // return the packet, if buffer is empty return an empty packet
{
CanData temp;
- mutex.lock();
+ miosix::Lock<miosix::FastMutex> l(mutex);
if (!buffer.isEmpty())
{
temp = buffer.pop();
}
- mutex.unlock();
return temp;
}
bool isEmpty()
{
- mutex.lock();
+ miosix::Lock<miosix::FastMutex> l(mutex);
return buffer.isEmpty();
- mutex.unlock();
}
void waitEmpty() { buffer.waitUntilNotEmpty(); }
@@ -175,46 +173,63 @@ protected:
{
packet = can->getRXBuffer().pop().packet;
- sourceId = packet.id & idMask.source;
- if (data[sourceId].canId == -1 ||
- (data[sourceId].canId & idMask.source) == sourceId)
+ sourceId = (packet.id & idMask.source) >> 15;
+ // TRACE("source id %d\n", sourceId);
+ if (sourceId >= 0 &&
+ sourceId < NPACKET) // check for maximum size
{
- uint32_t leftToSend = 63 - (packet.id & idMask.leftToSend);
- if (packet.id & idMask.firstPacket) // it is a first
- // packet of a data;
- {
- data[sourceId].len = leftToSend + 1;
- data[sourceId].canId =
- packet.id >> 7; // discard the sequence number
- }
-
- if ((data[sourceId].len - (data[sourceId].nRec + 1)) ==
- leftToSend)
+ if (data[sourceId].canId == -1 ||
+ (((data[sourceId].canId << 7) & idMask.source) >> 15) ==
+ sourceId)
{
+ uint32_t leftToSend =
+ 63 - (packet.id & idMask.leftToSend);
- uint64_t tempPayload = 0;
- for (int f = 0; f < packet.length; f++)
+ if ((packet.id & idMask.firstPacket) >>
+ 6) // it is a first
+ // packet of a data;
{
- uint64_t tempData = packet.data[f];
- tempPayload = tempPayload | (tempData << (f * 8));
+
+ data[sourceId].len = leftToSend + 1;
+ data[sourceId].canId =
+ packet.id >> 7; // discard the sequence number
}
+ /*TRACE("len %d, nrec+1 %d, left to send %d\n",
+ data[sourceId].len, (data[sourceId].nRec + 1),
+ leftToSend);*/
+ if ((data[sourceId].len - (data[sourceId].nRec + 1)) ==
+ leftToSend)
+ {
- data[sourceId]
- .payload[data[sourceId].len - leftToSend - 1] =
- tempPayload;
- data[sourceId].nRec++;
- }
+ uint64_t tempPayload = 0;
+ for (int f = 0; f < packet.length; f++)
+ {
+ uint64_t tempData = packet.data[f];
+ tempPayload =
+ tempPayload | (tempData << (f * 8));
+ }
+ /*TRACE(
+ "sourceID %d, Payload index %d, tempPayload "
+ "%llu\n",
+ sourceId, data[sourceId].len - leftToSend - 1,
+ tempPayload);*/
+ // check for index
+ data[sourceId]
+ .payload[data[sourceId].len - leftToSend - 1] =
+ tempPayload;
+ data[sourceId].nRec++;
+ }
- if (data[sourceId].nRec == data[sourceId].len &&
- data[sourceId].nRec != 0)
- {
- mutex.lock();
- buffer.put(data[sourceId]);
- // empties the struct
- data[sourceId].canId = -1;
- data[sourceId].nRec = 0;
- data[sourceId].len = 0;
- mutex.unlock();
+ if (data[sourceId].nRec == data[sourceId].len &&
+ data[sourceId].nRec != 0)
+ {
+ miosix::Lock<miosix::FastMutex> l(mutex);
+ buffer.put(data[sourceId]);
+ // empties the struct
+ data[sourceId].canId = -1;
+ data[sourceId].nRec = 0;
+ data[sourceId].len = 0;
+ }
}
}
}
diff --git a/src/shared/drivers/canbus/Canbus.h b/src/shared/drivers/canbus/Canbus.h
index 96a686b9206069d6fac893ef38efb5c643769acb..b841096f29ade3f3ec47fb8af18198709075e4bf 100644
--- a/src/shared/drivers/canbus/Canbus.h
+++ b/src/shared/drivers/canbus/Canbus.h
@@ -64,13 +64,13 @@ public:
uint8_t awum = 1; // Automatic wakeup (1: automatic wakeup upon new
// message received)
uint8_t nart =
- 0; // No auto retransmission (0: packets are retrasmitted until
+ 0; // No auto retransmission (0: packets are retransmitted until
// success, 1: only one transfer attempt)
uint8_t abom = 1; // Automatic bus off management (1: automatically
// recovers from bus-off state)
uint8_t rflm = 1; // Receive fifo locked (0: new messages overwrite
// last ones, 1: new message discarded)
- uint8_t txfp = 0; // TX Fifo priority (0: identifier, 1: chronological)
+ uint8_t txfp = 1; // TX Fifo priority (0: identifier, 1: chronological)
};
/**
diff --git a/src/tests/drivers/canbus/test-can-protocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp
index db56f7da23dff69915954e9d896fd61718794590..9f8eef1ed68be96fccad479a789df19a701d2ba5 100644
--- a/src/tests/drivers/canbus/test-can-protocol.cpp
+++ b/src/tests/drivers/canbus/test-can-protocol.cpp
@@ -48,17 +48,23 @@ using CanRX = Gpio<GPIOA_BASE, 11>;
using CanTX = Gpio<GPIOA_BASE, 12>;
#endif
-#define SLP 5000
-
+#define SLP 100
+miosix::FastMutex mutex;
+CanData toSend1;
+CanData toSend2;
void sendData(CanProtocol* protocol, CanData* toSend)
{
while (true)
{
+
TRACE("send\n");
- (*protocol).sendCan(*toSend);
+ {
+ miosix::Lock<miosix::FastMutex> l(mutex);
+ (*protocol).sendCan(*toSend);
+ }
Thread::sleep(SLP);
}
-} // todo mchange the source id and send packet at the same time
+}
bool equal(CanData* first, CanData* second)
{
if ((*first).canId != (*second).canId || (*first).len != (*second).len)
@@ -97,11 +103,12 @@ int main()
CanProtocol protocol(c);
// Allow every message
Mask32FilterBank f2(0, 0, 0, 0, 0, 0, 0);
+
c->addFilter(f2);
c->init();
protocol.start();
- CanData toSend1;
- toSend1.canId = 0x01;
+
+ toSend1.canId = 0x200;
toSend1.len = 8;
toSend1.payload[0] = 0xffffffffffffffff;
toSend1.payload[1] = 2;
@@ -112,14 +119,15 @@ int main()
toSend1.payload[6] = 12;
toSend1.payload[7] = 0;
std::thread firstSend(sendData, &protocol, &toSend1);
- CanData toSend2;
+
+ Thread::sleep(10);
toSend2.canId = 0x100;
toSend2.len = 4;
toSend2.payload[0] = 0xffffff;
toSend2.payload[1] = 2;
toSend2.payload[2] = 0x123ff;
toSend2.payload[3] = 1;
- // std::thread secondSend(sendData, &protocol, &toSend2);
+ std::thread secondSend(sendData, &protocol, &toSend2);
TRACE("start \n");
for (;;)
{
@@ -127,20 +135,19 @@ int main()
protocol.waitEmpty();
CanData temp = protocol.getPacket();
TRACE("received packet \n");
- if (!equal(&temp, &toSend1))
+ if ((!equal(&temp, &toSend1) && !equal(&temp, &toSend2)))
{
TRACE("Error\n");
- TRACE("Received %lu, expected %lu\n", temp.canId, toSend1.canId);
- TRACE("Received %d, expected %d\n", temp.len, toSend1.len);
+ TRACE("Received %lu\n", temp.canId);
+ TRACE("Received %d\n", temp.len);
for (int i = 0; i < temp.len; i++)
{
- TRACE("Received payload %d: %llu, expected %llu\n", i,
- temp.payload[i], toSend1.payload[i]);
+ TRACE("Received payload %d: %llu,\n", i, temp.payload[i]);
}
}
else
{
- TRACE("OK :)\n");
+ TRACE("OK :) id %lu\n", temp.canId);
}
}
}