diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index 3256980434..a16b49e8b9 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -124,6 +124,34 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GARRAY(can_protocol,    2, "CAN3_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN),
 #endif
 
+#if HAL_CANFD_SUPPORTED
+    // @Param: CAN_FDMODE
+    // @DisplayName: Enable CANFD mode
+    // @Description: Enabling this option sets the CAN bus to be in CANFD mode with BRS.
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    // @RebootRequired: True
+    GSCALAR(can_fdmode,     "CAN_FDMODE", 0),
+
+    // @Param: CAN_FDBAUDRATE
+    // @DisplayName: Set up bitrate for data section on CAN1
+    // @Description: This sets the bitrate for the data section of CAN1.
+    // @Values: 1:1M, 2:2M, 4:4M, 5:5M, 8:8M
+    // @User: Advanced
+    // @RebootRequired: True
+    GARRAY(can_fdbaudrate,    0, "CAN_FDBAUDRATE", HAL_CANFD_SUPPORTED),
+
+#if HAL_NUM_CAN_IFACES >= 2
+    // @Param: CAN2_FDBAUDRATE
+    // @DisplayName: Set up bitrate for data section on CAN2
+    // @Description: This sets the bitrate for the data section of CAN2.
+    // @Values: 1:1M, 2:2M, 4:4M, 5:5M, 8:8M
+    // @User: Advanced
+    // @RebootRequired: True
+    GARRAY(can_fdbaudrate,    1, "CAN2_FDBAUDRATE", HAL_CANFD_SUPPORTED),
+#endif
+#endif
+
 #if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT)
     // @Param: FLASH_BOOTLOADER
     // @DisplayName: Trigger bootloader update
@@ -375,10 +403,6 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(scripting, "SCR_", AP_Scripting),
 #endif
 
-#if HAL_CANFD_SUPPORTED
-    // can node FD Out mode
-    GSCALAR(can_fdmode,     "CAN_FDMODE", 0),
-#endif
     AP_VAREND
 };
 
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 2bc7c46d93..412f200451 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -52,6 +52,8 @@ public:
         k_param_scripting,
         k_param_esc_telem_port,
         k_param_can_fdmode,
+        k_param_can_fdbaudrate0,
+        k_param_can_fdbaudrate1,
     };
 
     AP_Int16 format_version;
@@ -126,6 +128,7 @@ public:
 
 #if HAL_CANFD_SUPPORTED
     AP_Int8 can_fdmode;
+    AP_Int8 can_fdbaudrate[HAL_NUM_CAN_IFACES];
 #else
     static constexpr uint8_t can_fdmode = 0;
 #endif
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index 9d8a7b4949..3099e8b494 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1470,7 +1470,11 @@ void AP_Periph_FW::can_start()
         CANSensor::set_periph(i, can_protocol_cached[i], can_iface_periph[i]);
 #endif
         if (can_iface_periph[i] != nullptr) {
+#if HAL_CANFD_SUPPORTED
+            can_iface_periph[i]->init(g.can_baudrate[i], g.can_fdbaudrate[i]*1000000U, AP_HAL::CANIface::NormalMode);
+#else
             can_iface_periph[i]->init(g.can_baudrate[i], AP_HAL::CANIface::NormalMode);
+#endif
         }
         canardInit(&instances[i].canard, (uint8_t *)instances[i].canard_memory_pool, sizeof(instances[i].canard_memory_pool),
                 onTransferReceived, shouldAcceptTransfer, NULL);