diff --git a/examples/avr/can/mcp2515_uart/main.cpp b/examples/avr/can/mcp2515_uart/main.cpp
index 5d899d07de..46fdd3b9c7 100644
--- a/examples/avr/can/mcp2515_uart/main.cpp
+++ b/examples/avr/can/mcp2515_uart/main.cpp
@@ -109,7 +109,7 @@ main()
 				}
 				stream << modm::endl;
 
-				stream << " dlc  = " << message.getLength() << modm::endl;
+				stream << " dlc  = " << message.getDataLengthCode() << modm::endl;
 				if (!message.isRemoteTransmitRequest())
 				{
 					stream << " data = ";
diff --git a/examples/generic/ros/can_bridge/main.cpp b/examples/generic/ros/can_bridge/main.cpp
index 172f848c9f..419f2aaad3 100644
--- a/examples/generic/ros/can_bridge/main.cpp
+++ b/examples/generic/ros/can_bridge/main.cpp
@@ -35,7 +35,7 @@ messageRosToModm(const can_msgs::Frame& source, modm::can::Message& destination)
 bool
 messageModmToRos(const modm::can::Message& source, can_msgs::Frame& destination) {
 	destination.id = source.getIdentifier();
-	destination.dlc = source.getLength();
+	destination.dlc = source.getDataLengthCode();
 	destination.is_rtr = source.isRemoteTransmitRequest();
 	destination.is_extended = source.isExtended();
 	memcpy(destination.data, source.data, source.getLength());
diff --git a/examples/nucleo_g474re/fdcan/main.cpp b/examples/nucleo_g474re/fdcan/main.cpp
new file mode 100644
index 0000000000..8713d3f482
--- /dev/null
+++ b/examples/nucleo_g474re/fdcan/main.cpp
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2022, Rasmus Kleist Hørlyck Sørensen
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+
+#include <modm/board.hpp>
+#include <modm/debug/logger.hpp>
+#include <modm/board.hpp>
+
+using namespace modm::literals;
+
+// Set the log level
+#undef	MODM_LOG_LEVEL
+#define	MODM_LOG_LEVEL modm::log::INFO
+
+int
+main()
+{
+	Board::initialize();
+
+	MODM_LOG_INFO << "CAN Test Program" << modm::endl;
+
+	MODM_LOG_INFO << "Initializing Fdcan1..." << modm::endl;
+	// Initialize Fdcan1
+	Fdcan1::connect<GpioB8::Rx, GpioB9::Tx>(Gpio::InputType::PullUp);
+	Fdcan1::initialize<Board::SystemClock, 125_kbps, 1_pct, 500_kbps>(9);
+
+	MODM_LOG_INFO << "Setting up Filter for Fdcan1..." << modm::endl;
+	// Receive every extended id message
+	Fdcan1::setExtendedFilter(0, Fdcan1::FilterConfig::Fifo0,
+			modm::can::ExtendedIdentifier(0),
+			modm::can::ExtendedMask(0));
+
+	MODM_LOG_INFO << "Initializing Fdcan2..." << modm::endl;
+	// Initialize Fdcan2
+	Fdcan2::connect<GpioB5::Rx, GpioB6::Tx>(Gpio::InputType::PullUp);
+	Fdcan2::initialize<Board::SystemClock, 125_kbps, 1_pct, 500_kbps>(12);
+
+	MODM_LOG_INFO << "Setting up Filter for Fdcan2..." << modm::endl;
+	// Receive every message
+	Fdcan2::setExtendedFilter(0, Fdcan2::FilterConfig::Fifo0,
+			modm::can::ExtendedIdentifier(0),
+			modm::can::ExtendedMask(0));
+
+	// Send a message
+	MODM_LOG_INFO << "Sending message on Fdcan1..." << modm::endl;
+	modm::can::Message msg(1, 1);
+	msg.setExtended(true);
+	msg.data[0] = 0x11;
+	Fdcan1::sendMessage(msg);
+
+	// Send a message
+	MODM_LOG_INFO << "Sending message on Fdcan2..." << modm::endl;
+	msg.data[0] = 0x22;
+	Fdcan2::sendMessage(msg);
+
+	// Send a message
+	MODM_LOG_INFO << "Sending message on Fdcan1..." << modm::endl;
+	modm::can::Message longMsg(2, 32);
+	longMsg.setExtended(true);
+	for (uint8_t i = 0; i < 32; i++) longMsg.data[i] = i;
+	Fdcan1::sendMessage(longMsg);
+
+	// Send a message
+	MODM_LOG_INFO << "Sending message on Fdcan2..." << modm::endl;
+	longMsg.data[0] = 0x22;
+	Fdcan2::sendMessage(longMsg);
+
+	while (true)
+	{
+		if (Fdcan1::isMessageAvailable())
+		{
+			MODM_LOG_INFO << "Fdcan1: Message is available..." << modm::endl;
+			modm::can::Message message;
+			Fdcan1::getMessage(message);
+			MODM_LOG_INFO << message << modm::endl;
+		}
+		if (Fdcan2::isMessageAvailable())
+		{
+			MODM_LOG_INFO << "Fdcan2: Message is available..." << modm::endl;
+			modm::can::Message message;
+			Fdcan2::getMessage(message);
+			MODM_LOG_INFO << message << modm::endl;
+		}
+	}
+
+	return 0;
+}
\ No newline at end of file
diff --git a/examples/nucleo_g474re/fdcan/project.xml b/examples/nucleo_g474re/fdcan/project.xml
new file mode 100644
index 0000000000..a291322fe0
--- /dev/null
+++ b/examples/nucleo_g474re/fdcan/project.xml
@@ -0,0 +1,15 @@
+<library>
+	<extends>modm:nucleo-g474re</extends>
+	<options>
+		<option name="modm:build:build.path">../../../build/nucleo_g474re/fdcan</option>
+		<option name="modm:architecture:can:message.buffer">64</option>
+	</options>
+	<modules>
+		<module>modm:debug</module>
+		<module>modm:platform:can:1</module>
+		<module>modm:platform:can:2</module>
+		<module>modm:platform:gpio</module>
+		<module>modm:platform:uart:2</module>
+		<module>modm:build:scons</module>
+	</modules>
+</library>
diff --git a/examples/stm32f072_discovery/can/main.cpp b/examples/stm32f072_discovery/can/main.cpp
index a1f518c472..37fd285010 100644
--- a/examples/stm32f072_discovery/can/main.cpp
+++ b/examples/stm32f072_discovery/can/main.cpp
@@ -53,7 +53,8 @@ displayMessage(const modm::can::Message& message)
 	}
 	MODM_LOG_INFO<< modm::endl;
 
-	MODM_LOG_INFO<< "dlc =" << message.getLength() << modm::endl;
+	MODM_LOG_INFO<< "dlc =" << message.getDataLengthCode() << modm::endl;
+	MODM_LOG_INFO<< "length =" << message.getLength() << modm::endl;
 	if (!message.isRemoteTransmitRequest())
 	{
 		MODM_LOG_INFO << "data=";
diff --git a/examples/stm32f3_discovery/can/main.cpp b/examples/stm32f3_discovery/can/main.cpp
index 513c8bd721..7aefc7c952 100644
--- a/examples/stm32f3_discovery/can/main.cpp
+++ b/examples/stm32f3_discovery/can/main.cpp
@@ -54,7 +54,8 @@ displayMessage(const modm::can::Message& message)
 	}
 	MODM_LOG_INFO<< modm::endl;
 
-	MODM_LOG_INFO<< "dlc =" << message.getLength() << modm::endl;
+	MODM_LOG_INFO<< "dlc =" << message.getDataLengthCode() << modm::endl;
+	MODM_LOG_INFO<< "length =" << message.getLength() << modm::endl;
 	if (!message.isRemoteTransmitRequest())
 	{
 		MODM_LOG_INFO << "data=";
diff --git a/examples/stm32f469_discovery/can/main.cpp b/examples/stm32f469_discovery/can/main.cpp
index ba51682ea0..9045373e58 100644
--- a/examples/stm32f469_discovery/can/main.cpp
+++ b/examples/stm32f469_discovery/can/main.cpp
@@ -46,7 +46,8 @@ displayMessage(const modm::can::Message& message)
 	}
 	MODM_LOG_INFO<< modm::endl;
 
-	MODM_LOG_INFO<< "dlc =" << message.getLength() << modm::endl;
+	MODM_LOG_INFO<< "dlc =" << message.getDataLengthCode() << modm::endl;
+	MODM_LOG_INFO<< "length =" << message.getLength() << modm::endl;
 	if (!message.isRemoteTransmitRequest())
 	{
 		MODM_LOG_INFO << "data=";
diff --git a/examples/stm32f4_discovery/can/main.cpp b/examples/stm32f4_discovery/can/main.cpp
index a948162688..97b109b457 100644
--- a/examples/stm32f4_discovery/can/main.cpp
+++ b/examples/stm32f4_discovery/can/main.cpp
@@ -40,7 +40,8 @@ displayMessage(const modm::can::Message& message)
 	}
 	MODM_LOG_INFO<< modm::endl;
 
-	MODM_LOG_INFO<< "dlc =" << message.getLength() << modm::endl;
+	MODM_LOG_INFO<< "dlc =" << message.getDataLengthCode() << modm::endl;
+	MODM_LOG_INFO<< "length =" << message.getLength() << modm::endl;
 	if (!message.isRemoteTransmitRequest())
 	{
 		MODM_LOG_INFO << "data=";
diff --git a/examples/stm32f4_discovery/can2/main.cpp b/examples/stm32f4_discovery/can2/main.cpp
index c8fc1d9538..1edd388372 100644
--- a/examples/stm32f4_discovery/can2/main.cpp
+++ b/examples/stm32f4_discovery/can2/main.cpp
@@ -55,7 +55,8 @@ displayMessage(const modm::can::Message& message)
 	}
 	MODM_LOG_INFO<< modm::endl;
 
-	MODM_LOG_INFO<< "dlc =" << message.getLength() << modm::endl;
+	MODM_LOG_INFO<< "dlc =" << message.getDataLengthCode() << modm::endl;
+	MODM_LOG_INFO<< "length =" << message.getLength() << modm::endl;
 	if (!message.isRemoteTransmitRequest())
 	{
 		MODM_LOG_INFO << "data=";
diff --git a/src/modm/architecture/interface/can_message.hpp b/src/modm/architecture/interface/can_message.hpp.in
similarity index 50%
rename from src/modm/architecture/interface/can_message.hpp
rename to src/modm/architecture/interface/can_message.hpp.in
index 45cc27970a..81f4581c96 100644
--- a/src/modm/architecture/interface/can_message.hpp
+++ b/src/modm/architecture/interface/can_message.hpp.in
@@ -1,6 +1,7 @@
 /*
  * Copyright (c) 2014, 2016, Niklas Hauser
  * Copyright (c) 2015-2016, Sascha Schade
+ * Copyright (c) 2022, Rasmus Kleist Hørlyck Sørensen
  *
  * This file is part of the modm project.
  *
@@ -16,23 +17,23 @@
 #include <stdint.h>
 #include <string.h>  // strlen
 #include <algorithm>
+#include <array>
 #include <modm/architecture/utils.hpp>
 
 namespace modm::can
 {
 
-/// Representation of a CAN message
 /// @ingroup modm_architecture_can
 struct Message
 {
 	inline Message(uint32_t inIdentifier = 0, uint8_t inLength = 0) :
-		identifier(inIdentifier), flags(), length(inLength)
+		identifier(inIdentifier), flags()
 	{
+        setLength(inLength);
 	}
 
-	// Create CAN message from long data in Network Order.
-	inline Message(uint32_t inIdentifier, uint8_t inLength, const uint64_t &inData, bool extended=false) :
-		identifier(inIdentifier), length(std::min(inLength, uint8_t(8)))
+	inline Message(uint32_t inIdentifier, uint8_t inLength, const uint64_t &inData, bool extended = false) :
+        Message(inIdentifier, std::min(inLength, uint8_t(8)))
 	{
 		flags.extended = extended;
 		const uint8_t *inDataB = reinterpret_cast<const uint8_t *>(&inData);
@@ -40,6 +41,14 @@ struct Message
  			data[ii] = inDataB[length - ii - 1];
 	}
 
+	inline Message(uint32_t inIdentifier, uint8_t inLength, const uint8_t *inData, bool extended = false) :
+		Message(inIdentifier, inLength)
+	{
+		flags.extended = extended;
+		for (uint8_t ii = 0; ii < length; ++ii)
+ 			data[ii] = inData[ii];
+	}
+
 	inline uint32_t
 	getIdentifier() const
 	{
@@ -52,6 +61,30 @@ struct Message
 		identifier = id;
 	}
 
+	inline constexpr uint8_t
+	getCapacity() const
+	{
+		return capacity;
+	}
+
+	inline void
+	setFlexibleData(bool fd = true)
+	{
+		flags.fd = fd;
+	}
+
+	inline bool
+	isFlexibleData() const
+	{
+		return (flags.fd != 0);
+	}
+
+	inline bool
+	isBitRateSwitching() const
+	{
+		return (flags.brs != 0);
+	}
+
 	inline void
 	setExtended(bool extended = true)
 	{
@@ -76,42 +109,89 @@ struct Message
 		return (flags.rtr != 0);
 	}
 
+	inline void
+	setDataLengthCode(uint8_t inDlc)
+	{
+		while (dlcConversionTable[inDlc] > capacity) inDlc--;
+
+		dlc = inDlc;
+		length = dlcConversionTable[inDlc];
+		if (dlc > 8) setFlexibleData();
+	}
+
+	inline void
+	setLength(uint8_t inLength)
+	{
+		if constexpr (capacity <= 8)
+		{
+			length = dlc = std::min<uint8_t>(inLength, capacity);
+		}
+		else
+		{
+			uint8_t inDlc = 0;
+			while (dlcConversionTable[inDlc] < std::min<uint8_t>(inLength, capacity)) inDlc++;
+			setDataLengthCode(inDlc);
+		}
+	}
+
 	inline uint8_t
 	getLength() const
 	{
 		return length;
 	}
 
-	inline void
-	setLength(uint8_t len)
+	inline uint8_t
+	getDataLengthCode() const
 	{
-		length = len;
+		return dlc;
 	}
 
+public:
+	static constexpr uint8_t capacity = {{ options["message.buffer"] }};
+	static constexpr uint8_t dlcConversionTable[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64};
 public:
 	uint32_t identifier;
-	uint8_t modm_aligned(4) data[8];
 	struct Flags
 	{
 		Flags() :
-			rtr(0), extended(1)
+			fd(0), brs(0), rtr(0), extended(1)
 		{
 		}
 
+		bool fd : 1;
+		bool brs : 1;
 		bool rtr : 1;
 		bool extended : 1;
 	} flags;
+	uint8_t dlc;
 	uint8_t length;
+	uint8_t modm_aligned(4) data[capacity];
 
 public:
 	inline bool
 	operator == (const modm::can::Message& rhs) const
 	{
 		return ((this->identifier     == rhs.identifier) and
+				(this->dlc            == rhs.dlc)        and
 				(this->length         == rhs.length)     and
+				(this->flags.fd       == rhs.flags.fd)   and
+				(this->flags.brs      == rhs.flags.brs)  and
 				(this->flags.rtr      == rhs.flags.rtr)  and
 				(this->flags.extended == rhs.flags.extended) and
-				std::equal(data, data + length, rhs.data));
+				std::equal(data, data + getLength(), rhs.data));
+	}
+
+	inline void
+	operator = (const modm::can::Message& rhs)
+	{
+		this->identifier     = rhs.identifier;
+        this->dlc            = rhs.dlc;
+		this->length         = rhs.length;
+		this->flags.fd       = rhs.flags.fd;
+		this->flags.brs      = rhs.flags.brs;
+		this->flags.rtr      = rhs.flags.rtr;
+		this->flags.extended = rhs.flags.extended;
+		std::copy(std::begin(rhs.data), std::end(rhs.data), std::begin(this->data));
 	}
 
 	inline bool
@@ -120,9 +200,10 @@ struct Message
 		return (this->identifier << (this->flags.extended ? 0 : 18))
 			< (rhs.identifier << (rhs.flags.extended ? 0 : 18));
 	}
+
 };
 
-}	// namespace modm::can
+}   // namespace modm::can
 
 #if MODM_HAS_IOSTREAM
 #include <inttypes.h>
@@ -135,13 +216,15 @@ namespace modm
 inline modm::IOStream&
 operator << (modm::IOStream& s, const modm::can::Message& m)
 {
-	s.printf("id = %04" PRIx32 ", len = ", m.identifier);
-	s << m.length;
-	s.printf(", flags = %c%c, data = ",
+	s.printf("id = %04" PRIx32 ", len = %u, dlc = %u", m.identifier, m.length, m.dlc);
+	s.printf(", flags = %c%c%c%c",
+			 m.flags.fd  ? 'F' : 'f',
+			 m.flags.brs ? 'B' : 'b',
 			 m.flags.rtr ? 'R' : 'r',
 			 m.flags.extended ? 'E' : 'e');
 	if (not m.isRemoteTransmitRequest()) {
-		for (uint_fast8_t ii = 0; ii < m.length; ++ii) {
+		s.printf(", data = ");
+		for (uint_fast8_t ii = 0; ii < m.getLength(); ++ii) {
 			s.printf("%02x ", m.data[ii]);
 		}
 	}
@@ -149,6 +232,7 @@ operator << (modm::IOStream& s, const modm::can::Message& m)
 }
 
 } // modm namespace
+
 #endif
 
 #endif // MODM_CAN_MESSAGE_HPP
diff --git a/src/modm/architecture/module.lb b/src/modm/architecture/module.lb
index 1d880d0f79..062a3547c4 100644
--- a/src/modm/architecture/module.lb
+++ b/src/modm/architecture/module.lb
@@ -121,6 +121,12 @@ class Can(Module):
         module.description = FileReader("interface/can.md")
 
     def prepare(self, module, options):
+        module.add_option(
+            NumericOption(
+                name="message.buffer",
+                description="",
+                minimum=8, maximum=64,
+                default=8))
         return True
 
     def build(self, env):
@@ -128,7 +134,7 @@ class Can(Module):
         env.copy("interface/can.hpp")
         env.copy("interface/can.cpp")
         env.copy("interface/can_filter.hpp")
-        env.copy("interface/can_message.hpp")
+        env.template("interface/can_message.hpp.in")
 # -----------------------------------------------------------------------------
 
 class Clock(Module):
diff --git a/src/modm/platform/can/lpc/c_can.cpp.in b/src/modm/platform/can/lpc/c_can.cpp.in
index 1898ac0733..3813e52c33 100644
--- a/src/modm/platform/can/lpc/c_can.cpp.in
+++ b/src/modm/platform/can/lpc/c_can.cpp.in
@@ -64,7 +64,7 @@ sendMessageObject(const modm::can::Message & message, uint8_t messageObjectId)
 	msg_obj.mask    = 0x0;
 
 	// Set up the DLC
-	msg_obj.dlc     = message.getLength();
+	msg_obj.dlc     = message.getDataLengthCode();
 
 	// Copy the data.
 	// TODO reinterpret_cast or memcpy
@@ -107,7 +107,7 @@ readMessageObject(modm::can::Message & message, uint8_t messageObjectId)
 	message.setRemoteTransmitRequest( (msg_obj.mode_id & CAN_MSGOBJ_RTR) );
 
 	// Get DLC
-	message.length = msg_obj.dlc;
+	message.setDataLengthCode(msg_obj.dlc);
 
 	// Copy Data
 	// TODO Use memcpy or reinterpret_cast
diff --git a/src/modm/platform/can/socketcan/socketcan.cpp b/src/modm/platform/can/socketcan/socketcan.cpp
index 63012e8644..cc311da371 100644
--- a/src/modm/platform/can/socketcan/socketcan.cpp
+++ b/src/modm/platform/can/socketcan/socketcan.cpp
@@ -102,7 +102,7 @@ modm::platform::SocketCan::getMessage(can::Message& message)
 	if (nbytes > 0)
 	{
 		message.identifier = frame.can_id;
-		message.length = frame.can_dlc;
+		message.setDataLengthCode(frame.can_dlc);
 		message.setExtended(frame.can_id & CAN_EFF_FLAG);
 		message.setRemoteTransmitRequest(frame.can_id & CAN_RTR_FLAG);
 		for (uint8_t ii = 0; ii < frame.can_dlc; ++ii) {
diff --git a/src/modm/platform/can/stm32-fdcan/can.cpp.in b/src/modm/platform/can/stm32-fdcan/can.cpp.in
index b4aa788871..1499d9836e 100644
--- a/src/modm/platform/can/stm32-fdcan/can.cpp.in
+++ b/src/modm/platform/can/stm32-fdcan/can.cpp.in
@@ -1,6 +1,7 @@
 /*
  * Copyright (c) 2019, Raphael Lehmann
  * Copyright (c) 2021, Christopher Durand
+ * Copyright (c) 2022, Rasmus Kleist Hørlyck Sørensen
  *
  * This file is part of the modm project.
  *
@@ -32,9 +33,9 @@ modm::atomic::Queue<modm::can::Message, {{ options["buffer.tx"] }}> txQueue;
 %% endif
 
 struct RxMessage {
-    modm::can::Message message;
-    uint8_t filter_id;
-    uint16_t timestamp;
+	modm::can::Message message;
+	uint8_t filter_id;
+	uint16_t timestamp;
 };
 %% if options["buffer.rx"] > 0
 modm::atomic::Queue<RxMessage, {{ options["buffer.rx"] }}> rxQueue;
@@ -118,11 +119,7 @@ readMsg(modm::can::Message& message, uint8_t fifoIndex, uint8_t* filter_id, uint
 	}
 
 	const uint8_t dlcValue = MessageRam::RxDlc_t::get(rxHeader);
-	// TODO: fd large frames not supported yet
-	//if (rxHeader & MessageRam::RxHeader::FdFrame)
-		//message.setDLC(dlcValue);
-	// else
-	message.setLength(std::min<uint8_t>(8u, dlcValue));
+	message.setDataLengthCode(dlcValue);
 
 	// required for optimization in MessageRam::readData()
 	static_assert((std::size(decltype(message.data){}) % 4) == 0);
@@ -142,16 +139,9 @@ sendMsg(const modm::can::Message& message)
 		return false;
 	}
 
-	// TODO: for fdcan frame format with bit rate switching set:
-	// (TxFifoHeader::FdFrame | TxFifoHeader::RateSwitching)
-	MessageRam::TxFifoHeader_t txHeader{};
-
-	// TODO: large frame support
-	const uint8_t dlc = std::min<uint8_t>(8, message.getLength());
-	MessageRam::TxDlc_t::set(txHeader, dlc);
-
 	const uint8_t putIndex = retrieveTxFifoPutIndex();
 	const auto commonHeader = MessageRam::headerFromMessage(message);
+	const auto txHeader = MessageRam::txHeaderFromMessage(message);
 	MessageRam::writeTxHeaders(putIndex, commonHeader, txHeader);
 
 	// required for optimization in MessageRam::readData()
@@ -362,7 +352,6 @@ modm::platform::Fdcan{{ id }}::isMessageAvailable()
 %% endif
 }
 
-
 bool
 modm::platform::Fdcan{{ id }}::getMessage(can::Message& message, uint8_t *filter_id, uint16_t *timestamp)
 {
@@ -396,7 +385,6 @@ modm::platform::Fdcan{{ id }}::getMessage(can::Message& message, uint8_t *filter
 %% endif
 }
 
-
 bool
 modm::platform::Fdcan{{ id }}::isReadyToSend()
 {
@@ -422,12 +410,10 @@ modm::platform::Fdcan{{ id }}::sendMessage(const can::Message& message)
 		return false;
 %% endif
 	} else {
-		sendMsg(message);
-		return true;
+		return sendMsg(message);
 	}
 }
 
-
 modm::platform::Fdcan{{ id }}::BusState
 modm::platform::Fdcan{{ id }}::getBusState()
 {
diff --git a/src/modm/platform/can/stm32-fdcan/can.hpp.in b/src/modm/platform/can/stm32-fdcan/can.hpp.in
index 17efb3dce9..61d07f7911 100644
--- a/src/modm/platform/can/stm32-fdcan/can.hpp.in
+++ b/src/modm/platform/can/stm32-fdcan/can.hpp.in
@@ -1,6 +1,7 @@
 /*
  * Copyright (c) 2019, Raphael Lehmann
  * Copyright (c) 2021, Christopher Durand
+ * Copyright (c) 2022, Rasmus Kleist Hørlyck Sørensen
  *
  * This file is part of the modm project.
  *
@@ -185,10 +186,10 @@ public:
 	isMessageAvailable();
 
 	static bool
-	getMessage(can::Message& message, uint8_t *filter_id=nullptr, uint16_t *timestamp=nullptr);
+	isReadyToSend();
 
 	static bool
-	isReadyToSend();
+	getMessage(can::Message& message, uint8_t *filter_id=nullptr, uint16_t *timestamp=nullptr);
 
 	static bool
 	sendMessage(const can::Message& message);
diff --git a/src/modm/platform/can/stm32-fdcan/message_ram.hpp b/src/modm/platform/can/stm32-fdcan/message_ram.hpp
index f8173438dd..6fce25dd84 100644
--- a/src/modm/platform/can/stm32-fdcan/message_ram.hpp
+++ b/src/modm/platform/can/stm32-fdcan/message_ram.hpp
@@ -1,6 +1,7 @@
 /*
  * Copyright (c) 2019, Raphael Lehmann
  * Copyright (c) 2021, Christopher Durand
+ * Copyright (c) 2022, Rasmus Kleist Hørlyck Sørensen
  *
  * This file is part of the modm project.
  *
@@ -50,7 +51,7 @@ class MessageRam
 		StoreEvents		= Bit23,
 		// bit 22: reserved
 		FdFrame 		= Bit21,
-		RateSwitching	= Bit20,
+		BitRateSwitching	= Bit20,
 		// bit 19-16: dlc
 		// bit 15-0: reserved
 	};
@@ -65,7 +66,7 @@ class MessageRam
 		StoreEvents		= Bit23,
 		// bit 22: reserved
 		FdFrame 		= Bit21,
-		RateSwitching	= Bit20,
+		BitRateSwitching	= Bit20,
 		// bit 19-16: dlc
 		// bit 15-0: timestamp
 	};
@@ -210,7 +211,6 @@ class MessageRam
 			// copy in 32 bit words, memcpy is optimized to single store instruction
 			// CAN message buffer must fit full multiples of 4 bytes
 			std::memcpy(&outputData, &inData[i], 4);
-
 			*messageRam++ = outputData;
 		}
 	}
@@ -228,6 +228,19 @@ class MessageRam
 		return header;
 	}
 
+	/// Construct Tx Header from CanMessage
+	static TxFifoHeader_t
+	txHeaderFromMessage(const modm::can::Message& message)
+	{
+		TxFifoHeader_t header = (message.isFlexibleData() ? TxFifoHeader::FdFrame : TxFifoHeader(0))
+			| (message.isBitRateSwitching() ? TxFifoHeader::BitRateSwitching : TxFifoHeader(0));
+
+		const uint8_t dlc = message.getDataLengthCode();
+		MessageRam::TxDlc_t::set(header, dlc);
+
+		return header;
+	}
+
 	static void
 	setStandardFilter(uint8_t index, FilterType type, FilterConfig config, uint16_t id1, uint16_t id2)
 	{
diff --git a/src/modm/platform/can/stm32/can.cpp.in b/src/modm/platform/can/stm32/can.cpp.in
index 50c4c8032c..69340b03a1 100644
--- a/src/modm/platform/can/stm32/can.cpp.in
+++ b/src/modm/platform/can/stm32/can.cpp.in
@@ -176,7 +176,7 @@ sendMailbox(const modm::can::Message& message, uint32_t mailboxId)
 	}
 
 	// Set up the DLC
-	mailbox->TDTR = message.getLength();
+	mailbox->TDTR = message.getDataLengthCode();
 
 	// Set up the data field (copy the 8x8-bits into two 32-bit registers)
 	const uint8_t * modm_may_alias data = message.data;
@@ -205,8 +205,7 @@ readMailbox(modm::can::Message& message, uint32_t mailboxId, uint8_t* filter_id)
 		message.setExtended(false);
 	}
 	message.setRemoteTransmitRequest(rir & CAN_RI0R_RTR);
-
-	message.length = mailbox->RDTR & CAN_TDT1R_DLC;
+	message.setDataLengthCode(mailbox->RDTR & CAN_TDT1R_DLC);
 	if(filter_id != nullptr)
         (*filter_id) = (mailbox->RDTR & CAN_RDT1R_FMI) >> CAN_RDT1R_FMI_Pos;