From 959ee196d339d84b9b8039c1258356e5fdd50c28 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Mon, 25 Jul 2022 11:15:43 +0000
Subject: [PATCH] [CanProtocol] Added enqueueEvent and fixed bugs
---
.../drivers/canbus/CanDriver/CanDriver.h | 1 +
.../canbus/CanProtocol/CanProtocol.cpp | 37 ++++++++++++++++++-
.../drivers/canbus/CanProtocol/CanProtocol.h | 7 ++++
.../canbus/CanProtocol/CanProtocolData.h | 13 ++++---
4 files changed, 51 insertions(+), 7 deletions(-)
diff --git a/src/shared/drivers/canbus/CanDriver/CanDriver.h b/src/shared/drivers/canbus/CanDriver/CanDriver.h
index f9d9b4932..ed845f5f5 100644
--- a/src/shared/drivers/canbus/CanDriver/CanDriver.h
+++ b/src/shared/drivers/canbus/CanDriver/CanDriver.h
@@ -140,6 +140,7 @@ public:
/**
* @brief Adds a new filter to the bus, or returns false if there are no
* more filter banks available.
+ *
* @warning Can only be called before init().
*
* @param filter Filter to be added
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
index f8454775b..64c6a27bd 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
@@ -39,6 +39,9 @@ bool CanProtocol::start()
{
stopFlag = false;
+ if (can == nullptr)
+ return false;
+
// Start sender (joinable thread)
if (!sndStarted)
{
@@ -109,7 +112,10 @@ bool CanProtocol::addFilter(uint8_t src, uint64_t dst)
Mask32FilterBank filterBank(id, mask, 1, 1, 0, 0, 0);
- return can->addFilter(filterBank);
+ if (can == nullptr)
+ return false;
+ else
+ return can->addFilter(filterBank);
}
void CanProtocol::sendMessage(const CanMessage& msg)
@@ -153,6 +159,33 @@ void CanProtocol::sendMessage(const CanMessage& msg)
}
}
+bool CanProtocol::enqueueEvent(uint8_t priority, uint8_t primaryType,
+ uint8_t source, uint8_t destination,
+ uint8_t secondaryType)
+{
+ if (priority > 0xF || primaryType > 0x3F || source > 0xF ||
+ destination > 0xF || secondaryType > 0xF)
+ return false;
+
+ CanMessage msg{};
+
+ // Length set to a minumum of 1 even if there is no payload
+ msg.length = 1;
+ msg.payload[0] = 0xFF;
+
+ // 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
+
+ LOG_DEBUG(logger, "Sending message with id: {:x}", msg.id);
+
+ return enqueueMsg(msg);
+}
+
void CanProtocol::runReceiver()
{
CanMessage msg;
@@ -216,6 +249,8 @@ void CanProtocol::runReceiver()
// reset the message
if (nReceived == msg.length && nReceived != 0)
{
+ LOG_DEBUG(logger, "Message ready with id: {:x}", msg.id);
+
onReceive(msg);
// Reset the packet
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
index a8792d7c0..fc79de55e 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocol.h
@@ -93,6 +93,13 @@ public:
*/
bool enqueueMsg(const CanMessage& msg);
+ /**
+ * @brief Non-blocking send function for an event (a message without
+ * payload).
+ */
+ bool enqueueEvent(uint8_t priority, uint8_t primaryType, uint8_t source,
+ uint8_t destination, uint8_t secondaryType);
+
/**
* @brief Non-blocking send function for a generic data type.
*
diff --git a/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h b/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h
index ce602864c..b73b40ca8 100644
--- a/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h
+++ b/src/shared/drivers/canbus/CanProtocol/CanProtocolData.h
@@ -85,7 +85,7 @@ enum class CanProtocolIdMask : uint32_t
PRIORITY = 0x1E000000,
PRIMARY_TYPE = 0x01F80000,
SOURCE = 0x00078000,
- DESTINATION = 0x00003800,
+ DESTINATION = 0x00007800,
SECONDARY_TYPE = 0x00000780,
MESSAGE_INFORMATION = 0x1FFFFF80,
@@ -138,27 +138,28 @@ struct CanMessage
uint8_t getPrimaryType() const
{
- return (id | static_cast<uint32_t>(CanProtocolIdMask::PRIMARY_TYPE)) >>
+ return (id & static_cast<uint32_t>(CanProtocolIdMask::PRIMARY_TYPE)) >>
static_cast<uint8_t>(CanProtocolShiftInformation::PRIMARY_TYPE);
}
uint8_t getSource() const
{
- return (id | static_cast<uint32_t>(CanProtocolIdMask::SOURCE)) >>
+ return (id & static_cast<uint32_t>(CanProtocolIdMask::SOURCE)) >>
static_cast<uint8_t>(CanProtocolShiftInformation::SOURCE);
}
uint8_t getDestination() const
{
- return (id | static_cast<uint32_t>(CanProtocolIdMask::DESTINATION)) >>
+ return (id & static_cast<uint32_t>(CanProtocolIdMask::DESTINATION)) >>
static_cast<uint8_t>(CanProtocolShiftInformation::DESTINATION);
}
uint8_t getSecondaryType() const
{
- return (id |
+ return (id &
static_cast<uint32_t>(CanProtocolIdMask::SECONDARY_TYPE)) >>
- static_cast<uint8_t>(CanProtocolShiftInformation::PRIMARY_TYPE);
+ static_cast<uint8_t>(
+ CanProtocolShiftInformation::SECONDARY_TYPE);
}
};
--
GitLab