diff --git a/libuavcan_drivers/stm32/test_stm32f107/src/board/board.cpp b/libuavcan_drivers/stm32/test_stm32f107/src/board/board.cpp
index 28779986db..f1e3d7a923 100644
--- a/libuavcan_drivers/stm32/test_stm32f107/src/board/board.cpp
+++ b/libuavcan_drivers/stm32/test_stm32f107/src/board/board.cpp
@@ -28,7 +28,9 @@ namespace board
 void init()
 {
     halInit();
+
     chibios_rt::System::init();
+
     sdStart(&STDOUT_SD, NULL);
 }
 
@@ -79,6 +81,19 @@ void boardInit(void)
         AFIO_MAPR_CAN_REMAP_REMAP3 |
         AFIO_MAPR_CAN2_REMAP       |
         AFIO_MAPR_USART2_REMAP;
+
+    /*
+     * Enabling the CAN controllers, then configuring GPIO functions for CAN_TX.
+     * Order matters, otherwise the CAN_TX pins will twitch, disturbing the CAN bus.
+     * This is why we can't perform this initialization using ChibiOS GPIO configuration.
+     */
+    RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
+#if UAVCAN_STM32_NUM_IFACES > 1
+    RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
+#endif
+
+    palSetPadMode(GPIOD, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
+    palSetPadMode(GPIOB, 6, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
 }
 
 }
diff --git a/libuavcan_drivers/stm32/test_stm32f107/src/sys/board.h b/libuavcan_drivers/stm32/test_stm32f107/src/sys/board.h
index 7de04ac5e4..c0608f24c1 100644
--- a/libuavcan_drivers/stm32/test_stm32f107/src/sys/board.h
+++ b/libuavcan_drivers/stm32/test_stm32f107/src/sys/board.h
@@ -20,6 +20,8 @@
 #define GPIO_PORT_LED    GPIOB
 #define GPIO_PIN_LED     9
 
+// GPIOD 10 is configured as OUTPUT, it is used as board reboot monitor.
+
 /*
  * I/O ports initial setup, this configuration is established soon after reset
  * in the initialization code.
@@ -48,7 +50,7 @@
 #define VAL_GPIOACRH            0x88888888      // 15..8
 #define VAL_GPIOAODR            0x00000000
 
-#define VAL_GPIOBCRL            0x8B488888
+#define VAL_GPIOBCRL            0x84488888      // CAN2 TX initialized as INPUT, it must be configured later!
 #define VAL_GPIOBCRH            0x88888828
 #define VAL_GPIOBODR            0x00000000
 
@@ -56,9 +58,9 @@
 #define VAL_GPIOCCRH            0x88888888
 #define VAL_GPIOCODR            0x00000000
 
-#define VAL_GPIODCRL            0x88b888B4
-#define VAL_GPIODCRH            0x88888888
-#define VAL_GPIODODR            0x00000000
+#define VAL_GPIODCRL            0x88b88844      // CAN1 TX initialized as INPUT, it must be configured later!
+#define VAL_GPIODCRH            0x88888288
+#define VAL_GPIODODR            ((1 << 10))
 
 #define VAL_GPIOECRL            0x88888888
 #define VAL_GPIOECRH            0x88888888