diff --git a/.hgsubstate b/.hgsubstate
--- a/.hgsubstate
+++ b/.hgsubstate
@@ -1,1 +1,1 @@
-5b244673cf7f522b38dd6ac9ab8c45f334cdaf28 hydrobot-sharedlibs
+20618713ba15db54576863ba8bbf58ce14418d54 hydrobot-sharedlibs
diff --git a/Inc/config.h b/Inc/config.h
new file mode 100644
--- /dev/null
+++ b/Inc/config.h
@@ -0,0 +1,17 @@
+#ifndef _CONFIG_H_
+#define _CONFIG_H_
+
+#include "stm32f0xx_hal.h"
+
+#define DEFAULT_CAN_ID 0x208
+#define DEFAULT_BROADCAST_ID 0x00000000
+#define DEFAULT_DATA_RATE 1000
+#define DEFAULT_LED_BRIGHTNESS 100
+
+typedef struct {
+    uint8_t x;
+    uint16_t y;
+    uint32_t z;
+} flash_user_vars_t;
+
+#endif //_CONFIG_H_
diff --git a/Inc/gpio.h b/Inc/gpio.h
new file mode 100644
--- /dev/null
+++ b/Inc/gpio.h
@@ -0,0 +1,24 @@
+#ifndef _GPIO_H_
+#define _GPIO_H_
+
+#include "stm32f0xx_hal.h"
+#include <stdbool.h>
+#include <led.h>
+#include <protocol.h>
+#include <config.h>
+
+#define GPIO_NUM_LEDS 3
+
+typedef struct {
+    uint16_t pin;
+    GPIO_TypeDef* port;
+    TIM_HandleTypeDef* timer;
+    uint16_t channel;
+} gpio_led_t;
+
+void gpio_init(void);
+bool gpio_set_led(led_name_t led, bool value);
+bool gpio_toggle_led(led_name_t led);
+bool gpio_set_led_brightness(uint8_t brightness);
+
+#endif /*_GPIO_H_ */
diff --git a/Inc/stm32f0xx_it.h b/Inc/stm32f0xx_it.h
--- a/Inc/stm32f0xx_it.h
+++ b/Inc/stm32f0xx_it.h
@@ -52,6 +52,7 @@ void HardFault_Handler(void);
 void SVC_Handler(void);
 void PendSV_Handler(void);
 void SysTick_Handler(void);
+void CEC_CAN_IRQHandler(void);
 
 #ifdef __cplusplus
 }
diff --git a/Inc/system.h b/Inc/system.h
new file mode 100644
--- /dev/null
+++ b/Inc/system.h
@@ -0,0 +1,9 @@
+#ifndef _SYSTEM_H
+#define _SYSTEM_H
+
+#include <stdbool.h>
+#include "stm32f0xx_hal.h"
+
+void system_init(void);
+
+#endif //_SYSTEM_H
diff --git a/Makefile b/Makefile
--- a/Makefile
+++ b/Makefile
@@ -48,14 +48,12 @@ BUILD_DIR = build
 ######################################
 # C sources
 C_SOURCES =  \
-Src/system_stm32f0xx.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_can.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_i2c_ex.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_rcc_ex.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_pwr.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_dma.c \
-Src/stm32f0xx_hal_msp.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_i2c.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_flash.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_pwr_ex.c \
@@ -63,10 +61,19 @@ Drivers/STM32F0xx_HAL_Driver/Src/stm32f0
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_flash_ex.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_tim.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_rcc.c \
-Src/stm32f0xx_it.c \
-Src/main.c \
 Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_gpio.c \
-Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_cortex.c  
+Drivers/STM32F0xx_HAL_Driver/Src/stm32f0xx_hal_cortex.c \
+Src/gpio.c \
+Src/main.c \
+Src/stm32f0xx_hal_msp.c \
+Src/stm32f0xx_it.c \
+Src/system_stm32f0xx.c \
+Src/system.c \
+hydrobot-sharedlibs/src/can_buffer.c \
+hydrobot-sharedlibs/src/can.c \
+hydrobot-sharedlibs/src/flash.c \
+hydrobot-sharedlibs/src/led.c \
+hydrobot-sharedlibs/src/protocol.c
 
 # ASM sources
 ASM_SOURCES =  \
@@ -84,11 +91,11 @@ PERIFLIB_SOURCES =
 #######################################
 BINPATH = 
 PREFIX = arm-none-eabi-
-CC = $(BINPATH)/$(PREFIX)gcc
-AS = $(BINPATH)/$(PREFIX)gcc -x assembler-with-cpp
-CP = $(BINPATH)/$(PREFIX)objcopy
-AR = $(BINPATH)/$(PREFIX)ar
-SZ = $(BINPATH)/$(PREFIX)size
+CC = $(PREFIX)gcc
+AS = $(PREFIX)gcc -x assembler-with-cpp
+CP = $(PREFIX)objcopy
+AR = $(PREFIX)ar
+SZ = $(PREFIX)size
 HEX = $(CP) -O ihex
 BIN = $(CP) -O binary -S
  
@@ -126,8 +133,8 @@ C_INCLUDES =  \
 -IDrivers/STM32F0xx_HAL_Driver/Inc \
 -IDrivers/STM32F0xx_HAL_Driver/Inc/Legacy \
 -IDrivers/CMSIS/Device/ST/STM32F0xx/Include \
--IDrivers/CMSIS/Include
-
+-IDrivers/CMSIS/Include \
+-Ihydrobot-sharedlibs/inc
 
 # compile gcc flags
 ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections
diff --git a/STM32F042K6Tx_FLASH.ld b/STM32F042K6Tx_FLASH.ld
--- a/STM32F042K6Tx_FLASH.ld
+++ b/STM32F042K6Tx_FLASH.ld
@@ -42,7 +42,8 @@ ENTRY(Reset_Handler)
 MEMORY
 {
 RAM (xrw)      : ORIGIN = 0x20000000, LENGTH = 6K
-FLASH (rx)      : ORIGIN = 0x8000000, LENGTH = 32K
+FLASH (rx)      : ORIGIN = 0x8000000, LENGTH = 31K
+EEPROM (xrw)    : ORIGIN = 0x8007C00, LENGTH = 1K
 }
 
 /* Define output sections */
@@ -153,7 +154,12 @@ SECTIONS
     . = ALIGN(8);
   } >RAM
 
-  
+  .eeprom :
+  {
+    . = ALIGN(4);
+      *(.eeprom)
+    . = ALIGN(4);
+  } >EEPROM
 
   /* Remove information from the standard libraries */
   /DISCARD/ :
diff --git a/Src/gpio.c b/Src/gpio.c
new file mode 100644
--- /dev/null
+++ b/Src/gpio.c
@@ -0,0 +1,125 @@
+#include "gpio.h"
+
+TIM_HandleTypeDef htim1;
+
+uint8_t led_brightness = 0;
+
+gpio_led_t led1 = {GPIO_PIN_10, GPIOA, &htim1, TIM_CHANNEL_3};
+gpio_led_t led2 = {GPIO_PIN_9, GPIOA, &htim1, TIM_CHANNEL_2};
+gpio_led_t led3 = {GPIO_PIN_8, GPIOA, &htim1, TIM_CHANNEL_1};
+
+gpio_led_t* leds[GPIO_NUM_LEDS] = {&led1, &led2, &led3};
+
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
+
+void gpio_init(void)
+{
+    /* GPIO Ports Clock Enable */
+    __HAL_RCC_GPIOF_CLK_ENABLE();
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+
+//    /*Configure GPIO pin Output Level */
+//    HAL_GPIO_WritePin(GPIOA, LED3_Pin|LED2_Pin|LED1_Pin, GPIO_PIN_RESET);
+//
+//    /*Configure GPIO pins : PAPin PAPin PAPin */
+//    GPIO_InitStruct.Pin = LED3_Pin|LED2_Pin|LED1_Pin;
+//    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+//    GPIO_InitStruct.Pull = GPIO_NOPULL;
+//    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+//    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+    TIM_ClockConfigTypeDef sClockSourceConfig;
+    TIM_MasterConfigTypeDef sMasterConfig;
+    TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig;
+    TIM_OC_InitTypeDef sConfigOC;
+
+    htim1.Instance = TIM1;
+    htim1.Init.Prescaler = 0;
+    htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
+    htim1.Init.Period = 0xFFF;
+    htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+    htim1.Init.RepetitionCounter = 0;
+    HAL_TIM_Base_Init(&htim1);
+
+    sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+    HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig);
+
+    HAL_TIM_PWM_Init(&htim1);
+
+    sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+    sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+    HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig);
+
+    sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
+    sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
+    sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
+    sBreakDeadTimeConfig.DeadTime = 0;
+    sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
+    sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
+    sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
+    HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig);
+
+    sConfigOC.OCMode = TIM_OCMODE_PWM1;
+    sConfigOC.Pulse = 0;
+    sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
+    sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
+    sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+    sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
+    sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+    HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1);
+
+    HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2);
+
+    HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3);
+
+    HAL_TIM_MspPostInit(&htim1);
+
+    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
+    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
+    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
+}
+
+
+
+bool gpio_set_led(led_name_t led, bool value)
+{
+    bool result = true;
+
+    __HAL_TIM_SET_COMPARE(leds[led]->timer, leds[led]->channel, (uint16_t)(led_brightness * 40.95 * value));
+
+    return result;
+}
+
+bool gpio_toggle_led(led_name_t led)
+{
+    bool result = true;
+
+    if (__HAL_TIM_GET_COMPARE(leds[led]->timer, leds[led]->channel))
+    {
+        __HAL_TIM_SET_COMPARE(leds[led]->timer, leds[led]->channel, 0);
+    }
+    else
+    {
+        __HAL_TIM_SET_COMPARE(leds[led]->timer, leds[led]->channel, (uint16_t)(led_brightness * 40.95));
+    }
+
+    return result;
+}
+
+bool gpio_set_led_brightness(uint8_t brightness)
+{
+    bool result = true;
+
+    led_brightness = brightness;
+
+    for (uint8_t i = 0; i < GPIO_NUM_LEDS; i++)
+    {
+        gpio_led_t* led = leds[i];
+        if (__HAL_TIM_GET_COMPARE(led->timer, led->channel))
+        {
+            __HAL_TIM_SET_COMPARE(led->timer, led->channel, (uint16_t)(led_brightness * 40.95));
+        }
+    }
+
+    return result;
+}
diff --git a/Src/main.c b/Src/main.c
--- a/Src/main.c
+++ b/Src/main.c
@@ -1,317 +1,100 @@
-/**
-  ******************************************************************************
-  * File Name          : main.c
-  * Description        : Main program body
-  ******************************************************************************
-  ** This notice applies to any and all portions of this file
-  * that are not between comment pairs USER CODE BEGIN and
-  * USER CODE END. Other portions of this file, whether 
-  * inserted by the user or by software development tools
-  * are owned by their respective copyright owners.
-  *
-  * COPYRIGHT(c) 2017 STMicroelectronics
-  *
-  * Redistribution and use in source and binary forms, with or without modification,
-  * are permitted provided that the following conditions are met:
-  *   1. Redistributions of source code must retain the above copyright notice,
-  *      this list of conditions and the following disclaimer.
-  *   2. Redistributions in binary form must reproduce the above copyright notice,
-  *      this list of conditions and the following disclaimer in the documentation
-  *      and/or other materials provided with the distribution.
-  *   3. Neither the name of STMicroelectronics nor the names of its contributors
-  *      may be used to endorse or promote products derived from this software
-  *      without specific prior written permission.
-  *
-  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-  *
-  ******************************************************************************
-  */
-/* Includes ------------------------------------------------------------------*/
-#include "main.h"
 #include "stm32f0xx_hal.h"
-
-/* USER CODE BEGIN Includes */
-
-/* USER CODE END Includes */
-
-/* Private variables ---------------------------------------------------------*/
-CAN_HandleTypeDef hcan;
-
-TIM_HandleTypeDef htim1;
-
-/* USER CODE BEGIN PV */
-/* Private variables ---------------------------------------------------------*/
-
-/* USER CODE END PV */
+#include "can.h"
+#include "gpio.h"
+#include "system.h"
+#include "led.h"
+#include "protocol.h"
 
-/* Private function prototypes -----------------------------------------------*/
-void SystemClock_Config(void);
-static void MX_GPIO_Init(void);
-static void MX_CAN_Init(void);
-static void MX_TIM1_Init(void);
-                                    
-void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
-                                
-
-/* USER CODE BEGIN PFP */
-/* Private function prototypes -----------------------------------------------*/
-
-/* USER CODE END PFP */
-
-/* USER CODE BEGIN 0 */
-
-/* USER CODE END 0 */
 
 int main(void)
 {
 
-  /* USER CODE BEGIN 1 */
-
-  /* USER CODE END 1 */
-
-  /* MCU Configuration----------------------------------------------------------*/
-
-  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
-  HAL_Init();
-
-  /* USER CODE BEGIN Init */
+    HAL_Init();
+    system_init();
+    gpio_init();
 
-  /* USER CODE END Init */
-
-  /* Configure the system clock */
-  SystemClock_Config();
-
-  /* USER CODE BEGIN SysInit */
+    protocol_init(PROTOMODULE);
 
-  /* USER CODE END SysInit */
-
-  /* Initialize all configured peripherals */
-  MX_GPIO_Init();
-  MX_CAN_Init();
-  MX_TIM1_Init();
+    led_init();
+    led_blink(LED_STATUS, 2000);
+    led_blink_once(LED_CAN, 2000);
+    led_set(LED_ERROR, 0);
 
-  /* USER CODE BEGIN 2 */
-
-  /* USER CODE END 2 */
-
-  /* Infinite loop */
-  /* USER CODE BEGIN WHILE */
-  while (1)
-  {
-  /* USER CODE END WHILE */
-
-  /* USER CODE BEGIN 3 */
+    uint32_t loop_timer_1ms = 0;
+    uint32_t loop_timer_10ms = 0;
+    uint32_t loop_timer_200ms = 0;
+    uint32_t loop_timer_1s = 0;
+    uint32_t loop_timer_2s = 0;
+    uint32_t loop_timer_data = 0;
 
-  }
-  /* USER CODE END 3 */
-
-}
+    while (1)
+    {
+        can_receive();
 
-/** System Clock Configuration
-*/
-void SystemClock_Config(void)
-{
-
-  RCC_OscInitTypeDef RCC_OscInitStruct;
-  RCC_ClkInitTypeDef RCC_ClkInitStruct;
+        if (HAL_GetTick() - loop_timer_1ms >= 1)
+        {
+            can_process_receive_buffer();
+            led_update_all();
 
-    /**Initializes the CPU, AHB and APB busses clocks 
-    */
-  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
-  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
-  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
-  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
-  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
-  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL4;
-  RCC_OscInitStruct.PLL.PREDIV = RCC_PREDIV_DIV1;
-  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
-  {
-    _Error_Handler(__FILE__, __LINE__);
-  }
+            loop_timer_1ms = HAL_GetTick();
+        }
+
+        if (HAL_GetTick() - loop_timer_10ms >= 10)
+        {
 
-    /**Initializes the CPU, AHB and APB busses clocks 
-    */
-  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
-                              |RCC_CLOCKTYPE_PCLK1;
-  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
-  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
-  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
+            loop_timer_10ms = HAL_GetTick();
+        }
 
-  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)
-  {
-    _Error_Handler(__FILE__, __LINE__);
-  }
+        if (HAL_GetTick() - loop_timer_200ms >= 200)
+        {
+
+            loop_timer_200ms = HAL_GetTick();
+        }
 
-    /**Configure the Systick interrupt time 
-    */
-  HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
-
-    /**Configure the Systick 
-    */
-  HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
+        if (HAL_GetTick() - loop_timer_1s >= 1000)
+        {
 
-  /* SysTick_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
-}
-
-/* CAN init function */
-static void MX_CAN_Init(void)
-{
+            loop_timer_1s = HAL_GetTick();
+        }
+        if (HAL_GetTick() - loop_timer_2s >= 2000)
+        {
 
-  hcan.Instance = CAN;
-  hcan.Init.Prescaler = 16;
-  hcan.Init.Mode = CAN_MODE_NORMAL;
-  hcan.Init.SJW = CAN_SJW_1TQ;
-  hcan.Init.BS1 = CAN_BS1_1TQ;
-  hcan.Init.BS2 = CAN_BS2_1TQ;
-  hcan.Init.TTCM = DISABLE;
-  hcan.Init.ABOM = DISABLE;
-  hcan.Init.AWUM = DISABLE;
-  hcan.Init.NART = DISABLE;
-  hcan.Init.RFLM = DISABLE;
-  hcan.Init.TXFP = DISABLE;
-  if (HAL_CAN_Init(&hcan) != HAL_OK)
-  {
-    _Error_Handler(__FILE__, __LINE__);
-  }
+            loop_timer_2s = HAL_GetTick();
+        }
+        if (HAL_GetTick() - loop_timer_data >= (protocol_get_settings()->val.data_rate))
+        {
+
+            loop_timer_data = HAL_GetTick();
+        }
+    }
 
 }
 
-/* TIM1 init function */
-static void MX_TIM1_Init(void)
+bool protocol_set_output(protocol_message_t* message)
 {
-
-  TIM_MasterConfigTypeDef sMasterConfig;
-  TIM_OC_InitTypeDef sConfigOC;
-  TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig;
-
-  htim1.Instance = TIM1;
-  htim1.Init.Prescaler = 0;
-  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim1.Init.Period = 0;
-  htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-  htim1.Init.RepetitionCounter = 0;
-  htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
-  {
-    _Error_Handler(__FILE__, __LINE__);
-  }
-
-  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
-  {
-    _Error_Handler(__FILE__, __LINE__);
-  }
+    bool result = false;
 
-  sConfigOC.OCMode = TIM_OCMODE_PWM1;
-  sConfigOC.Pulse = 0;
-  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
-  sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
-  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
-  sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
-  sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
-  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
-  {
-    _Error_Handler(__FILE__, __LINE__);
-  }
-
-  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
-  {
-    _Error_Handler(__FILE__, __LINE__);
-  }
+    return result;
+}
 
-  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
-  {
-    _Error_Handler(__FILE__, __LINE__);
-  }
+bool protocol_get_data(protocol_message_t* message)
+{
+    bool result = false;
 
-  sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
-  sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
-  sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
-  sBreakDeadTimeConfig.DeadTime = 0;
-  sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
-  sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
-  sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
-  if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
-  {
-    _Error_Handler(__FILE__, __LINE__);
-  }
-
-  HAL_TIM_MspPostInit(&htim1);
-
+    return result;
 }
 
-/** Configure pins as 
-        * Analog 
-        * Input 
-        * Output
-        * EVENT_OUT
-        * EXTI
-*/
-static void MX_GPIO_Init(void)
+bool protocol_config(protocol_message_t* message)
 {
+    bool result = false;
+    protocol_data_key_t data = (protocol_data_key_t)(uint16_t)message->data.float_data;
 
-  /* GPIO Ports Clock Enable */
-  __HAL_RCC_GPIOF_CLK_ENABLE();
-  __HAL_RCC_GPIOA_CLK_ENABLE();
-
+    return result;
 }
 
-/* USER CODE BEGIN 4 */
-
-/* USER CODE END 4 */
-
-/**
-  * @brief  This function is executed in case of error occurrence.
-  * @param  None
-  * @retval None
-  */
-void _Error_Handler(char * file, int line)
+bool protocol_calibrate(protocol_message_t* message)
 {
-  /* USER CODE BEGIN Error_Handler_Debug */
-  /* User can add his own implementation to report the HAL error return state */
-  while(1) 
-  {
-  }
-  /* USER CODE END Error_Handler_Debug */ 
-}
-
-#ifdef USE_FULL_ASSERT
+    bool result = false;
 
-/**
-   * @brief Reports the name of the source file and the source line number
-   * where the assert_param error has occurred.
-   * @param file: pointer to the source file name
-   * @param line: assert_param error line source number
-   * @retval None
-   */
-void assert_failed(uint8_t* file, uint32_t line)
-{
-  /* USER CODE BEGIN 6 */
-  /* User can add his own implementation to report the file name and line number,
-    ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
-  /* USER CODE END 6 */
-
+    return result;
 }
-
-#endif
-
-/**
-  * @}
-  */ 
-
-/**
-  * @}
-*/ 
-
-/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/Src/system.c b/Src/system.c
new file mode 100644
--- /dev/null
+++ b/Src/system.c
@@ -0,0 +1,29 @@
+#include "system.h"
+
+
+void system_init(void)
+{
+    RCC_OscInitTypeDef RCC_OscInitStruct;
+    RCC_ClkInitTypeDef RCC_ClkInitStruct;
+
+    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
+    RCC_OscInitStruct.HSEState = RCC_HSE_ON;
+    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
+    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
+    RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL4;
+    RCC_OscInitStruct.PLL.PREDIV = RCC_PREDIV_DIV1;
+    HAL_RCC_OscConfig(&RCC_OscInitStruct);
+
+    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK|RCC_CLOCKTYPE_PCLK1;
+    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
+    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
+    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
+    HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1);
+
+    HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
+
+    HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
+
+    /* SysTick_IRQn interrupt configuration */
+    HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
+}