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