diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
index 9510c6161f..2258f0f040 100644
--- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
+++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
@@ -57,20 +57,16 @@
 #define FDCAN2_IT0_IRQHandler      STM32_FDCAN2_IT0_HANDLER
 #define FDCAN2_IT1_IRQHandler      STM32_FDCAN2_IT1_HANDLER
 
-#if defined(STM32G4)
-// on G4 FIFO elements are spaced at 18 words
+// FIFO elements are spaced at 18 words
 #define FDCAN_FRAME_BUFFER_SIZE 18
-#else
-// on H7 they are spaced at 4 words
-#define FDCAN_FRAME_BUFFER_SIZE 4
-#endif
+
 
 //Message RAM Allocations in Word lengths
 
 #if defined(STM32H7)
-#define MAX_FILTER_LIST_SIZE 80U            //80 element Standard Filter List elements or 40 element Extended Filter List
-#define FDCAN_NUM_RXFIFO0_SIZE 104U         //26 Frames
-#define FDCAN_TX_FIFO_BUFFER_SIZE 128U      //32 Frames
+#define MAX_FILTER_LIST_SIZE 78U            //78 element Standard Filter List elements or 40 element Extended Filter List
+#define FDCAN_NUM_RXFIFO0_SIZE 108U         //6 Frames
+#define FDCAN_TX_FIFO_BUFFER_SIZE 126U      //7 Frames
 #define MESSAGE_RAM_END_ADDR 0x4000B5FC
 
 #elif defined(STM32G4)
@@ -243,6 +239,7 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing
 
     const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum);
     if ((prescaler < 1U) || (prescaler > 1024U)) {
+        Debug("Timings: No Solution found\n");
         return false;              // No solution
     }
 
@@ -295,6 +292,9 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing
         solution = BsPair(bs1_bs2_sum, uint8_t((7 * bs1_bs2_sum - 1) / 8));
     }
 
+    Debug("Timings: quanta/bit: %d, sample point location: %.1f%%\n",
+          int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F);
+
     /*
      * Final validation
      * Helpful Python:
@@ -305,12 +305,10 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing
      *
      */
     if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) {
+        Debug("Timings: Invalid Solution %lu %lu %d %d %lu \n", pclk, prescaler, int(solution.bs1), int(solution.bs2), (pclk / (prescaler * (1 + solution.bs1 + solution.bs2))));
         return false;
     }
 
-    Debug("Timings: quanta/bit: %d, sample point location: %.1f%%\n",
-          int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F);
-
     out_timings.prescaler = uint16_t(prescaler - 1U);
     out_timings.sjw = 0;                                        // Which means one
     out_timings.bs1 = uint8_t(solution.bs1 - 1);
@@ -318,11 +316,23 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing
     return true;
 }
 
+static uint32_t bytes_to_word(const uint8_t data[]) {
+    return (uint32_t(data[3]) << 24) |
+           (uint32_t(data[2]) << 16) |
+           (uint32_t(data[1]) << 8)  |
+           (uint32_t(data[0]) << 0);
+}
+
 int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
                        CanIOFlags flags)
 {
+    if (!initialised_) {
+        return -1;
+    }
+
     stats.tx_requests++;
-    if (frame.isErrorFrame() || frame.dlc > 8 || !initialised_) {
+    if (frame.isErrorFrame() || (frame.dlc > 8 && !frame.isCanFDFrame()) ||
+        frame.dlc > 15) {
         stats.tx_rejected++;
         return -1;
     }
@@ -336,7 +346,7 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
         uint8_t index;
 
         if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) {
-            stats.tx_rejected++;
+            stats.tx_overflow++;
             return 0;    //we don't have free space
         }
         index = ((can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos);
@@ -357,9 +367,19 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
         //Write Data Length Code, and Message Marker
         buffer[1] =  frame.dlc << 16 | index << 24;
 
+        if (frame.isCanFDFrame()) {
+            buffer[1] |= FDF | BRS; // do CAN FD transfer and bit rate switching
+            stats.fdf_tx_requests++;
+            pending_tx_[index].canfd_frame = true;
+        } else {
+            pending_tx_[index].canfd_frame = false;
+        }
+
         // Write Frame to the message RAM
-        buffer[2] = frame.data_32[0];
-        buffer[3] = frame.data_32[1];
+        const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
+        for (uint8_t i = 0, l = 2; i < data_length; i+=4, l++) {
+            buffer[l] = bytes_to_word(&frame.data[i]);
+        }
 
         //Set Add Request
         can_->TXBAR = (1 << index);
@@ -649,6 +669,19 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode)
                   (timings.bs2 << FDCAN_NBTP_NTSEG2_Pos)  |
                   (timings.prescaler << FDCAN_NBTP_NBRP_Pos));
 
+    if (!computeTimings(bitrate*8, timings)) { // Do 8x fast Data transmission for CAN FD frames
+        can_->CCCR &= ~FDCAN_CCCR_INIT;
+        uint32_t while_start_ms = AP_HAL::millis();
+        while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) {
+            if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) {
+                return false;
+            }
+        }
+        return false;
+    }
+    Debug("CANFD Timings: presc=%u bs1=%u bs2=%u\n",
+          unsigned(timings.prescaler), unsigned(timings.bs1), unsigned(timings.bs2));
+
     can_->DBTP = ((timings.bs1 << FDCAN_DBTP_DTSEG1_Pos) |
                   (timings.bs2 << FDCAN_DBTP_DTSEG2_Pos)  |
                   (timings.prescaler << FDCAN_DBTP_DBRP_Pos));
@@ -684,6 +717,8 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode)
 #endif
     can_->ILE = 0x3;
 
+    can_->CCCR |= FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE; // enable sending CAN FD frames, and Bitrate switching
+
     // If mode is Filtered then we finish the initialisation in configureFilter method
     // otherwise we finish here
     if (mode != FilteredMode) {
@@ -722,6 +757,8 @@ void CANIface::setupMessageRam()
 #else
     uint32_t num_elements = 0;
 
+    can_->RXESC = 0x777; //Support upto 64byte long frames
+    can_->TXESC = 0x7; //Support upto 64byte long frames
     // Rx FIFO 0 start address and element count
     num_elements = MIN((FDCAN_NUM_RXFIFO0_SIZE/FDCAN_FRAME_BUFFER_SIZE), 64U);
     if (num_elements) {
@@ -753,6 +790,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us)
 
             if (!pending_tx_[i].pushed) {
                 stats.tx_success++;
+                if (pending_tx_[i].canfd_frame) {
+                    stats.fdf_tx_success++;
+                }
                 pending_tx_[i].pushed = true;
             } else {
                 continue;
@@ -821,7 +861,7 @@ bool CANIface::readRxFIFO(uint8_t fifo_index)
     }
 
     // Read the frame contents
-    AP_HAL::CANFrame frame;
+    AP_HAL::CANFrame frame {};
     uint32_t id = frame_ptr[0];
     if ((id & IDE) == 0) {
         //Standard ID
@@ -835,10 +875,18 @@ bool CANIface::readRxFIFO(uint8_t fifo_index)
     if ((id & RTR) != 0) {
         frame.id |= AP_HAL::CANFrame::FlagRTR;
     }
+
+    if (frame_ptr[1] & FDF) {
+        frame.setCanFD(true);
+        stats.fdf_rx_received++;
+    } else {
+        frame.setCanFD(false);
+    }
+
     frame.dlc = (frame_ptr[1] & DLC_MASK) >> 16;
     uint8_t *data = (uint8_t*)&frame_ptr[2];
-    //We only handle Data Length of 8 Bytes for now
-    for (uint8_t i = 0; i < 8; i++) {
+
+    for (uint8_t i = 0; i < AP_HAL::CANFrame::MaxDataLen; i++) {
         frame.data[i] = data[i];
     }
 
@@ -1039,6 +1087,7 @@ void CANIface::get_stats(ExpandingString &str)
     CriticalSectionLocker lock;
     str.printf("tx_requests:    %lu\n"
                "tx_rejected:    %lu\n"
+               "tx_overflow:    %lu\n"
                "tx_success:     %lu\n"
                "tx_timedout:    %lu\n"
                "tx_abort:       %lu\n"
@@ -1047,9 +1096,13 @@ void CANIface::get_stats(ExpandingString &str)
                "rx_errors:      %lu\n"
                "num_busoff_err: %lu\n"
                "num_events:     %lu\n"
-               "ECR:            %lx\n",
+               "ECR:            %lx\n"
+               "fdf_rx:         %lu\n"
+               "fdf_tx_req:     %lu\n"
+               "fdf_tx:         %lu\n",
                stats.tx_requests,
                stats.tx_rejected,
+               stats.tx_overflow,
                stats.tx_success,
                stats.tx_timedout,
                stats.tx_abort,
@@ -1058,7 +1111,10 @@ void CANIface::get_stats(ExpandingString &str)
                stats.rx_errors,
                stats.num_busoff_err,
                stats.num_events,
-               stats.ecr);
+               stats.ecr,
+               stats.fdf_rx_received,
+               stats.fdf_tx_requests,
+               stats.fdf_tx_success);
 }
 #endif
 
diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h
index b133c4cc26..4d1dcb2f55 100644
--- a/libraries/AP_HAL_ChibiOS/CANFDIface.h
+++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h
@@ -61,6 +61,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
     static constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask
     static constexpr unsigned long RTR       = (0x20000000U); // Remote Transmission Request
     static constexpr unsigned long DLC_MASK  = (0x000F0000U); // Data Length Code
+    static constexpr unsigned long FDF       = (0x00200000U); // CAN FD Frame
+    static constexpr unsigned long BRS       = (0x00100000U); // Bit Rate Switching
+    
 
     /**
      * CANx register sets
@@ -151,6 +154,7 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
     struct {
         uint32_t tx_requests;
         uint32_t tx_rejected;
+        uint32_t tx_overflow;
         uint32_t tx_success;
         uint32_t tx_timedout;
         uint32_t tx_abort;
@@ -160,6 +164,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
         uint32_t num_busoff_err;
         uint32_t num_events;
         uint32_t ecr;
+        uint32_t fdf_tx_requests;
+        uint32_t fdf_tx_success;
+        uint32_t fdf_rx_received;
     } stats;
 
 public: