diff --git a/can.c b/can.c
index d93d9ba..3c8d2d8 100644
--- a/can.c
+++ b/can.c
@@ -144,7 +144,7 @@ void can_init(void)
   CanHandle.Init.Mode = CAN_MODE_NORMAL;
   CanHandle.Init.SyncJumpWidth = CAN_SJW_1TQ;
   CanHandle.Init.TimeSeg1 = CAN_BS1_6TQ;
-  CanHandle.Init.TimeSeg2 = CAN_BS2_5TQ;
+  CanHandle.Init.TimeSeg2 = CAN_BS2_1TQ;
   CanHandle.Init.Prescaler = 4;
 
   if (HAL_CAN_Init(&CanHandle) != HAL_OK)
diff --git a/main.cpp b/main.cpp
index 1bb0c77..174d8fe 100644
--- a/main.cpp
+++ b/main.cpp
@@ -229,14 +229,15 @@ int main()
 //#define UART_DMA_CHANNEL DMA1_Channel2
     //UART3_Init();
 
+#ifdef FEATURE_CAN
+    can_init();
+#endif
+
 #ifdef MOTOR_TEST
     int pwm = 0;
     int8_t dir = 1;
 #else
 
-    can_config();
-    MODIFY_REG(RCC->CR, RCC_CR_HSITRIM, (0x1aU << RCC_CR_HSITRIM_Pos));
-
 #ifndef FEATURE_CAN
     HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, sizeof(command));
 #endif // FEATURE_CAN