diff --git a/.vscode/settings.json b/.vscode/settings.json
index ba530265aee5551604bf78df79042eaae66e1dba..68d8988bcafbddb302ab2276396aab2f81057fc8 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -139,7 +139,9 @@
"Boardcorev",
"boudrate",
"Canbus",
+ "canprotocol",
"Carlucci",
+ "compid",
"Corigliano",
"CORTEXM",
"cpitch",
@@ -205,7 +207,10 @@
"LISR",
"logdecoder",
"Luca",
+ "Mandelli",
"Matteo",
+ "Mavlink",
+ "mavlinkdriver",
"MEKF",
"microcontrollers",
"MINC",
@@ -271,9 +276,12 @@
"TEIE",
"Terraneo",
"testsuite",
+ "THID",
"TMEIE",
+ "tparam",
"TSCPP",
"TSVREFE",
+ "Tweakable",
"txfp",
"TXIRQ",
"TXOK",
@@ -295,4 +303,4 @@
],
"cSpell.language": "en",
"cSpell.enabled": true
-}
\ No newline at end of file
+}
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3dec47d98c5ef5522710677cddbbb93df618f99d..1ff18054a313ad837a9647b44d283827b9cc7eac 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,7 +52,7 @@ add_executable(runcam-settings src/entrypoints/runcam-settings.cpp)
sbs_target(runcam-settings stm32f407vg_stm32f4discovery)
add_executable(sdcard-benchmark src/entrypoints/sdcard-benchmark.cpp)
-sbs_target(sdcard-benchmark stm32f205rc_skyward_ciuti)
+sbs_target(sdcard-benchmark stm32f429zi_skyward_death_stack_x)
#-----------------------------------------------------------------------------#
# Tests #
@@ -159,14 +159,18 @@ sbs_target(test-ada stm32f429zi_skyward_death_stack_v3)
#-----------------------------------------------------------------------------#
# Tests - Drivers #
#-----------------------------------------------------------------------------#
-add_executable(test-can-protocol src/tests/drivers/canbus/test-can-protocol.cpp)
-sbs_target(test-can-protocol stm32f429zi_skyward_pyxis_auxiliary)
-add_executable(test-canbus-loopback src/tests/drivers/canbus/test-canbus-loopback.cpp)
-sbs_target(test-canbus-loopback stm32f429zi_stm32f4discovery)
+add_executable(test-can-2way src/tests/drivers/canbus/CanDriver/test-can-2way.cpp)
+sbs_target(test-can-2way stm32f429zi_skyward_pyxis_auxiliary)
-add_executable(test-canbus-2way src/tests/drivers/canbus/test-canbus-2way.cpp)
-sbs_target(test-canbus-2way stm32f429zi_skyward_pyxis_auxiliary)
+add_executable(test-can-filters src/tests/drivers/canbus/CanDriver/test-can-filters.cpp)
+sbs_target(test-can-filters stm32f429zi_skyward_pyxis_auxiliary)
+
+add_executable(test-can-loopback src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp)
+sbs_target(test-can-loopback stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-can-protocol src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp)
+sbs_target(test-can-protocol stm32f429zi_skyward_death_stack_x)
add_executable(test-dma-spi src/tests/drivers/dma/test-dma-spi.cpp)
sbs_target(test-dma-spi stm32f429zi_stm32f4discovery)
diff --git a/src/shared/drivers/canbus/CanDriver/CanData.h b/src/shared/drivers/canbus/CanDriver/CanData.h
index 8123c8514b1e25e58ea88eb04d453bb742c7b509..97e583073d3f823b35b79ea7dde0df4c7e2c599d 100644
--- a/src/shared/drivers/canbus/CanDriver/CanData.h
+++ b/src/shared/drivers/canbus/CanDriver/CanData.h
@@ -54,7 +54,7 @@ struct CanPacket
uint32_t timestamp = 0;
uint32_t id;
- bool ext = false;
+ bool ext = false; ///< Whether to use extended packet id
bool rtr = false;
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
index a1c8aade82ccd9ee2b246f69a9f21e7cdd668e18..5f6189f22817e0f9811ffaa1b4650db14eed2c61 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
@@ -22,169 +22,242 @@
#include "CanProtocol.h"
+using namespace miosix;
+
namespace Boardcore
{
namespace Canbus
{
-// For each board contains the packet that we are re-assembling
-CanData data[N_BOARDS];
-CanProtocol::CanProtocol(CanbusDriver* can, MsgHandler callback)
+CanProtocol::CanProtocol(CanbusDriver* can, MsgHandler onReceive)
+ : can(can), onReceive(onReceive)
{
- this->can = can;
- this->callback = callback;
}
-CanProtocol::~CanProtocol() { delete can; }
+bool CanProtocol::start()
+{
+ stopFlag = false;
-void CanProtocol::send(CanData toSend) { TXbuffer.put(toSend); }
+ // Start sender (joinable thread)
+ if (!sndStarted)
+ {
+ sndThread = miosix::Thread::create(
+ sndLauncher, skywardStack(4 * 1024), miosix::MAIN_PRIORITY,
+ reinterpret_cast<void*>(this), miosix::Thread::JOINABLE);
-void CanProtocol::sendData()
-{
+ if (sndThread != nullptr)
+ sndStarted = true;
+ else
+ LOG_ERR(logger, "Could not start sender!");
+ }
- while (true)
+ // Start receiver
+ if (!rcvStarted)
{
- TXbuffer.waitUntilNotEmpty();
- if (!TXbuffer.IRQisEmpty())
- {
- CanData dataToSend = TXbuffer.pop();
- CanPacket packet = {};
- uint8_t tempLen = dataToSend.length - 1;
- uint32_t tempId = dataToSend.canId;
-
- // Send the first packet
- packet.ext = true;
- // 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);
- tempLen--;
-
- for (int i = 1; i < dataToSend.length; i++)
- {
- // 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);
-
- can->send(packet);
- tempLen--;
- }
- }
+ rcvThread = miosix::Thread::create(rcvLauncher, skywardStack(4 * 1024),
+ miosix::MAIN_PRIORITY,
+ reinterpret_cast<void*>(this));
+
+ if (rcvThread != nullptr)
+ rcvStarted = true;
+ else
+ LOG_ERR(logger, "Could not start receiver!");
}
+
+ if (sndStarted && rcvStarted)
+ LOG_DEBUG(logger, "Sender and receiver started");
+
+ return sndStarted && rcvStarted;
}
-uint8_t CanProtocol::byteForInt(uint64_t number)
+bool CanProtocol::isStarted() { return sndStarted && rcvStarted; }
+
+void CanProtocol::stop()
{
- uint8_t i;
+ stopFlag = true;
- for (i = 1; i <= 8; i++)
+ // Wait for sender to stop
+ sndThread->join();
+}
+
+bool CanProtocol::enqueueMsg(const CanMessage& msg)
+{
+ // Append the message to the queue
+ outQueue.put(msg);
+
+ // Update stats
+ // updateQueueStats(appended);
+
+ // Return always true because the circular buffer overrides current packets
+ // and can't get full.
+ return true;
+}
+
+void CanProtocol::sendMessage(const CanMessage& msg)
+{
+ CanPacket packet = {};
+ uint32_t leftToSend = msg.length - 1;
+
+ // Create the id for the first packet
+ packet.ext = true; // Use extended packet id
+
+ // The number of left to send packets
+ packet.id = static_cast<uint32_t>(msg.id) |
+ ((static_cast<uint32_t>(0x3F) - leftToSend) &
+ static_cast<uint32_t>(CanPacketIdMask::LEFT_TO_SEND));
+ packet.length = byteForUint64(msg.payload[0]);
+
+ // Splits payload[0] in the right number of uint8_t
+ for (int i = 0; i < packet.length; i++)
+ packet.data[i] = msg.payload[0] >> (8 * i);
+
+ // Send the first packet
+ can->send(packet);
+ leftToSend--;
+
+ // Prepare the remaining packets
+ for (int i = 1; i < msg.length; i++)
{
- number >>= 8;
- if (number == 0)
- return i;
- }
+ packet.id = static_cast<uint32_t>(msg.id) |
+ static_cast<uint32_t>(CanPacketIdMask::FIRST_PACKET_FLAG) |
+ ((static_cast<uint32_t>(0x3F) - leftToSend) &
+ static_cast<uint32_t>(CanPacketIdMask::LEFT_TO_SEND));
+ packet.length = byteForUint64(msg.payload[i]);
- return i;
+ // Splits payload[i] in the right number of uint8_t
+ for (int k = 0; k < packet.length; k++)
+ packet.data[k] = msg.payload[i] >> (8 * k);
+
+ can->send(packet);
+ leftToSend--;
+ }
}
-void CanProtocol::run()
+void CanProtocol::runReceiver()
{
- std::thread send(std::bind(&CanProtocol::sendData, this));
- while (!shouldStop())
+ CanMessage msg;
+ uint8_t nReceived = 0;
+
+ while (!stopFlag)
{
// 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;
+ CanPacket pkt = can->getRXBuffer().pop().packet;
- // Discard the sequence number
- uint32_t idNoSeq = packet.id >> shiftSequentialInfo;
- // Extract the sourceID
- uint8_t sourceId = (idNoSeq & source) >> shiftSource;
+ uint8_t leftToReceive =
+ static_cast<uint32_t>(0x3F) -
+ (pkt.id & static_cast<uint32_t>(CanPacketIdMask::LEFT_TO_SEND));
- // Check for maximum size
- if (sourceId < N_BOARDS)
+ // Check if the packet is the first in the sequence, if this is the
+ // case then the previous message is overriden
+ if ((pkt.id & static_cast<uint32_t>(
+ CanPacketIdMask::FIRST_PACKET_FLAG)) == 0)
{
+ // If it is we save the id (without the sequence number) and the
+ // message length
+ msg.id = pkt.id & static_cast<uint32_t>(
+ CanPacketIdMask::MESSAGE_INFORMATION);
+ msg.length = leftToReceive + 1;
+
+ // Reset the number of received packets
+ nReceived = 0;
+ }
- uint8_t left = 63 - (packet.id & leftToSend);
+ // Accept the packet only if it has the expected id
+ // clang-format off
+ if (msg.id != -1 &&
+ (pkt.id & static_cast<uint32_t>(CanPacketIdMask::MESSAGE_INFORMATION)) ==
+ (msg.id & static_cast<uint32_t>(CanPacketIdMask::MESSAGE_INFORMATION)))
+ // clang-format on
+ {
+ // Check if the packet is expected in the sequence. The received
+ // packet must have the expected left to send value
- // Check if it is the first packet in the sequence
- if (((packet.id & firstPacket) >> shiftFirstPacket) == 0)
+ if (msg.length - nReceived - 1 == leftToReceive)
{
- // if it is we save the id (without the sequence number)
- // the number of packet (left to send + 1)
- data[sourceId].length = left + 1;
+ uint64_t payload = 0;
- // the number of packet = left to send + 1 since it is
- // the first packet
- data[sourceId].canId = idNoSeq;
+ // Assemble the packet data into a uint64_t
+ for (uint8_t i = 0; i < pkt.length; i++)
+ payload |= pkt.data[i] << (i * 8);
- // And we reset nRec
- data[sourceId].nRec = 0;
+ // Add the data to the message
+ msg.payload[pkt.length - leftToReceive - 1] = payload;
+ nReceived++;
}
+ }
- // we accept the packet only if we already received a first
- // packet (it is already in data)
- if (data[sourceId].canId == -1 ||
- ((data[sourceId].canId & source) >> shiftSource) ==
- sourceId)
- {
- // 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++)
- {
- 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 to avoid out of bounds
- // error
- {
- // and put it in data
- data[sourceId]
- .payload[data[sourceId].length - left - 1] =
- tempPayload;
- data[sourceId].nRec++;
- }
- }
- // 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
- callback(data[sourceId]);
-
- // Empties the struct
- data[sourceId].canId = -1;
- data[sourceId].length = 0;
- }
- }
+ // If we have received the right number of packet call onReceive and
+ // reset the message
+ if (nReceived == msg.length && nReceived != 0)
+ {
+ onReceive(msg);
+
+ // Reset the packet
+ msg.id = -1;
+ msg.length = 0;
}
}
}
}
+void CanProtocol::runSender()
+{
+ LOG_DEBUG(logger, "Sender is running");
+ CanMessage msg;
+
+ while (!stopFlag)
+ {
+ outQueue.waitUntilNotEmpty();
+
+ if (!outQueue.isEmpty())
+ {
+ // Get the first packet in the queue, without removing it
+ msg = outQueue.pop();
+
+ LOG_DEBUG(logger, "Sending message, length: {}", msg.length);
+
+ sendMessage(msg);
+
+ // updateSenderStats();
+ }
+ else
+ {
+ // Wait before sending something else
+ miosix::Thread::sleep(50);
+ }
+ }
+}
+
+void CanProtocol::rcvLauncher(void* arg)
+{
+ reinterpret_cast<CanProtocol*>(arg)->runReceiver();
+}
+
+void CanProtocol::sndLauncher(void* arg)
+{
+ reinterpret_cast<CanProtocol*>(arg)->runSender();
+}
+
+uint8_t CanProtocol::byteForUint64(uint64_t number)
+{
+ uint8_t i;
+
+ for (i = 1; i <= 8; i++)
+ {
+ number >>= 8;
+ if (number == 0)
+ return i;
+ }
+
+ return i;
+}
+
} // namespace Canbus
} // namespace Boardcore
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
index 88316ffad940f4884fa1a000f5a02e10dee9a732..1526ca461cbaca68edc80532b500001ae828219f 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
@@ -22,14 +22,12 @@
#pragma once
-#include <ActiveObject.h>
+#include <diagnostic/PrintLogger.h>
#include <drivers/canbus/CanDriver/CanDriver.h>
#include <utils/Debug.h>
-#include <utils/collections/IRQCircularBuffer.h>
+#include <utils/collections/SyncCircularBuffer.h>
-#include <thread>
-
-#define N_BOARDS 3 ///< Number of boards on the bus.
+#include "CanProtocolData.h"
namespace Boardcore
{
@@ -38,85 +36,77 @@ namespace Canbus
{
/**
- * 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 /
+ * The CanProtocol allows to transmit arbitrarily sized messages over the CanBus
+ * overcoming the 8 byte limitation of each single packet.
+ *
+ * Our CanProtocol uses the extended can packet, the 29 bits id is divided such
+ * as:
+ * - Priority 4 bit - priority \
+ * - Primary type 6 bit - primaryType |
+ * - Source 4 bit - source | 22 bits - Message informations
+ * - Destination 4 bit - destination |
+ * - Secondary type 4 bit - secondaryType /
+ * - First packet flag 1 bit - firstPacket \ 7 bits - Sequential informations
+ * - 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.
*
- * CompleteID = (IDMask << shiftSequentialInfo) || SequentialInformation
+ * The id is split into 2 parts:
+ * - Message information: Common to every packet of a given message
+ * - Sequential information: Used to distinguish between packets
+ *
+ * The sender splits into multiple packets a message that is then recomposed on
+ * the receiver end. The message informations are encoded into the packets id,
+ * therefore they have an effect on packets priorities.
*/
/**
- * @brief Enumeration that contains masks of the elements composing the can
- * packet id without sequential information.
+ * @brief Masks of the elements composing can packets ids.
*/
-enum IDMask : uint32_t
+enum class CanPacketIdMask : uint32_t
{
- priority = 0x3C0000,
+ PRIORITY = 0x1E000000,
+ PRIMARY_TYPE = 0x01F80000,
+ SOURCE = 0x00078000,
+ DESTINATION = 0x00003800,
+ SECONDARY_TYPE = 0x00000780,
- type = 0x03F000,
- source = 0x000F00,
- destination = 0x0000F0,
- idType = 0x00000F
-};
+ MESSAGE_INFORMATION = 0x1FFFFF80,
-/**
- * @brief Enumeration that contains masks of the elements composing the
- * sequential information.
- */
-enum SequentialInformation : uint8_t
-{
- firstPacket = 0x40,
- leftToSend = 0x3F
-};
+ FIRST_PACKET_FLAG = 0x00000040,
+ LEFT_TO_SEND = 0x0000003F,
-enum ShiftInformation : uint8_t
-{
- // Shift info of IDMask
- shiftPriority = 18,
- shiftType = 12,
- shiftSource = 8,
- shiftDestination = 4,
- shiftIdType = 0,
- // Shift info of SequentialInformation
- shiftFirstPacket = 6,
- shiftLeftToSend = 0,
- shiftSequentialInfo = 7
+ SEQUENTIAL_INFORMATION = 0x0000007F
};
-/**
- * @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
+enum ShiftInformation : uint8_t
{
- int32_t canId = -1; ///< Id of the packet without the sequential info.
- uint8_t length;
- uint8_t nRec = 0;
- uint64_t payload[32];
+ // Shift values for message informations
+ PRIORITY = 25,
+ PRIMARY_TYPE = 19,
+ SOURCE = 15,
+ DESCRIPTION = 11,
+ SECONDARY_TYPE = 7,
+
+ // Shift values for sequential informations
+ FIRST_PACKET_FLAG = 6,
+ LEFT_TO_SEND = 0,
+
+ // Position of the message infos relative to the entire can packet id
+ SEQUENTIAL_INFORMATION = 7
};
/**
* @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.
+ * Given a can interface this class takes care of sending messages segmented
+ * into multiple packets, and receiving single packets that are then reframed
+ * into messages.
+ *
+ * This driver has been implemented following the MavlinkDriver.
*/
-class CanProtocol : public ActiveObject
+class CanProtocol
{
- using MsgHandler = std::function<void(CanData data)>;
+ using MsgHandler = std::function<void(CanMessage data)>;
public:
/**
@@ -124,51 +114,97 @@ public:
*
* @param can Pointer to a CanbusDriver object.
*/
- explicit CanProtocol(CanbusDriver* can, MsgHandler callback);
+ CanProtocol(CanbusDriver* can, MsgHandler onReceive);
+
+ /**
+ * @brief Start the receiving and sending threads.
+ *
+ * @return False if at least one could not start.
+ */
+ bool start();
+
+ /**
+ * @brief Tells whether the driver was started.
+ */
+ bool isStarted();
+
+ /**
+ * @brief Stops sender and receiver threads.
+ */
+ void stop();
- ~CanProtocol();
+ /**
+ * @brief Non-blocking send function, puts the messages in a queue.
+ * Message is discarded if the queue is full.
+ *
+ * @param msg Message to send (CanMessage struct).
+ * @return True if the message could be enqueued.
+ */
+ bool enqueueMsg(const CanMessage& msg);
+private:
/**
- * @brief Sends a CanData object on the bus.
+ * @brief Blocking send function, puts the CanMessage object on the bus.
*
- * Takes a CanData object, splits it into multiple CanPackets with the
+ * Takes a CanMessage object, splits it into multiple CanPackets with the
* correct sequential id.
- * @warning requires @param data to be not empty.
*
- * @param data Contains the id and the data of the packet to send.
+ * @param msg Contains the id and the data of the packet to send.
*/
- void send(CanData toSend);
+ void sendMessage(const CanMessage& msg);
-private:
/**
- * @brief Count the number of bytes needed to encode a uint64_t number.
+ * @brief Receiver thread: waits for one packet at a time from the can
+ * driver and tries to parse a message.
+ *
+ * If the message is successfully parsed, the onReceive function is
+ * executed.
+ *
+ * For now if a packet is received in the wrong order or if a packet with a
+ * different id is received, the current (incomplete) message will be lost.
+ * Once we receive a new first packet, currently saved data are reset.
*/
- uint8_t byteForInt(uint64_t number);
+ void runReceiver();
- void sendData();
+ /**
+ * @brief Sender Thread: Periodically flushes the message queue and sends
+ * all the enqueued messages.
+ */
+ void runSender();
/**
- * @brief Keeps listening on the canbus for packets.
+ * @brief Calls the run member function.
*
- * Once a packet is received, it checks if it is expected (that id is
- * 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.
+ * @param arg The object pointer cast to void*.
+ */
+ static void rcvLauncher(void* arg);
+
+ /**
+ * @brief Calls the run member function.
*
- * 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.
+ * @param arg The object pointer cast to void*.
*/
- void run() override;
+ static void sndLauncher(void* arg);
+
+ /**
+ * @brief Count the number of bytes needed to encode a uint64_t number.
+ */
+ uint8_t byteForUint64(uint64_t number);
+
+ CanbusDriver* can; ///< Device used to send and receive packets.
+ MsgHandler onReceive; ///< Function executed when a message is ready.
- // The physical implementation of the CanBus
- CanbusDriver* can;
+ // Threads
+ bool stopFlag = false;
+ bool sndStarted = false;
+ bool rcvStarted = false;
- // The buffer used to store the CanData to send
- IRQCircularBuffer<CanData, N_BOARDS> TXbuffer;
+ miosix::Thread* sndThread = nullptr;
+ miosix::Thread* rcvThread = nullptr;
- miosix::FastMutex mutex;
+ SyncCircularBuffer<CanMessage, 10> outQueue;
- MsgHandler callback;
+ PrintLogger logger = Logging::getLogger("canprotocol");
};
} // namespace Canbus
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h b/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h
new file mode 100644
index 0000000000000000000000000000000000000000..7eb3c8808a43a60d70f52a1f778a1bb89e7212f1
--- /dev/null
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h
@@ -0,0 +1,70 @@
+/* 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.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+namespace Boardcore
+{
+
+namespace Canbus
+{
+
+/**
+ * @brief Generic struct that contains a can protocol message.
+ *
+ * For example an accelerometer message could have:
+ * - 4 bytes for the timestamp
+ * - 3x4 bytes for float values
+ * This message would be divided into 2 can packets.
+ *
+ * Note that the maximum size for a message is 520 bytes since the remaining
+ * packet information is 6 bit wide.
+ */
+struct CanMessage
+{
+ int32_t id = -1; ///< Id of the message without sequential infos.
+ uint8_t length = 0; ///< Length of the message content.
+ uint64_t payload[65];
+};
+
+inline bool operator==(const CanMessage& lhs, const CanMessage& rhs)
+{
+ if (lhs.id != rhs.id || lhs.length != rhs.length)
+ return false;
+
+ for (int i = 0; i < lhs.length; i++)
+ if (lhs.payload[i] != rhs.payload[i])
+ return false;
+
+ return true;
+}
+
+inline bool operator!=(const CanMessage& lhs, const CanMessage& rhs)
+{
+ return !(lhs == rhs);
+}
+
+} // namespace Canbus
+
+} // namespace Boardcore
diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
index 1bf8c8047e51277aa25155f5221b9ac51625ffa6..81607862cfe14f10b7de94b1faf00bd2fcce23bb 100644
--- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h
+++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
@@ -22,15 +22,12 @@
#pragma once
-#include <diagnostic/PrintLogger.h>
-
#include <vector>
/**
* This object includes only the protocol header (`protocol.h`). To use this
* driver, you should include YOUR OWN implementation of the messages definition
- * (`mavlink.h`) before including this header. To create your implementation you
- * can use Skyward's *Mavlink Editor*.
+ * (`mavlink.h`) before including this header.
*/
#ifndef MAVLINK_H
#error \
@@ -38,6 +35,7 @@
implementation before including MavlinkDriver.h"
#endif
+#include <diagnostic/PrintLogger.h>
#include <diagnostic/SkywardStack.h>
#include <diagnostic/StackLogger.h>
#include <mavlink_lib/mavlink_types.h>
@@ -84,7 +82,8 @@ public:
/**
* @brief Start the receiving and sending threads.
- * @return false if at least one could not start.
+ *
+ * @return False if at least one could not start.
*/
bool start();
@@ -116,6 +115,17 @@ public:
*/
bool enqueueRaw(uint8_t* msg, size_t size);
+ /**
+ * @brief Synchronized status getter.
+ */
+ MavlinkStatus getStatus();
+
+ /**
+ * @brief Setter for the sleep after send value.
+ */
+ void setSleepAfterSend(uint16_t newSleepTime);
+
+private:
/**
* @brief Receiver thread: reads one char at a time from the transceiver and
* tries to parse a mavlink message.
@@ -134,21 +144,10 @@ public:
*/
void runSender();
- /**
- * @brief Synchronized status getter.
- */
- MavlinkStatus getStatus();
-
- /**
- * @brief Setter for the sleep after send value.
- */
- void setSleepAfterSend(uint16_t newSleepTime);
-
-private:
/**
* @brief Calls the run member function.
*
- * @param arg the object pointer cast to void*
+ * @param arg The object pointer cast to void*.
*/
static void rcvLauncher(void* arg)
{
@@ -158,7 +157,7 @@ private:
/**
* @brief Calls the run member function.
*
- * @param arg the object pointer cast to void*
+ * @param arg The object pointer cast to void*.
*/
static void sndLauncher(void* arg)
{
@@ -169,8 +168,8 @@ private:
void updateSenderStats(size_t msgCount, bool sent);
- Transceiver* device; ///< transceiver used to send and receive
- MavHandler onReceive; ///< function executed on message rcv
+ Transceiver* device; ///< Transceiver used to send and receive packets.
+ MavHandler onReceive; ///< Function executed when a message is ready.
// Tweakable params
uint16_t sleepAfterSend;
@@ -273,7 +272,7 @@ bool MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::enqueueMsg(
uint8_t msgTempBuf[MAVLINK_NUM_NON_PAYLOAD_BYTES + MavMsgLength];
int msgLen = mavlink_msg_to_send_buffer(msgTempBuf, &msg);
- // Append message to the queue
+ // Append the message to the queue
bool appended = outQueue.put(msgTempBuf, msgLen);
// Update stats
diff --git a/src/tests/drivers/canbus/SimpleCanManager.h b/src/tests/drivers/canbus/CanDriver/SimpleCanManager.h
similarity index 100%
rename from src/tests/drivers/canbus/SimpleCanManager.h
rename to src/tests/drivers/canbus/CanDriver/SimpleCanManager.h
diff --git a/src/tests/drivers/canbus/test-canbus-2way.cpp b/src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
similarity index 100%
rename from src/tests/drivers/canbus/test-canbus-2way.cpp
rename to src/tests/drivers/canbus/CanDriver/test-can-2way.cpp
diff --git a/src/tests/drivers/canbus/test-canbus-filters.cpp b/src/tests/drivers/canbus/CanDriver/test-can-filters.cpp
similarity index 97%
rename from src/tests/drivers/canbus/test-canbus-filters.cpp
rename to src/tests/drivers/canbus/CanDriver/test-can-filters.cpp
index c56e948c238fa50528df5466e64290f296fa2914..b6c3f6b667ffcac3ff1021d99634f5430ec08b78 100644
--- a/src/tests/drivers/canbus/test-canbus-filters.cpp
+++ b/src/tests/drivers/canbus/CanDriver/test-can-filters.cpp
@@ -22,8 +22,8 @@
#include <ActiveObject.h>
#include <diagnostic/PrintLogger.h>
-#include <drivers/canbus/BusLoadEstimation.h>
-#include <drivers/canbus/CanDriver.h>
+#include <drivers/canbus/CanDriver/BusLoadEstimation.h>
+#include <drivers/canbus/CanDriver/CanDriver.h>
#include <string>
diff --git a/src/tests/drivers/canbus/test-canbus-loopback.cpp b/src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp
similarity index 100%
rename from src/tests/drivers/canbus/test-canbus-loopback.cpp
rename to src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp
diff --git a/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp b/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..de42a9c3d9b3cd911b44f0b628fd5eee14a66ef2
--- /dev/null
+++ b/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
@@ -0,0 +1,88 @@
+/* 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 <drivers/canbus/CanDriver/BusLoadEstimation.h>
+#include <drivers/canbus/CanDriver/CanDriver.h>
+#include <drivers/canbus/CanProtocol/CanProtocol.h>
+#include <scheduler/TaskScheduler.h>
+#include <utils/collections/CircularBuffer.h>
+
+#include <functional>
+#include <thread>
+
+using namespace std;
+using namespace miosix;
+using namespace Boardcore;
+using namespace Canbus;
+
+void print(CanMessage data) { printf("Received packet %lu\n", data.id); }
+
+int main()
+{
+ // Prepare the cab driver
+ CanbusDriver::CanbusConfig config;
+ config.loopback = true;
+ CanbusDriver::AutoBitTiming bitTiming;
+ bitTiming.baudRate = 500 * 1000;
+ bitTiming.samplePoint = 87.5f / 100.0f;
+ CanbusDriver* driver = new CanbusDriver(CAN1, config, bitTiming);
+
+ // Prepare the can driver
+ CanProtocol protocol(driver, print);
+
+ // Add a filter to allow every message
+ Mask32FilterBank f2(0, 0, 1, 1, 0, 0, 0);
+ driver->addFilter(f2);
+ driver->init();
+
+ // Start the protocol
+ protocol.start();
+
+ CanMessage msg1, msg2;
+
+ msg1.id = 0x200;
+ msg1.length = 2;
+ msg1.payload[0] = 0xffffffffffffffff;
+ msg1.payload[1] = 0x0123456789ABCDEF;
+
+ msg2.id = 0x100;
+ msg2.length = 1;
+ msg2.payload[0] = 0;
+
+ TaskScheduler scheduler;
+
+ scheduler.addTask([&]() { protocol.enqueueMsg(msg1); }, 1000);
+ scheduler.addTask(
+ [&]()
+ {
+ msg2.payload[0]++;
+ protocol.enqueueMsg(msg2);
+ },
+ 2000);
+
+ scheduler.start();
+
+ printf("Started\n");
+
+ while (true)
+ Thread::sleep(1000);
+}
diff --git a/src/tests/drivers/canbus/test-can-protocol.cpp b/src/tests/drivers/canbus/test-can-protocol.cpp
deleted file mode 100644
index 01b8a5f271fbaa49bc0834ca788c8a164f7edae1..0000000000000000000000000000000000000000
--- a/src/tests/drivers/canbus/test-can-protocol.cpp
+++ /dev/null
@@ -1,160 +0,0 @@
-
-
-/* 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 <drivers/canbus/CanDriver/BusLoadEstimation.h>
-#include <drivers/canbus/CanDriver/CanDriver.h>
-#include <drivers/canbus/CanProtocol/CanProtocol.h>
-
-#include <functional>
-#include <thread>
-
-#include "utils/collections/CircularBuffer.h"
-
-constexpr uint32_t BAUD_RATE = 500 * 1000;
-constexpr float SAMPLE_POINT = 87.5f / 100.0f;
-
-using std::string;
-using namespace Boardcore;
-using namespace Boardcore::Canbus;
-using namespace miosix;
-
-#ifdef _ARCH_CORTEXM3_STM32
-using CanRX = Gpio<GPIOA_BASE, 11>;
-using CanTX = Gpio<GPIOA_BASE, 12>;
-#else
-using CanRX = Gpio<GPIOA_BASE, 11>;
-using CanTX = Gpio<GPIOA_BASE, 12>;
-#endif
-
-#define SLP 100
-miosix::FastMutex mutex;
-CanData toSend1;
-CanData toSend2;
-bool equal(CanData* first, CanData* second)
-{
- if ((*first).canId != (*second).canId ||
- (*first).length != (*second).length)
- {
- return false;
- }
- for (int i = 0; i < (*first).length; i++)
- {
- if ((*first).payload[i] != (*second).payload[i])
- return false;
- }
- return true;
-}
-
-void print(CanData data)
-{
- if ((!equal(&data, &toSend1) && !equal(&data, &toSend2)))
- {
- TRACE("Error\n");
- TRACE("Received id %lu\n", data.canId);
- TRACE("length %d\n", data.length);
- for (int i = 0; i < data.length; i++)
- {
- TRACE("Received payload %d: %llu,\n", i, data.payload[i]);
- }
- }
- else
- {
- TRACE("OK :) id %lu\n", data.canId);
- }
-}
-void sendData(CanProtocol* protocol, CanData* toSend)
-{
- while (true)
- {
-
- // TRACE("send id %d\n", toSend->canId);
- {
- miosix::Lock<miosix::FastMutex> l(mutex);
- (*protocol).send(*toSend);
- }
- Thread::sleep(SLP);
- }
-}
-int main()
-{
- {
- miosix::FastInterruptDisableLock dLock;
-
-#ifdef _ARCH_CORTEXM3_STM32
- CanRX::mode(Mode::ALTERNATE);
- CanTX::mode(Mode::ALTERNATE);
-#else
- CanRX::mode(Mode::ALTERNATE);
- CanTX::mode(Mode::ALTERNATE);
-
- CanRX::alternateFunction(9);
- CanTX::alternateFunction(9);
-#endif
- }
- CanbusDriver::CanbusConfig cfg{};
- CanbusDriver::AutoBitTiming bt;
- bt.baudRate = BAUD_RATE;
- bt.samplePoint = SAMPLE_POINT;
- CanbusDriver* c = new CanbusDriver(CAN1, cfg, bt);
- CanProtocol protocol(c,
- &print); // std::bind(&print, std::placeholders::_1));
- // Allow every message
- Mask32FilterBank f2(0, 0, 1, 1, 0, 0, 0);
-
- c->addFilter(f2);
- c->init();
- protocol.start();
-
- toSend1.canId = 0x200;
- toSend1.length = 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.length = 4;
- toSend2.payload[0] = 0xffffff;
- toSend2.payload[1] = 2;
- toSend2.payload[2] = 0x123ff;
- toSend2.payload[3] = 1;
- std::thread secondSend(sendData, &protocol, &toSend2);
-
- /* code */
-
- TRACE("start \n");
- while (true)
- {
- protocol.send(toSend1);
- Thread::sleep(SLP);
-
- // TRACE("received packet \n");
- }
-}