diff --git a/src/shared/drivers/canbus/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol.cpp
index 1f405fabd1ba828a5656c8273d5dd8afee58f4e4..f48d0bba7b53d6fe17452f52ecfa6cfc9557bc6e 100644
--- a/src/shared/drivers/canbus/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol.cpp
@@ -27,8 +27,8 @@ namespace Boardcore
namespace Canbus
{
-
-CanData data[N_PACKET];
+// For each board contains the packet that we are re-assembling
+CanData data[N_BOARDS];
CanProtocol::CanProtocol(CanbusDriver* can) { this->can = can; }
@@ -60,9 +60,10 @@ void CanProtocol::sendData(CanData dataToSend)
// Send the first packet
packet.ext = true;
- packet.id = (tempId << shiftSequentialInfo) | firstPacket |
- (63 - (tempLen & leftToSend));
+ // 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);
@@ -71,10 +72,11 @@ void CanProtocol::sendData(CanData dataToSend)
for (int i = 1; i < dataToSend.length; i++)
{
tempId = dataToSend.canId;
- packet.id =
- (tempId << shiftSequentialInfo) | (63 - (tempLen & leftToSend));
+ // 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);
@@ -111,57 +113,67 @@ void CanProtocol::run()
// Discard the sequence number
uint32_t idNoSeq = packet.id >> shiftSequentialInfo;
+ // Extract the sourceID
uint8_t sourceId = (idNoSeq & source) >> shiftSource;
// Check for maximum size
- if (sourceId < N_PACKET)
+ if (sourceId < N_BOARDS)
{
- if (data[sourceId].canId == -1 ||
- ((data[sourceId].canId & source) >> shiftSource) ==
- sourceId)
+ uint8_t left = 63 - (packet.id & leftToSend);
+
+ // Check if it is the first packet in the sequence
+ if (((packet.id & firstPacket) >> shiftFirstPacket) == 0)
{
- uint8_t left = 63 - (packet.id & leftToSend);
+ // if it is we save the id (without the sequence number) the
+ // number of packet (left to send + 1)
+ data[sourceId].length = left + 1;
+
+ // the number of packet = left to send + 1 since it is the
+ // first packet
+ data[sourceId].canId = idNoSeq;
- // Check if it is the first packet in the sequence
- if ((packet.id & firstPacket) >> shiftFirstPacket)
+ // And we reset nRec
+ data[sourceId].nRec = 0;
+ }
+
+ // if the packet is expected, the length of data - the number of
+ // packet recorded +1 (+1 since we are not counting the last
+ // packet) equals the number of packet left to receive
+ if ((data[sourceId].length - (data[sourceId].nRec + 1)) == left)
+ {
+ uint64_t tempPayload = 0;
+
+ // we reassemble the payload
+ for (int f = 0; f < packet.length; f++)
{
- data[sourceId].length = left + 1;
- data[sourceId].canId = idNoSeq;
+ uint64_t tempData = packet.data[f];
+ tempPayload = tempPayload | (tempData << (f * 8));
}
- if ((data[sourceId].length - (data[sourceId].nRec + 1)) ==
- left)
+ if (data[sourceId].length - left - 1 >= 0 &&
+ data[sourceId].length - left - 1 <
+ 32) // check for index to avoid out of bounds error
{
- uint64_t tempPayload = 0;
-
- for (int f = 0; f < packet.length; f++)
- {
- uint64_t tempData = packet.data[f];
- tempPayload = tempPayload | (tempData << (f * 8));
- }
-
- if (data[sourceId].length - left - 1 >= 0 &&
- data[sourceId].length - left - 1 <
- 32) // check for index
- {
- data[sourceId]
- .payload[data[sourceId].length - left - 1] =
- tempPayload;
- data[sourceId].nRec++;
- }
+ // and put it in data
+ data[sourceId]
+ .payload[data[sourceId].length - left - 1] =
+ tempPayload;
+ data[sourceId].nRec++;
}
-
- if (data[sourceId].nRec == data[sourceId].length &&
- data[sourceId].nRec != 0)
+ }
+ // If we have received the right number of packet
+ if (data[sourceId].nRec == data[sourceId].length &&
+ data[sourceId].nRec != 0)
+ {
{
+ // We put the element of data in buffer
miosix::Lock<miosix::FastMutex> l(mutex);
buffer.put(data[sourceId]);
-
- // Empties the struct
- data[sourceId].canId = -1;
- data[sourceId].nRec = 0;
- data[sourceId].length = 0;
}
+
+ // Empties the struct
+ data[sourceId].canId = -1;
+ data[sourceId].length = 0;
}
}
}
diff --git a/src/shared/drivers/canbus/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol.h
index 7e9efa91f4269209c9ec6649f164ae8f6f6ba68a..c68c859dc127247d23f0ee155163b3c6c764cd15 100644
--- a/src/shared/drivers/canbus/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol.h
@@ -28,7 +28,7 @@
#include "Canbus.h"
-#define N_PACKET 3 ///< Number of boards on the bus.
+#define N_BOARDS 3 ///< Number of boards on the bus.
namespace Boardcore
{
@@ -58,29 +58,36 @@ namespace Canbus
* @brief Enumeration that contains masks of the elements composing the can
* packet id without sequential information.
*/
-enum IDMask
+enum IDMask : uint32_t
{
- priority = 0x3C0000,
- shiftPriority = 18,
- type = 0x03F000,
- shiftType = 12,
- source = 0x000F00,
- shiftSource = 8,
- destination = 0x0000F0,
- shiftDestination = 4,
- idType = 0x00000F,
- shiftIdType = 0
+ priority = 0x3C0000,
+
+ type = 0x03F000,
+ source = 0x000F00,
+ destination = 0x0000F0,
+ idType = 0x00000F
};
/**
* @brief @brief Enumeration that contains masks of the elements composing the
* sequential information.
*/
-enum SequentialInformation
+enum SequentialInformation : uint8_t
+{
+ firstPacket = 0x40,
+ leftToSend = 0x3F
+};
+
+enum ShiftInformation : uint8_t
{
- firstPacket = 0x40,
+ // Shift info of IDMask
+ shiftPriority = 18,
+ shiftType = 12,
+ shiftSource = 8,
+ shiftDestination = 4,
+ shiftIdType = 0,
+ // Shift info of SequentialInformation
shiftFirstPacket = 6,
- leftToSend = 0x3F,
shiftLeftToSend = 0,
shiftSequentialInfo = 7
};
@@ -109,11 +116,13 @@ struct CanData
class CanProtocol : public ActiveObject
{
private:
- // TODO: Add mutex and create get data in can protocol
+ // the physical implementation of the CanBus
+ CanbusDriver* can;
+
+ // the buffer used to store the completed CanData
+ IRQCircularBuffer<CanData, N_BOARDS> buffer;
+
miosix::FastMutex mutex;
- CanbusDriver* can; // the physical can
- IRQCircularBuffer<CanData, N_PACKET>
- buffer; // the buffer used to send data from CanProtocol to CanHandler
public:
/**
@@ -144,7 +153,7 @@ public:
* correct sequential id.
* @warning requires @param data to be not empty.
*
- * @param data Contains the id e the data of the packet to send.
+ * @param data Contains the id and the data of the packet to send.
*/
void sendData(CanData dataToSend);
@@ -158,8 +167,8 @@ private:
* @brief Keeps listening on the canbus for packets.
*
* Once a packet is received, it checks if it is expected (that id is
- * already present in data), if that is the case, it is added to the list.
- * Once we receive the correct amount of packet we send it to can handler.
+ * already present in data), if it is the case, it's added to the list.
+ * Once we receive the correct amount of packet we offer it in buffer.
*
* For now if a packet is missed/received in the wrong order the whole
* packet will be lost once we receive a new first packet without warning
diff --git a/src/tests/drivers/canbus/test-can-protocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp
index b370187437d0357c916bb6223e4e4ebeff0e03b4..171db2aee1d7c6fa8cf964879d9331817c08e175 100644
--- a/src/tests/drivers/canbus/test-can-protocol.cpp
+++ b/src/tests/drivers/canbus/test-can-protocol.cpp
@@ -57,7 +57,7 @@ void sendData(CanProtocol* protocol, CanData* toSend)
while (true)
{
- TRACE("send\n");
+ TRACE("send id %d\n", toSend->canId);
{
miosix::Lock<miosix::FastMutex> l(mutex);
(*protocol).sendData(*toSend);