diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5537aca..7681c8f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -22,6 +22,9 @@ endif()
 # Set the project name
 set(CMAKE_PROJECT_NAME weight_cell)
 
+# Color output
+set(CMAKE_CXX_FLAGSS "-fdiagnostics-color=always")
+
 # Include toolchain file
 include("cmake/gcc-arm-none-eabi.cmake")
 
diff --git a/Core/Src/main.cpp b/Core/Src/main.cpp
index 3a0a60c..6cb138f 100644
--- a/Core/Src/main.cpp
+++ b/Core/Src/main.cpp
@@ -1,70 +1,13 @@
-/* USER CODE BEGIN Header */
-/**
-  ******************************************************************************
-  * @file           : main.c
-  * @brief          : Main program body
-  ******************************************************************************
-  * @attention
-  *
-  * Copyright (c) 2024 STMicroelectronics.
-  * All rights reserved.
-  *
-  * This software is licensed under terms that can be found in the LICENSE file
-  * in the root directory of this software component.
-  * If no LICENSE file comes with this software, it is provided AS-IS.
-  *
-  ******************************************************************************
-  */
-/* USER CODE END Header */
-/* Includes ------------------------------------------------------------------*/
 #include "main.h"
 
+#include <cstdlib>
+
 #include "gpio.h"
 #include "stm32f4xx_hal.h"
 #include "usart.hpp"
 
-/* Private includes ----------------------------------------------------------*/
-/* USER CODE BEGIN Includes */
-
-/* USER CODE END Includes */
-
-/* Private typedef -----------------------------------------------------------*/
-/* USER CODE BEGIN PTD */
-
-/* USER CODE END PTD */
-
-/* Private define ------------------------------------------------------------*/
-/* USER CODE BEGIN PD */
-
-/* USER CODE END PD */
-
-/* Private macro -------------------------------------------------------------*/
-/* USER CODE BEGIN PM */
-
-/* USER CODE END PM */
-
-/* Private variables ---------------------------------------------------------*/
-
-/* USER CODE BEGIN PV */
-
-/* USER CODE END PV */
-
-/* Private function prototypes -----------------------------------------------*/
 void SystemClock_Config(void);
 
-/* USER CODE BEGIN PFP */
-
-/* USER CODE END PFP */
-
-/* Private user code ---------------------------------------------------------*/
-/* USER CODE BEGIN 0 */
-
-/* USER CODE END 0 */
-
-/**
-  * @brief  The application entry point.
-  * @retval int
-  */
 int main(void) {
 
     /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
@@ -81,19 +24,19 @@ int main(void) {
                                 UART_OVERSAMPLING_16);
 
     usart2.init();
+    usart2.println("");
     usart2.println("\r\nWeight cell init.");
 
-    while (1) {
-        usart2.println("\r\n Hello, world!  cell init.");
+    int i{0};
 
-        HAL_Delay(500);
+    while (1) {
+        std::string buf{"Iteration #"};
+
+        usart2.println(buf + std::to_string(i++));
+        HAL_Delay(1000);
     }
 }
 
-/**
-  * @brief System Clock Configuration
-  * @retval None
-  */
 void SystemClock_Config(void) {
     RCC_OscInitTypeDef RCC_OscInitStruct = {0};
     RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
@@ -132,10 +75,6 @@ void SystemClock_Config(void) {
     }
 }
 
-/* USER CODE BEGIN 4 */
-
-/* USER CODE END 4 */
-
 /**
   * @brief  This function is executed in case of error occurrence.
   * @retval None
diff --git a/Core/Src/sysmem.c b/Core/Src/sysmem.c
index 246470e..1dec8d2 100644
--- a/Core/Src/sysmem.c
+++ b/Core/Src/sysmem.c
@@ -27,7 +27,7 @@
 /**
  * Pointer to the current high watermark of the heap usage
  */
-static uint8_t *__sbrk_heap_end = NULL;
+static uint8_t* __sbrk_heap_end = NULL;
 
 /**
  * @brief _sbrk() allocates memory to the newlib heap and is used by malloc
@@ -50,30 +50,27 @@ static uint8_t *__sbrk_heap_end = NULL;
  * @param incr Memory size
  * @return Pointer to allocated memory
  */
-void *_sbrk(ptrdiff_t incr)
-{
-  extern uint8_t _end; /* Symbol defined in the linker script */
-  extern uint8_t _estack; /* Symbol defined in the linker script */
-  extern uint32_t _Min_Stack_Size; /* Symbol defined in the linker script */
-  const uint32_t stack_limit = (uint32_t)&_estack - (uint32_t)&_Min_Stack_Size;
-  const uint8_t *max_heap = (uint8_t *)stack_limit;
-  uint8_t *prev_heap_end;
+void* _sbrk(ptrdiff_t incr) {
+    extern uint8_t _end;             /* Symbol defined in the linker script */
+    extern uint8_t _estack;          /* Symbol defined in the linker script */
+    extern uint32_t _Min_Stack_Size; /* Symbol defined in the linker script */
+    const uint32_t stack_limit = (uint32_t)&_estack - (uint32_t)&_Min_Stack_Size;
+    const uint8_t* max_heap    = (uint8_t*)stack_limit;
+    uint8_t* prev_heap_end;
 
-  /* Initialize heap end at first call */
-  if (NULL == __sbrk_heap_end)
-  {
-    __sbrk_heap_end = &_end;
-  }
+    /* Initialize heap end at first call */
+    if (NULL == __sbrk_heap_end) {
+        __sbrk_heap_end = &_end;
+    }
 
-  /* Protect heap from growing into the reserved MSP stack */
-  if (__sbrk_heap_end + incr > max_heap)
-  {
-    errno = ENOMEM;
-    return (void *)-1;
-  }
+    /* Protect heap from growing into the reserved MSP stack */
+    if (__sbrk_heap_end + incr > max_heap) {
+        errno = ENOMEM;
+        return (void*)-1;
+    }
 
-  prev_heap_end = __sbrk_heap_end;
-  __sbrk_heap_end += incr;
+    prev_heap_end = __sbrk_heap_end;
+    __sbrk_heap_end += incr;
 
-  return (void *)prev_heap_end;
+    return (void*)prev_heap_end;
 }
diff --git a/Core/Src/usart.cpp b/Core/Src/usart.cpp
index 8a84580..f3be9cc 100644
--- a/Core/Src/usart.cpp
+++ b/Core/Src/usart.cpp
@@ -21,7 +21,7 @@ Usart::Usart(USART_TypeDef* usart, uint32_t baudRate, uint32_t wordLength, uint3
                     .OverSampling = overSampling}} {}
 
 bool Usart::init() {
-    GPIO_InitTypeDef GPIO_InitStruct = {0};
+    GPIO_InitTypeDef GPIO_InitStruct;
     if (mHandle.Instance == USART2) {
         // USART2 clock enable
         __HAL_RCC_USART2_CLK_ENABLE();
@@ -61,8 +61,12 @@ void Usart::println(const std::string_view str) {
 
 void Usart::tx(const std::string_view range) {
     for (uint32_t pt{0}; pt <= range.size(); pt += TX_BUFSIZE) {
-        HAL_UART_Transmit(&mHandle, reinterpret_cast<const uint8_t*>(range.begin() + pt),
-                          TX_BUFSIZE, TX_TIMEOUT_MS);
+        uint8_t txLen{TX_BUFSIZE};
+        if (range.length() < TX_BUFSIZE) {
+            txLen = range.length();
+        }
+        HAL_UART_Transmit(&mHandle, reinterpret_cast<const uint8_t*>(range.begin() + pt), txLen,
+                          TX_TIMEOUT_MS);
     }
 };