diff --git a/Core/Src/main.c b/Core/Src/main.c
index d340e9f..5644656 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -390,6 +390,17 @@ void _putchar(char character) {
   HAL_UART_Transmit(&huart1, (uint8_t *)&character, 1, 1000);
 }
 
+static void update_frequency_profile(user_pm_vfs_preset_t profile) {
+    taskENTER_CRITICAL();
+
+    user_pm_scale_vfs(profile);
+    SysTick_Config(configCPU_CLOCK_HZ / configTICK_RATE_HZ);
+
+    HAL_UART_Init(&huart1);
+
+    taskEXIT_CRITICAL();
+}
+
 /* USER CODE END 4 */
 
 /* USER CODE BEGIN Header_StartDefaultTask */
@@ -402,28 +413,20 @@ void _putchar(char character) {
 void StartDefaultTask(void *argument)
 {
   /* USER CODE BEGIN 5 */
+
   HeapStats_t stats;
+  vPortGetHeapStats(&stats);
+  printf("Heap available: %d bytes.\r\n", stats.xAvailableHeapSpaceInBytes);
+
   /* Infinite loop */
   for(;;)
   {
-    taskENTER_CRITICAL();
-    user_pm_scale_vfs(USER_PM_VFS_HIGH_1);
-    HAL_UART_Init(&huart1);
-    taskEXIT_CRITICAL();
-
-    printf("Set frequency to 168MHz.\r\n");
+    update_frequency_profile(USER_PM_VFS_HIGH_1);
+    printf("[%012u] Set frequency to 168MHz.\r\n", xTaskGetTickCount());
     osDelay(10000);
 
-    vPortGetHeapStats(&stats);
-    printf("Heap available: %d bytes.\r\n", stats.xAvailableHeapSpaceInBytes);
-
-    taskENTER_CRITICAL();
-    user_pm_scale_vfs(USER_PM_VFS_MID_1);
-    HAL_UART_Init(&huart1);
-    taskEXIT_CRITICAL();
-
-    printf("Set frequency to 84MHz.\r\n");
-
+    update_frequency_profile(USER_PM_VFS_MID_1);
+    printf("[%012u] Set frequency to  84MHz.\r\n", xTaskGetTickCount());
     osDelay(10000);
   }
   /* USER CODE END 5 */
diff --git a/Core/Src/user_power_mgmt.c b/Core/Src/user_power_mgmt.c
index a7078a1..ec50ceb 100644
--- a/Core/Src/user_power_mgmt.c
+++ b/Core/Src/user_power_mgmt.c
@@ -160,6 +160,9 @@ HAL_StatusTypeDef user_pm_scale_vfs(user_pm_vfs_preset_t preset) {
         user_pm_error_handler();
     }
 
+    // Update global variables
+    SystemCoreClockUpdate();
+
     return HAL_OK;
 }
 
diff --git a/Makefile b/Makefile
index 8aa3e41..9f1b74c 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,5 @@
 ##########################################################################################################################
-# File automatically-generated by tool: [projectgenerator] version: [3.14.1] date: [Tue Aug 24 04:58:49 CST 2021] 
+# File automatically-generated by tool: [projectgenerator] version: [3.14.1] date: [Tue Aug 24 17:01:59 CST 2021] 
 ##########################################################################################################################
 
 # ------------------------------------------------