diff --git a/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
index 66bf37394c23a1fc14617028d1b350e0c830a968..d635553acec07638bffa7aa20876123429fbe2de 100644
--- a/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
+++ b/src/shared/drivers/canbus/CanDriver/BusLoadEstimation.h
@@ -27,7 +27,7 @@
#include <cstdint>
-#include "CanData.h"
+#include "CanDriverData.h"
using miosix::FastMutex;
using miosix::Lock;
diff --git a/src/shared/drivers/canbus/CanDriver/CanDriver.h b/src/shared/drivers/canbus/CanDriver/CanDriver.h
index 07006aa3752021ebad17d7fe3a25ff13d19562da..f9d9b493268bd321000865bcfa0e05692ccc51f4 100644
--- a/src/shared/drivers/canbus/CanDriver/CanDriver.h
+++ b/src/shared/drivers/canbus/CanDriver/CanDriver.h
@@ -26,7 +26,7 @@
#include <miosix.h>
#include <utils/collections/IRQCircularBuffer.h>
-#include "CanData.h"
+#include "CanDriverData.h"
#include "Filters.h"
using miosix::Thread;
diff --git a/src/shared/drivers/canbus/CanDriver/CanData.h b/src/shared/drivers/canbus/CanDriver/CanDriverData.h
similarity index 100%
rename from src/shared/drivers/canbus/CanDriver/CanData.h
rename to src/shared/drivers/canbus/CanDriver/CanDriverData.h
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
index 5f6189f22817e0f9811ffaa1b4650db14eed2c61..f8454775b2842f3feb099b0d088397d50832149d 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
@@ -94,6 +94,24 @@ bool CanProtocol::enqueueMsg(const CanMessage& msg)
return true;
}
+bool CanProtocol::addFilter(uint8_t src, uint64_t dst)
+{
+ if (src > 0xF || dst > 0xF)
+ return false;
+
+ // The filter mask will cover only the source and destination bits
+ uint32_t mask = static_cast<uint32_t>(CanProtocolIdMask::SOURCE) |
+ static_cast<uint32_t>(CanProtocolIdMask::DESTINATION);
+
+ uint32_t id =
+ src << static_cast<uint8_t>(CanProtocolShiftInformation::SOURCE) |
+ dst << static_cast<uint8_t>(CanProtocolShiftInformation::DESTINATION);
+
+ Mask32FilterBank filterBank(id, mask, 1, 1, 0, 0, 0);
+
+ return can->addFilter(filterBank);
+}
+
void CanProtocol::sendMessage(const CanMessage& msg)
{
CanPacket packet = {};
@@ -105,7 +123,7 @@ void CanProtocol::sendMessage(const CanMessage& msg)
// 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));
+ static_cast<uint32_t>(CanProtocolIdMask::LEFT_TO_SEND));
packet.length = byteForUint64(msg.payload[0]);
// Splits payload[0] in the right number of uint8_t
@@ -119,10 +137,11 @@ void CanProtocol::sendMessage(const CanMessage& msg)
// Prepare the remaining packets
for (int i = 1; i < msg.length; 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.id =
+ static_cast<uint32_t>(msg.id) |
+ static_cast<uint32_t>(CanProtocolIdMask::FIRST_PACKET_FLAG) |
+ ((static_cast<uint32_t>(0x3F) - leftToSend) &
+ static_cast<uint32_t>(CanProtocolIdMask::LEFT_TO_SEND));
packet.length = byteForUint64(msg.payload[i]);
// Splits payload[i] in the right number of uint8_t
@@ -151,17 +170,18 @@ void CanProtocol::runReceiver()
uint8_t leftToReceive =
static_cast<uint32_t>(0x3F) -
- (pkt.id & static_cast<uint32_t>(CanPacketIdMask::LEFT_TO_SEND));
+ (pkt.id &
+ static_cast<uint32_t>(CanProtocolIdMask::LEFT_TO_SEND));
// 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)
+ CanProtocolIdMask::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);
+ CanProtocolIdMask::MESSAGE_INFORMATION);
msg.length = leftToReceive + 1;
// Reset the number of received packets
@@ -171,8 +191,8 @@ void CanProtocol::runReceiver()
// 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)))
+ (pkt.id & static_cast<uint32_t>(CanProtocolIdMask::MESSAGE_INFORMATION)) ==
+ (msg.id & static_cast<uint32_t>(CanProtocolIdMask::MESSAGE_INFORMATION)))
// clang-format on
{
// Check if the packet is expected in the sequence. The received
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
index 1526ca461cbaca68edc80532b500001ae828219f..a8792d7c08016fbbd3f9192d2bd3927bb1a4b962 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
@@ -28,6 +28,7 @@
#include <utils/collections/SyncCircularBuffer.h>
#include "CanProtocolData.h"
+#include "CanProtocolTypes.h"
namespace Boardcore
{
@@ -35,66 +36,6 @@ namespace Boardcore
namespace Canbus
{
-/**
- * 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
- *
- * 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 Masks of the elements composing can packets ids.
- */
-enum class CanPacketIdMask : uint32_t
-{
- PRIORITY = 0x1E000000,
- PRIMARY_TYPE = 0x01F80000,
- SOURCE = 0x00078000,
- DESTINATION = 0x00003800,
- SECONDARY_TYPE = 0x00000780,
-
- MESSAGE_INFORMATION = 0x1FFFFF80,
-
- FIRST_PACKET_FLAG = 0x00000040,
- LEFT_TO_SEND = 0x0000003F,
-
- SEQUENTIAL_INFORMATION = 0x0000007F
-};
-
-enum ShiftInformation : uint8_t
-{
- // 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.
*
@@ -133,6 +74,16 @@ public:
*/
void stop();
+ /**
+ * @brief Adds a filter to the can peripheral to receive only messages from
+ * the given source and targeted to the given destination.
+ *
+ * @param src Message source.
+ * @param dst Message destination.
+ * @return True if the filter was added successfully.
+ */
+ bool addFilter(uint8_t src, uint64_t dst);
+
/**
* @brief Non-blocking send function, puts the messages in a queue.
* Message is discarded if the queue is full.
@@ -142,6 +93,18 @@ public:
*/
bool enqueueMsg(const CanMessage& msg);
+ /**
+ * @brief Non-blocking send function for a generic data type.
+ *
+ * @warning There must be a function called with this prototype:
+ * CanMessage toCanMessage(const T& t);
+ *
+ * @param t The class to be logged.
+ */
+ template <typename T>
+ bool enqueueData(uint8_t priority, uint8_t primaryType, uint8_t source,
+ uint8_t destination, uint8_t secondaryType, const T& t);
+
private:
/**
* @brief Blocking send function, puts the CanMessage object on the bus.
@@ -207,6 +170,28 @@ private:
PrintLogger logger = Logging::getLogger("canprotocol");
};
+template <typename T>
+bool CanProtocol::enqueueData(uint8_t priority, uint8_t primaryType,
+ uint8_t source, uint8_t destination,
+ uint8_t secondaryType, const T& t)
+{
+ if (priority > 0xF || primaryType > 0x3F || source > 0xF ||
+ destination > 0xF || secondaryType > 0xF)
+ return false;
+
+ CanMessage msg = toCanMessage(t);
+
+ // clang-format off
+ msg.id = priority << static_cast<uint32_t>(CanProtocolShiftInformation::PRIORITY);
+ msg.id |= primaryType << static_cast<uint32_t>(CanProtocolShiftInformation::PRIMARY_TYPE);
+ msg.id |= source << static_cast<uint32_t>(CanProtocolShiftInformation::SOURCE);
+ msg.id |= destination << static_cast<uint32_t>(CanProtocolShiftInformation::DESTINATION);
+ msg.id |= secondaryType << static_cast<uint32_t>(CanProtocolShiftInformation::SECONDARY_TYPE);
+ // clang-format off
+
+ return enqueueMsg(msg);
+}
+
} // namespace Canbus
} // namespace Boardcore
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h b/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h
index 7eb3c8808a43a60d70f52a1f778a1bb89e7212f1..1a2b3b62e9835dddd237d74915342fc7e618ca4e 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h
@@ -30,6 +30,89 @@ namespace Boardcore
namespace Canbus
{
+/**
+ * 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 \
+ * - Primary type 6 bit |
+ * - Source 4 bit | 22 bits - Message informations
+ * - Destination 4 bit |
+ * - Secondary type 4 bit /
+ * - First packet flag 1 bit \ 7 bits - Sequential informations
+ * - Remaining packets 6 bit /
+ * shiftNameOfField the number of shift needed to reach that field
+ *
+ * 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.
+ */
+/**
+ * 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 \
+ * - Primary type 6 bit |
+ * - Source 4 bit | 22 bits - Message informations
+ * - Destination 4 bit |
+ * - Secondary type 4 bit /
+ * - First packet flag 1 bit \ 7 bits - Sequential informations
+ * - Remaining packets 6 bit /
+ * shiftNameOfField the number of shift needed to reach that field
+ *
+ * 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 Masks of the elements composing can packets ids.
+ */
+enum class CanProtocolIdMask : uint32_t
+{
+ PRIORITY = 0x1E000000,
+ PRIMARY_TYPE = 0x01F80000,
+ SOURCE = 0x00078000,
+ DESTINATION = 0x00003800,
+ SECONDARY_TYPE = 0x00000780,
+
+ MESSAGE_INFORMATION = 0x1FFFFF80,
+
+ FIRST_PACKET_FLAG = 0x00000040,
+ LEFT_TO_SEND = 0x0000003F,
+
+ SEQUENTIAL_INFORMATION = 0x0000007F
+};
+
+enum CanProtocolShiftInformation : uint8_t
+{
+ // Shift values for message informations
+ PRIORITY = 25,
+ PRIMARY_TYPE = 19,
+ SOURCE = 15,
+ DESTINATION = 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 Generic struct that contains a can protocol message.
*
@@ -46,6 +129,37 @@ 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];
+
+ uint8_t getPriority()
+ {
+ return id >>
+ static_cast<uint8_t>(CanProtocolShiftInformation::PRIORITY);
+ }
+
+ uint8_t getPrimaryType()
+ {
+ return (id | static_cast<uint32_t>(CanProtocolIdMask::PRIMARY_TYPE)) >>
+ static_cast<uint8_t>(CanProtocolShiftInformation::PRIMARY_TYPE);
+ }
+
+ uint8_t getSource()
+ {
+ return (id | static_cast<uint32_t>(CanProtocolIdMask::SOURCE)) >>
+ static_cast<uint8_t>(CanProtocolShiftInformation::SOURCE);
+ }
+
+ uint8_t getDestination()
+ {
+ return (id | static_cast<uint32_t>(CanProtocolIdMask::DESTINATION)) >>
+ static_cast<uint8_t>(CanProtocolShiftInformation::DESTINATION);
+ }
+
+ uint8_t getSecondaryType()
+ {
+ return (id |
+ static_cast<uint32_t>(CanProtocolIdMask::SECONDARY_TYPE)) >>
+ static_cast<uint8_t>(CanProtocolShiftInformation::PRIMARY_TYPE);
+ }
};
inline bool operator==(const CanMessage& lhs, const CanMessage& rhs)
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h b/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h
new file mode 100644
index 0000000000000000000000000000000000000000..b84db80a9c1e530dc9666af6c2e9951a7e3ed7f6
--- /dev/null
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocolTypes.h
@@ -0,0 +1,49 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <sensors/analog/Pitot/PitotData.h>
+
+#include <cstring>
+
+#include "CanProtocolData.h"
+
+namespace Boardcore
+{
+
+inline Canbus::CanMessage toCanMessage(const PitotData& data)
+{
+ Canbus::CanMessage message;
+
+ uint32_t tmp;
+ memcpy(&tmp, &(data.airspeed), sizeof(tmp));
+
+ message.id = -1;
+ message.length = 1;
+ message.payload[0] = (data.timestamp & ~0x3) << 30;
+ message.payload[0] |= tmp;
+
+ return message;
+}
+
+} // namespace Boardcore
diff --git a/src/shared/logger/Logger.h b/src/shared/logger/Logger.h
index 496b1cf311900f722a7070447cb9234f864791f4..7d404ae83eb064d15f9ef793e0f3cac9867eb11b 100644
--- a/src/shared/logger/Logger.h
+++ b/src/shared/logger/Logger.h
@@ -124,7 +124,7 @@ public:
*/
void logStats();
-public:
+private:
Logger();
static std::string getFileName(int logNumber);
diff --git a/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp b/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
index de42a9c3d9b3cd911b44f0b628fd5eee14a66ef2..2fcc424502a10db5c3936d2924a264120219e2cd 100644
--- a/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
+++ b/src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp
@@ -20,21 +20,25 @@
* THE SOFTWARE.
*/
-#include <drivers/canbus/CanDriver/BusLoadEstimation.h>
-#include <drivers/canbus/CanDriver/CanDriver.h>
#include <drivers/canbus/CanProtocol/CanProtocol.h>
+#include <drivers/timer/TimestampTimer.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); }
+void print(CanMessage data)
+{
+ printf("Received packet:\n");
+ printf("\tpriority: %d\n", data.getPriority());
+ printf("\tprimary type: %d\n", data.getPrimaryType());
+ printf("\tsource: %d\n", data.getSource());
+ printf("\tdestination: %d\n", data.getDestination());
+ printf("\tsecondary type: %d\n", data.getSecondaryType());
+ printf("\n");
+}
int main()
{
@@ -57,25 +61,21 @@ int main()
// Start the protocol
protocol.start();
- CanMessage msg1, msg2;
-
+ CanMessage msg1;
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);
+ PitotData data{TimestampTimer::getTimestamp(), 23};
+
+ protocol.enqueueData(0xF, 0xA, 0x1, 0x2, 0xB, data);
},
2000);