diff --git a/src/shared/drivers/canbus/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol.h index c692b7d051ce2bc8ecb28bd087e8ac3d39adf30b..de9f52462d0c5efaba73b3e7c4e08a1e7bc23caf 100644 --- a/src/shared/drivers/canbus/CanProtocol.h +++ b/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; - 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 + uint8_t tempLen = toSend.len - 1; + uint32_t tempId = toSend.canId; + packet.ext = true; + 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--; } @@ -167,23 +174,22 @@ protected: if (!can->getRXBuffer().isEmpty()) { - packet = can->getRXBuffer().pop().packet; - + 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(); diff --git a/src/tests/drivers/canbus/test-can-protocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp index 52627848383604022bfb6929ab8317a826a79d18..db56f7da23dff69915954e9d896fd61718794590 100644 --- a/src/tests/drivers/canbus/test-can-protocol.cpp +++ b/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"); + bt.baudRate = BAUD_RATE; + bt.samplePoint = SAMPLE_POINT; 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