From 93c9a3aba7d48fd89516406590868d54bb2c94bc Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Tue, 12 Jul 2022 12:08:13 +0000
Subject: [PATCH] [CanProtocol] Divided con into .cpp file
---
cmake/boardcore.cmake | 1 +
src/shared/drivers/canbus/CanProtocol.cpp | 173 ++++++++++++
src/shared/drivers/canbus/CanProtocol.h | 252 +++++-------------
.../drivers/canbus/test-can-protocol.cpp | 19 +-
4 files changed, 257 insertions(+), 188 deletions(-)
create mode 100644 src/shared/drivers/canbus/CanProtocol.cpp
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 49c7e5016..6521be856 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -46,6 +46,7 @@ foreach(OPT_BOARD ${BOARDS})
${SBS_BASE}/src/shared/drivers/adc/InternalADC.cpp
${SBS_BASE}/src/shared/drivers/canbus/Canbus.cpp
${SBS_BASE}/src/shared/drivers/canbus/CanInterrupt.cpp
+ ${SBS_BASE}/src/shared/drivers/canbus/CanProtocol.cpp
${SBS_BASE}/src/shared/drivers/i2c/stm32f2_f4_i2c.cpp
${SBS_BASE}/src/shared/drivers/interrupt/external_interrupts.cpp
${SBS_BASE}/src/shared/drivers/timer/PWM.cpp
diff --git a/src/shared/drivers/canbus/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol.cpp
new file mode 100644
index 000000000..1f405fabd
--- /dev/null
+++ b/src/shared/drivers/canbus/CanProtocol.cpp
@@ -0,0 +1,173 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Federico Mandelli
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "CanProtocol.h"
+
+namespace Boardcore
+{
+
+namespace Canbus
+{
+
+CanData data[N_PACKET];
+
+CanProtocol::CanProtocol(CanbusDriver* can) { this->can = can; }
+
+CanProtocol::~CanProtocol() { (*can).~CanbusDriver(); }
+
+CanData CanProtocol::getPacket()
+{
+ miosix::Lock<miosix::FastMutex> l(mutex);
+
+ if (!buffer.isEmpty())
+ return buffer.pop();
+ else
+ return {};
+}
+
+bool CanProtocol::isBufferEmpty()
+{
+ miosix::Lock<miosix::FastMutex> l(mutex);
+ return buffer.isEmpty();
+}
+
+void CanProtocol::waitBufferEmpty() { buffer.waitUntilNotEmpty(); }
+
+void CanProtocol::sendData(CanData dataToSend)
+{
+ CanPacket packet = {};
+ uint8_t tempLen = dataToSend.length - 1;
+ uint32_t tempId = dataToSend.canId;
+
+ // Send the first packet
+ packet.ext = true;
+ packet.id = (tempId << shiftSequentialInfo) | firstPacket |
+ (63 - (tempLen & leftToSend));
+ packet.length = byteForInt(dataToSend.payload[0]);
+ for (int k = 0; k < packet.length; k++)
+ packet.data[k] = dataToSend.payload[0] >> (8 * k);
+ can->send(packet);
+ tempLen--;
+
+ for (int i = 1; i < dataToSend.length; i++)
+ {
+ tempId = dataToSend.canId;
+ packet.id =
+ (tempId << shiftSequentialInfo) | (63 - (tempLen & leftToSend));
+ packet.length = byteForInt(dataToSend.payload[i]);
+
+ for (int k = 0; k < packet.length; k++)
+ packet.data[k] = dataToSend.payload[i] >> (8 * k);
+
+ can->send(packet);
+ tempLen--;
+ }
+}
+
+uint8_t CanProtocol::byteForInt(uint64_t number)
+{
+ uint8_t i;
+
+ for (i = 1; i <= 8; i++)
+ {
+ number >>= 8;
+ if (number == 0)
+ return i;
+ }
+
+ return i;
+}
+
+void CanProtocol::run()
+{
+ while (!shouldStop())
+ {
+ // Wait for the next packet
+ can->getRXBuffer().waitUntilNotEmpty();
+
+ // If the buffer is not empty retrieve the packet
+ if (!can->getRXBuffer().isEmpty())
+ {
+ CanPacket packet = can->getRXBuffer().pop().packet;
+
+ // Discard the sequence number
+ uint32_t idNoSeq = packet.id >> shiftSequentialInfo;
+ uint8_t sourceId = (idNoSeq & source) >> shiftSource;
+
+ // Check for maximum size
+ if (sourceId < N_PACKET)
+ {
+ 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)
+ {
+ data[sourceId].length = left + 1;
+ data[sourceId].canId = idNoSeq;
+ }
+
+ if ((data[sourceId].length - (data[sourceId].nRec + 1)) ==
+ left)
+ {
+ 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++;
+ }
+ }
+
+ if (data[sourceId].nRec == data[sourceId].length &&
+ 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].length = 0;
+ }
+ }
+ }
+ }
+ }
+}
+
+} // namespace Canbus
+
+} // namespace Boardcore
diff --git a/src/shared/drivers/canbus/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol.h
index a496f5799..7e9efa91f 100644
--- a/src/shared/drivers/canbus/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol.h
@@ -28,50 +28,53 @@
#include "Canbus.h"
-#define NPACKET 3 // equals the number of boards in the can system
+#define N_PACKET 3 ///< Number of boards on the bus.
namespace Boardcore
{
+
namespace Canbus
{
/**
- * The id of a can packet is composed of 29 bits
- * priority 4 bit
- * type 6 bit
- * source 4 bit
- * destination 4 bit
- * idType 4 bit
- * firstPacket 1 bit
- * leftToSend 6 bit
+ * The id of a can packet is composed of 29 bits and will be divided such as:
+ * - Priority 4 bit - priority \
+ * - Type 6 bit - type |
+ * - Source 4 bit - source | 22 bits
+ * - Destination 4 bit - destination |
+ * - Type id 4 bit - idType /
+ * - First packet flag 1 bit - firstPacket \ 7 bits
+ * - Remaining packets 6 bit - leftToSend /
* shiftNameOfField the number of shift needed to reach that field
*/
/**
- * @brief The mask of the ID without the sequential information (firstPacket and
- * leftToSend) CompleteID = (IDMask << shiftSequentialInfo)||
- * SequentialInformation
+ * @brief The mask of the ID without the sequential information.
+ *
+ * CompleteID = (IDMask << shiftSequentialInfo) || SequentialInformation
*/
/**
- * @brief enum that contains how the canId without sequential is composed
+ * @brief Enumeration that contains masks of the elements composing the can
+ * packet id without sequential information.
*/
enum IDMask
{
priority = 0x3C0000,
- shiftPriority = 18, // number of bit before priority
- type = 0x3F000,
+ shiftPriority = 18,
+ type = 0x03F000,
shiftType = 12,
- source = 0xF00,
+ source = 0x000F00,
shiftSource = 8,
- destination = 0xF0,
+ destination = 0x0000F0,
shiftDestination = 4,
- idType = 0xF,
+ idType = 0x00000F,
shiftIdType = 0
};
/**
- * @brief enum that contains how the Sequential information are composed
+ * @brief @brief Enumeration that contains masks of the elements composing the
+ * sequential information.
*/
enum SequentialInformation
{
@@ -81,199 +84,90 @@ enum SequentialInformation
shiftLeftToSend = 0,
shiftSequentialInfo = 7
};
+
/**
- * @brief Generic struct that contains a logical can packet
- * i.e. 1 accelerometer packet 3*4byte (acc: x,y,z)+timestamp, will be 4
+ * @brief Generic struct that contains a logical can packet.
+ *
+ * i.e. 1 accelerometer packet 3 * 4byte (acc: x,y,z) +timestamp, will be 4
* canPacket but a single canData.
*/
struct CanData
{
- int32_t canId =
- -1; // the id of the can packet without the last 7 bits (sequence bit)
- uint8_t len;
+ int32_t canId = -1; ///< Id of the packet without the sequential info.
+ uint8_t length;
uint8_t nRec = 0;
uint64_t payload[32];
-} data[NPACKET];
+};
/**
- * @brief Canbus protocol, given an initialized can this class takes care of
- * sending the multiple packet of CanData with the corresponding id and
- * receiving single CanPacket that are then reframed as one Candata.
+ * @brief Canbus protocol implementation.
+ *
+ * Given a can interface this class takes care of sending CanData packets
+ * segmented into multiple CanPackets and receiving single CanPackets that are
+ * then reframed as CanData.
*/
class CanProtocol : public ActiveObject
{
private:
- miosix::FastMutex
- mutex; // todo add mutex and create get data in can protocol
+ // TODO: Add mutex and create get data in can protocol
+ miosix::FastMutex mutex;
CanbusDriver* can; // the physical can
- IRQCircularBuffer<CanData, NPACKET>
+ IRQCircularBuffer<CanData, N_PACKET>
buffer; // the buffer used to send data from CanProtocol to CanHandler
public:
/**
- * @brief Construct a new CanProtocol object
- * @param can CanbusDriver pointer.
+ * @brief Construct a new CanProtocol object.
+ *
+ * @param can Pointer to a CanbusDriver object.
*/
- CanProtocol(CanbusDriver* can) { this->can = can; }
- /* Destructor */
- ~CanProtocol() { (*can).~CanbusDriver(); }
+ explicit CanProtocol(CanbusDriver* can);
+
+ ~CanProtocol();
/**
- * @brief return the packet, if buffer is empty return an empty packet
+ * @brief Returns the first packet in the buffer.
+ *
+ * If buffer is empty return an empty packet.
* @warning Should be called only after checking isEmpty()
*/
- CanData
- getPacket() // return the packet, if buffer is empty return an empty packet
- {
- CanData temp;
- miosix::Lock<miosix::FastMutex> l(mutex);
- if (!buffer.isEmpty())
- {
- temp = buffer.pop();
- }
-
- return temp;
- }
+ CanData getPacket();
- bool isEmpty()
- {
- miosix::Lock<miosix::FastMutex> l(mutex);
- return buffer.isEmpty();
- }
+ bool isBufferEmpty();
- void waitEmpty() { buffer.waitUntilNotEmpty(); }
+ void waitBufferEmpty();
/**
- * @brief Count the number of byte needed to encode a uint64_t number
+ * @brief Sends a CanData object on the bus.
+ *
+ * Takes a CanData object, splits it into multiple CanPackets with the
+ * correct sequential id.
+ * @warning requires @param data to be not empty.
+ *
+ * @param data Contains the id e the data of the packet to send.
*/
- 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;
- }
+ void sendData(CanData dataToSend);
+
+private:
/**
- * @brief Takes a canData, it splits it into single canpacket with the
- * correct sequential id
- * @param toSend = containing the id e the data of the packet to send
- * @warning requires toSend to be not empty
+ * @brief Count the number of bytes needed to encode a uint64_t number.
*/
- void sendCan(CanData toSend) //@requires toSen to not be empty
- {
- CanPacket packet;
- uint8_t tempLen = toSend.len - 1;
- uint32_t tempId = toSend.canId;
- packet.ext = true;
- packet.id = (tempId << shiftSequentialInfo) | firstPacket |
- (63 - (tempLen & 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 << shiftSequentialInfo) | !(firstPacket) |
- (63 - (tempLen & leftToSend));
- packet.length = byteForInt(toSend.payload[i]);
- for (int k = 0; k < packet.length; k++)
- {
- packet.data[k] = toSend.payload[i] >> (8 * k);
- }
- can->send(packet);
- tempLen--;
- }
- }
+ uint8_t byteForInt(uint64_t number);
-protected:
/**
- * @brief Keeps listening on hte canbus for packets, once received it checks
- * if they are expected (that id is already present in data), if they are
- * they are added to the list. once we receive the correct amount of packet
- * we send it to can handler.
+ * @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.
+ *
+ * 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
+ * CanHandler.
*/
- void run() override // 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 canhandler
- {
- uint32_t idNoSeq;
- uint8_t sourceId;
- CanPacket packet;
- // Infinite loop
- while (true)
- {
- can->getRXBuffer().waitUntilNotEmpty();
- if (!can->getRXBuffer().isEmpty())
- {
-
- packet = can->getRXBuffer().pop().packet;
- idNoSeq = packet.id >>
- shiftSequentialInfo; // discard the sequence number
- sourceId = (idNoSeq & source) >> shiftSource;
- if (sourceId >= 0 &&
- sourceId < NPACKET) // check for maximum size
- {
-
- if (data[sourceId].canId == -1 ||
- ((data[sourceId].canId & source) >> shiftSource) ==
- sourceId)
- {
- uint8_t left = 63 - (packet.id & leftToSend);
-
- if ((packet.id & firstPacket) >>
- shiftFirstPacket) // it is a first
- // packet of a data;
- {
- data[sourceId].len = left + 1;
- data[sourceId].canId = idNoSeq;
- }
- if ((data[sourceId].len - (data[sourceId].nRec + 1)) ==
- left)
- {
-
- 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 - left - 1 >= 0 &&
- data[sourceId].len - left - 1 <
- 32) // check for index
- {
-
- data[sourceId]
- .payload[data[sourceId].len - left - 1] =
- tempPayload;
- data[sourceId].nRec++;
- }
- }
-
- 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;
- }
- }
- }
- }
- }
- }
+ void run() override;
};
+
} // namespace Canbus
+
} // namespace Boardcore
diff --git a/src/tests/drivers/canbus/test-can-protocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp
index 9f8eef1ed..b37018743 100644
--- a/src/tests/drivers/canbus/test-can-protocol.cpp
+++ b/src/tests/drivers/canbus/test-can-protocol.cpp
@@ -60,18 +60,19 @@ void sendData(CanProtocol* protocol, CanData* toSend)
TRACE("send\n");
{
miosix::Lock<miosix::FastMutex> l(mutex);
- (*protocol).sendCan(*toSend);
+ (*protocol).sendData(*toSend);
}
Thread::sleep(SLP);
}
}
bool equal(CanData* first, CanData* second)
{
- if ((*first).canId != (*second).canId || (*first).len != (*second).len)
+ if ((*first).canId != (*second).canId ||
+ (*first).length != (*second).length)
{
return false;
}
- for (int i = 0; i < (*first).len; i++)
+ for (int i = 0; i < (*first).length; i++)
{
if ((*first).payload[i] != (*second).payload[i])
return false;
@@ -109,7 +110,7 @@ int main()
protocol.start();
toSend1.canId = 0x200;
- toSend1.len = 8;
+ toSend1.length = 8;
toSend1.payload[0] = 0xffffffffffffffff;
toSend1.payload[1] = 2;
toSend1.payload[2] = 78022;
@@ -122,25 +123,25 @@ int main()
Thread::sleep(10);
toSend2.canId = 0x100;
- toSend2.len = 4;
+ toSend2.length = 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 (;;)
{
-
- protocol.waitEmpty();
+ protocol.waitBufferEmpty();
CanData temp = protocol.getPacket();
TRACE("received packet \n");
if ((!equal(&temp, &toSend1) && !equal(&temp, &toSend2)))
{
TRACE("Error\n");
TRACE("Received %lu\n", temp.canId);
- TRACE("Received %d\n", temp.len);
- for (int i = 0; i < temp.len; i++)
+ TRACE("Received %d\n", temp.length);
+ for (int i = 0; i < temp.length; i++)
{
TRACE("Received payload %d: %llu,\n", i, temp.payload[i]);
}
--
GitLab