diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4df2e09419a93d1a797f9f094a98c6c3e0c0301a..c7c74ae2dea01fdf9b725f65b7dcb8e1f3feb510 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -80,7 +80,7 @@ add_executable(test-sensormanager src/tests/test-sensormanager.cpp)
sbs_target(test-sensormanager stm32f429zi_skyward_death_stack_x)
add_executable(test-serial src/tests/test-serial.cpp)
-sbs_target(test-serial stm32f407vg_stm32f4discovery)
+sbs_target(test-serial stm32f429zi_stm32f4discovery)
add_executable(test-taskscheduler src/tests/scheduler/test-taskscheduler.cpp)
sbs_target(test-taskscheduler stm32f407vg_stm32f4discovery)
diff --git a/src/shared/drivers/canbus/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol.h
index cf78a0c341fd185d846b8fc077c48e8d1497552d..ae2384a0bc15a1bdc64ec462a05b755b7f755c4f 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];
@@ -88,25 +88,34 @@ 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(); }
+ 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,33 +125,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);
- TRACE("tosend len %d\n", toSend.len);
-
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);
}
- TRACE("packetlen %d\n, dato %d\n", packet.length, packet.data[0]);
can->send(packet);
tempLen--;
}
@@ -168,49 +172,60 @@ protected:
if (!can->getRXBuffer().isEmpty())
{
- packet = can->getRXBuffer().pop().packet;
-
- sourceId = packet.id & idMask.source;
- if (data[sourceId].canId == 0 ||
- (data[sourceId].canId & idMask.source) == sourceId)
+ packet = can->getRXBuffer().pop().packet;
+ sourceId = (packet.id & idMask.source) >> 15;
+ if (sourceId >= 0 &&
+ sourceId < NPACKET) // check for maximum size
{
- if (packet.id & idMask.firstPacket) // it is a first
- // packet of a data;
- {
- data[sourceId].len =
- (packet.id & idMask.leftToSend) + 1;
- data[sourceId].canId =
- packet.id >> 7; // discard the sequence number
- }
- TRACE("pakcet %d, nrec %d, left %lu\n", packet.data[0],
- data[sourceId].nRec, (packet.id & idMask.leftToSend));
- if ((data[sourceId].len - (data[sourceId].nRec + 1)) ==
- (packet.id & idMask.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
}
+ if ((data[sourceId].len - (data[sourceId].nRec + 1)) ==
+ leftToSend)
+ {
- data[sourceId]
- .payload[data[sourceId].len -
- (packet.id & idMask.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));
+ }
+ if (data[sourceId].len - leftToSend - 1 >= 0 &&
+ data[sourceId].len - leftToSend - 1 <
+ 32) // check for index
+ {
+
+ data[sourceId].payload[data[sourceId].len -
+ leftToSend - 1] =
+ tempPayload;
+ data[sourceId].nRec++;
+ }
+ }
- if (data[sourceId].nRec == data[sourceId].len)
- {
- mutex.lock();
- buffer.put(data[sourceId]);
- // empties the struct
- data[sourceId].canId = 0;
- 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 e2126819497d35abdf939256d70a0e4bd8fb3caf..9f8eef1ed68be96fccad479a789df19a701d2ba5 100644
--- a/src/tests/drivers/canbus/test-can-protocol.cpp
+++ b/src/tests/drivers/canbus/test-can-protocol.cpp
@@ -23,7 +23,6 @@
*/
#include <drivers/canbus/CanProtocol.h>
-#include <inttypes.h>
#include <thread>
@@ -49,21 +48,39 @@ using CanRX = Gpio<GPIOA_BASE, 11>;
using CanTX = Gpio<GPIOA_BASE, 12>;
#endif
-#define SLP 5000
-
-void sendData(CanProtocol* protocol, CanData toSend)
+#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);
}
}
+bool equal(CanData* first, CanData* second)
+{
+ if ((*first).canId != (*second).canId || (*first).len != (*second).len)
+ {
+ return false;
+ }
+ for (int i = 0; i < (*first).len; i++)
+ {
+ if ((*first).payload[i] != (*second).payload[i])
+ return false;
+ }
+ return true;
+}
int main()
{
-
{
miosix::FastInterruptDisableLock dLock;
@@ -78,12 +95,10 @@ int main()
CanTX::alternateFunction(9);
#endif
}
-
CanbusDriver::CanbusConfig cfg{};
CanbusDriver::AutoBitTiming bt;
- bt.baudRate = BAUD_RATE;
- bt.samplePoint = SAMPLE_POINT;
-
+ bt.baudRate = BAUD_RATE;
+ bt.samplePoint = SAMPLE_POINT;
CanbusDriver* c = new CanbusDriver(CAN1, cfg, bt);
CanProtocol protocol(c);
// Allow every message
@@ -92,40 +107,47 @@ int main()
c->addFilter(f2);
c->init();
protocol.start();
- CanData toSend;
- toSend.canId = 0x01;
- toSend.len = 3;
- toSend.payload[0] = 1;
- toSend.payload[1] = 2;
- toSend.payload[2] = 3;
- std::thread second(sendData, &protocol, toSend);
+
+ toSend1.canId = 0x200;
+ toSend1.len = 8;
+ toSend1.payload[0] = 0xffffffffffffffff;
+ toSend1.payload[1] = 2;
+ 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);
+
+ 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);
+ TRACE("start \n");
for (;;)
{
- TRACE("start \n");
+
protocol.waitEmpty();
CanData temp = protocol.getPacket();
- if (temp.canId != toSend.canId || temp.len != toSend.len ||
- temp.payload[0] != toSend.payload[0] ||
- temp.payload[1] != toSend.payload[1] ||
- temp.payload[2] != toSend.payload[2])
+ TRACE("received packet \n");
+ if ((!equal(&temp, &toSend1) && !equal(&temp, &toSend2)))
{
TRACE("Error\n");
- TRACE("Expected id %lu, received %lu\n", toSend.canId, temp.canId);
- TRACE("Expected len %d , received %d\n", toSend.len, temp.len);
- TRACE(
- "Expected payload 0 %llu , received "
- "%llu\n",
- toSend.payload[0], temp.payload[0]);
- TRACE("Expected payload 1 %llu, received %llu\n",
- toSend.payload[1], temp.payload[1]);
- TRACE(
- "Expected payload 2 %llu, received "
- "%llu\n",
- toSend.payload[2], temp.payload[2]);
+ 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,\n", i, temp.payload[i]);
+ }
}
else
{
- TRACE("OK :)\n");
+ TRACE("OK :) id %lu\n", temp.canId);
}
}
}