diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 56d500e..3247450 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,7 +1,7 @@
 variables:
   SW_KERNEL: "0"
   SW_MAJOR: "1"
-  SW_MINOR: "4"
+  SW_MINOR: "5"
 
 before_script:
   - "echo $CI_BUILD_ID"
diff --git a/Makefile b/Makefile
index e6ad827..842bd1b 100644
--- a/Makefile
+++ b/Makefile
@@ -3,7 +3,7 @@ OS_NAME := kosmos
 
 # version numbering deployed by ci deploy script - no necessary for local build
 ifdef SW_KERNEL
-VERSION := -$(SW_KERNEL).$(SW_MAJOR).$(SW_MINOR)
+VERSION := -$(SW_KERNEL).$(SW_MAJOR).$(SW_MINOR).$(CI_BUILD_ID)
 else
 VERSION :=
 endif
@@ -26,7 +26,7 @@ endif
 
 C_SOURCES := $(foreach folder, $(SRC_DIR), $(wildcard $(folder)/*.c))
 C_OBJECTS := $(C_SOURCES:%.c=$(OBJ_DIR)/%.o)
-C_DEPS := $(SOURCES:%.c=$(OBJ_DIR)/%.d)
+C_DEPS := $(C_SOURCES:%.c=$(OBJ_DIR)/%.d)
 
 all: $(MAIN_FILE)
 
diff --git a/config/make/stm32f4xx.mk b/config/make/stm32f4xx.mk
index f5c58bc..1f3f231 100644
--- a/config/make/stm32f4xx.mk
+++ b/config/make/stm32f4xx.mk
@@ -57,7 +57,8 @@ C_FLAGS += \
 	-Wno-unused-parameter \
 	-Wno-sign-compare \
 	-Wno-missing-prototypes \
-	-Wno-missing-declarations
+	-Wno-missing-declarations \
+	-Wno-missing-field-initializers
 
 L_FLAGS := \
 	-T mem.ld \
@@ -66,7 +67,8 @@ L_FLAGS := \
 	-nostartfiles \
 	-Xlinker --gc-sections \
 	-L"config/linker" \
-	--specs=nano.specs
+	--specs=nano.specs \
+	-u _printf_float
 
 ifeq ($(DEBUG),y)
 OPTIM = g
diff --git a/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h b/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h
index 9860cb4..91ab510 100755
--- a/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h
+++ b/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h
@@ -172,6 +172,11 @@ static TIM_OC_InitTypeDef t4_output_compare_cfg = {
 		.OCNIdleState = TIM_OCNIDLESTATE_SET
 };
 
+static TIM_MasterConfigTypeDef t4_master_cfg = {
+		.MasterOutputTrigger = TIM_TRGO_RESET,
+		.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE,
+};
+
 static const GPIO_InitTypeDef port_cfg_D15 = {
 		.Pin = GPIO_PIN_15,
 		.Mode = GPIO_MODE_AF_PP,
@@ -188,7 +193,9 @@ static const struct stm32f4_gpio t4c4_gpio = {
 static struct stm32f4_pwm str32f4_pwm_4 = {
 		.pwm_gpio = &t4c4_gpio,
 		.timer_handle = &tim4_handle,
+		.timer_src_frequency_MHz = 84,
 		.output_compare_cfg = &t4_output_compare_cfg,
+		.master_cfg = &t4_master_cfg,
 		.channel = TIM_CHANNEL_4,
 };
 
@@ -222,7 +229,9 @@ static const struct stm32f4_gpio stm32f4_pwm_t4c3_gpio = {
 static struct stm32f4_pwm str32f4_pwm_3 = {
 		.pwm_gpio = &stm32f4_pwm_t4c3_gpio,
 		.timer_handle = &tim4_handle,
+		.timer_src_frequency_MHz = 84,
 		.output_compare_cfg = &t4_output_compare_cfg,
+		.master_cfg = &t4_master_cfg,
 		.channel = TIM_CHANNEL_3,
 };
 
@@ -256,7 +265,9 @@ static const struct stm32f4_gpio stm32f4_pwm_t4c2_gpio = {
 static struct stm32f4_pwm str32f4_pwm_2 = {
 		.pwm_gpio = &stm32f4_pwm_t4c2_gpio,
 		.timer_handle = &tim4_handle,
+		.timer_src_frequency_MHz = 84,
 		.output_compare_cfg = &t4_output_compare_cfg,
+		.master_cfg = &t4_master_cfg,
 		.channel = TIM_CHANNEL_2,
 };
 
@@ -290,7 +301,9 @@ static const struct stm32f4_gpio stm32f4_pwm_t4c1_gpio = {
 static struct stm32f4_pwm str32f4_pwm_1 = {
 		.pwm_gpio = &stm32f4_pwm_t4c1_gpio,
 		.timer_handle = &tim4_handle,
+		.timer_src_frequency_MHz = 84,
 		.output_compare_cfg = &t4_output_compare_cfg,
+		.master_cfg = &t4_master_cfg,
 		.channel = TIM_CHANNEL_1,
 };
 
@@ -308,6 +321,133 @@ const struct driver pwm_1 = {
 		&pwm_ch1,
 };
 
+// PWM5 CHANNEL 2
+/* apb1 clock = 84MHz */
+/* period_reg = src_clk / presc   / cnt_clk     */
+/*       1679 = 84MHZ   / 1000 / 50Hz   - 1 */
+static TIM_HandleTypeDef tim5_handle = {
+		.Instance = TIM5,
+		.Init.Prescaler = 1000,
+		.Init.CounterMode = TIM_COUNTERMODE_UP,
+		.Init.Period = 1679,
+		.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1,
+		.Init.RepetitionCounter = 0,
+};
+
+static TIM_OC_InitTypeDef t5_output_compare_cfg = {
+		.OCMode = TIM_OCMODE_PWM1,
+		.Pulse = 840,
+		.OCPolarity = TIM_OCPOLARITY_HIGH,
+		.OCNPolarity = TIM_OCNPOLARITY_HIGH,
+		.OCFastMode = TIM_OCFAST_DISABLE,
+		.OCIdleState = TIM_OCIDLESTATE_SET,
+		.OCNIdleState = TIM_OCNIDLESTATE_SET
+};
+
+static TIM_MasterConfigTypeDef t5_master_cfg = {
+		.MasterOutputTrigger = TIM_TRGO_RESET,
+		.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE,
+};
+
+static const GPIO_InitTypeDef port_cfg_A1 = {
+		.Pin = GPIO_PIN_1,
+		.Mode = GPIO_MODE_AF_PP,
+		.Speed = GPIO_SPEED_FREQ_HIGH,
+		.Pull = GPIO_PULLUP,
+		.Alternate = GPIO_AF2_TIM5,
+};
+
+static const struct stm32f4_gpio t5c2_gpio = {
+		.port = GPIOA,
+		.pin = &port_cfg_A1,
+};
+
+static struct stm32f4_pwm str32f4_pwm5_c2 = {
+		.pwm_gpio = &t5c2_gpio,
+		.timer_handle = &tim5_handle,
+		.timer_src_frequency_MHz = 84,
+		.output_compare_cfg = &t5_output_compare_cfg,
+		.master_cfg = &t5_master_cfg,
+		.channel = TIM_CHANNEL_2,
+};
+
+static const struct pwm pwm5_ch2 = {
+		.arch_dep_device = &str32f4_pwm5_c2,
+		.fp = &stm32f4_pwm_fp,
+};
+
+#ifdef TEST_APP
+static const struct driver pwm5_c2 = {
+#else
+const struct driver pwm5_c2 = {
+#endif
+		DRIVER_TYPE_PWM,
+		&pwm5_ch2,
+};
+
+// PWM2 CHANNEL 4
+/* apb1 clock = 84MHz */
+/* period_reg = src_clk / presc / cnt_clk  */
+/*       1679 = 84MHZ   / 1000  / 50Hz - 1 */
+static TIM_HandleTypeDef t2_handle = {
+		.Instance = TIM2,
+		.Init.Prescaler = 1000,
+		.Init.CounterMode = TIM_COUNTERMODE_UP,
+		.Init.Period = 0xffff,
+		.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1,
+		.Init.RepetitionCounter = 0,
+};
+
+static TIM_IC_InitTypeDef t2_input_capture_init = {
+		.ICPrescaler = TIM_ICPSC_DIV1,
+		.ICFilter = 0,
+		.ICPolarity = TIM_ICPOLARITY_BOTHEDGE,
+		.ICSelection = TIM_ICSELECTION_INDIRECTTI,
+};
+
+static TIM_SlaveConfigTypeDef t2_slave_config = {
+		.SlaveMode = TIM_SLAVEMODE_RESET,
+		.InputTrigger = TIM_TS_TI1FP1,
+		.TriggerPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE,
+		.TriggerFilter = 0,
+};
+
+static GPIO_InitTypeDef port_cfg_b10 = {
+		.Pin = GPIO_PIN_10,
+		.Mode = GPIO_MODE_AF_OD,
+		.Speed = GPIO_SPEED_FREQ_HIGH,
+		.Pull = GPIO_NOPULL,
+		.Alternate = GPIO_AF1_TIM2,
+};
+
+static const struct stm32f4_gpio b10_gpio = {
+		.port = GPIOB,
+		.pin = &port_cfg_b10,
+};
+
+static struct stm32f4_pwm stm32f4_pwm2_c4 = {
+		.pwm_gpio = &b10_gpio,
+		.timer_handle = &t2_handle,
+		.timer_src_frequency_MHz = 84,
+		.input_capture_init = &t2_input_capture_init,
+		.slave_config = &t2_slave_config,
+		.channel = TIM_CHANNEL_4,
+};
+
+static const struct pwm pwm2_ch4 = {
+		.arch_dep_device = &stm32f4_pwm2_c4,
+		.fp = &stm32f4_pwm_fp,
+};
+
+#ifdef TEST_APP
+static const struct driver pwm2_c4 = {
+#else
+const struct driver pwm2_c4 = {
+#endif
+		DRIVER_TYPE_PWM,
+		&pwm2_ch4,
+};
+
 // UART 1
 static const GPIO_InitTypeDef port_cfg_uart1 = {
 		.Pin = GPIO_PIN_6 | GPIO_PIN_7,
@@ -373,138 +513,6 @@ const struct driver uart_1 = {
 		&__uart_1,
 };
 
-#if 0
-// GPIOC0
-static const GPIO_InitTypeDef port_cfg_C0 = {
-		GPIO_Pin_0,
-		GPIO_Mode_OUT,
-		GPIO_Speed_100MHz,
-		GPIO_OType_PP,
-		GPIO_PuPd_NOPULL
-};
-
-static const struct stm32f4_gpio stm32_f4_gpio_C0 = {
-		GPIOC,
-		&port_cfg_C0,
-		NULL,
-		NULL,
-		NULL,
-		NULL
-};
-
-static const struct gpio __gpio_c0 = {
-		(void*)&stm32_f4_gpio_C0,
-		&gpio_fp
-};
-
-#ifdef TEST_APP
-static const struct driver gpio_c0 = {
-#else
-const struct driver gpio_c0 = {
-#endif
-		DRIVER_TYPE_GPIO,
-		&__gpio_c0,
-};
-#endif
-#if 0
-// GPIO_C1
-static const GPIO_InitTypeDef port_cfg_C1 = {
-		GPIO_Pin_1,
-		GPIO_Mode_OUT,
-		GPIO_Speed_100MHz,
-		GPIO_OType_PP,
-		GPIO_PuPd_NOPULL
-};
-
-static const struct stm32f4_gpio stm32_f4_gpio_C1 = {
-		GPIOC,
-		&port_cfg_C1,
-		NULL,
-		NULL,
-		NULL,
-		NULL
-};
-
-static const struct gpio __gpio_c1 = {
-		(void*)&stm32_f4_gpio_C1,
-		&gpio_fp
-};
-
-#ifdef TEST_APP
-static const struct driver gpio_c1 = {
-#else
-const struct driver gpio_c1 = {
-#endif
-		DRIVER_TYPE_GPIO,
-		&__gpio_c1,
-};
-#endif
-#if 0
-// GPIO_C2
-static const GPIO_InitTypeDef port_cfg_C2 = {
-		GPIO_Pin_2,
-		GPIO_Mode_OUT,
-		GPIO_Speed_100MHz,
-		GPIO_OType_PP,
-		GPIO_PuPd_NOPULL
-};
-
-static const struct stm32f4_gpio stm32_f4_gpio_C2 = {
-		GPIOC,
-		&port_cfg_C2,
-		NULL,
-		NULL,
-		NULL,
-		NULL
-};
-
-static const struct gpio __gpio_c2 = {
-		(void*)&stm32_f4_gpio_C2,
-		&gpio_fp
-};
-
-#ifdef TEST_APP
-static const struct driver gpio_c2 = {
-#else
-const struct driver gpio_c2 = {
-#endif
-		DRIVER_TYPE_GPIO,
-		&__gpio_c2,
-};
-#endif
-#if 0
-// GPIO_C3
-static const GPIO_InitTypeDef port_cfg_C3 = {
-		GPIO_Pin_3,
-		GPIO_Mode_OUT,
-		GPIO_Speed_100MHz,
-		GPIO_OType_PP,
-		GPIO_PuPd_NOPULL
-};
-
-static const struct stm32f4_gpio stm32_f4_gpio_C3 = {
-		GPIOC,
-		&port_cfg_C3,
-		NULL,
-		NULL,
-		NULL,
-		NULL
-};
-
-static const struct gpio __gpio_c3 = {
-		(void*)&stm32_f4_gpio_C3,
-		&gpio_fp
-};
-
-#ifdef TEST_APP
-static const struct driver gpio_c3 = {
-#else
-const struct driver gpio_c3 = {
-#endif
-		DRIVER_TYPE_GPIO,
-		&__gpio_c3,
-};
-#endif
 //! \brief Setup the hardware of the stm32f4-discovery board.
 void board_init(void);
 
diff --git a/source/firmware/arch/stm32f4xx/driver/include/stm32f4_pwm.h b/source/firmware/arch/stm32f4xx/driver/include/stm32f4_pwm.h
index 709c838..d15e30e 100644
--- a/source/firmware/arch/stm32f4xx/driver/include/stm32f4_pwm.h
+++ b/source/firmware/arch/stm32f4xx/driver/include/stm32f4_pwm.h
@@ -14,7 +14,11 @@ struct stm32f4_pwm {
 	const struct stm32f4_gpio *pwm_gpio;
 	TIM_HandleTypeDef *timer_handle;
 	TIM_OC_InitTypeDef *output_compare_cfg;
+	TIM_MasterConfigTypeDef *master_cfg;
+	TIM_IC_InitTypeDef *input_capture_init;
+	TIM_SlaveConfigTypeDef *slave_config;
 	uint32_t channel;
+	uint32_t timer_src_frequency_MHz;
 };
 #pragma pack(pop)
 
@@ -22,11 +26,15 @@ struct stm32f4_pwm {
 int stm32f4_pwm_open(const void *pwm);
 int stm32f4_pwm_close(const void *pwm);
 int stm32f4_pwm_set_duty_cycle(const void *pwm, unsigned int duty_cycle_percent);
+int stm32f4_pwm_get_period_ns(const void *pwm);
+int stm32f4_pwm_get_pulse_width_ns(const void *pwm);
 
 static const struct pwm_fp stm32f4_pwm_fp = {
 		.open = stm32f4_pwm_open,
 		.close = stm32f4_pwm_close,
 		.set_duty_cycle = stm32f4_pwm_set_duty_cycle,
+		.get_period = stm32f4_pwm_get_period_ns,
+		.get_pulse_width = stm32f4_pwm_get_pulse_width_ns,
 };
 
 #endif /* SOURCE_FIRMWARE_ARCH_STM32F4XX_DRIVER_INCLUDE_STM32F4_PWM_H_ */
diff --git a/source/firmware/arch/stm32f4xx/driver/stm32f4_pwm.c b/source/firmware/arch/stm32f4xx/driver/stm32f4_pwm.c
index bca8c13..e16de95 100644
--- a/source/firmware/arch/stm32f4xx/driver/stm32f4_pwm.c
+++ b/source/firmware/arch/stm32f4xx/driver/stm32f4_pwm.c
@@ -9,53 +9,158 @@
 
 #include "stm32f4xx.h"
 
+#include "isr.h"
+
 #include "gpio.h"
 #include "stm32f4_gpio.h"
 
 #include "pwm.h"
 #include "stm32f4_pwm.h"
 
-#pragma pack(push)
-#pragma pack(1)
-struct stm32f4_pwm_object {
-	uint8_t used_channels;
-	uint32_t channel_1_max_period;
-	uint32_t channel_2_max_period;
-	uint32_t channel_3_max_period;
-	uint32_t channel_4_max_period;
+struct stm32f4_pwm_channel_object {
+	struct stm32f4_pwm *pwm;
+	uint32_t period;
+	uint32_t period_start;
+	uint32_t pulse;
+	uint32_t pulse_start;
+};
+
+struct stm32f4_pwm_timer_object {
+	struct stm32f4_pwm_channel_object channel_1;
+	struct stm32f4_pwm_channel_object channel_2;
+	struct stm32f4_pwm_channel_object channel_3;
+	struct stm32f4_pwm_channel_object channel_4;
+};
+
+struct stm32f4_pwm_object {
+	struct stm32f4_pwm_timer_object timer_2;
+	struct stm32f4_pwm_timer_object timer_4;
+	struct stm32f4_pwm_timer_object timer_5;
+};
+
+static volatile struct stm32f4_pwm_object pwm_object = {
+		.timer_2.channel_1.pwm = NULL,
+		.timer_2.channel_1.period = 0,
+		.timer_2.channel_1.period_start = 0,
+		.timer_2.channel_1.pulse = 0,
+		.timer_2.channel_1.pulse_start = 0,
+		.timer_2.channel_2.pwm = NULL,
+		.timer_2.channel_2.period = 0,
+		.timer_2.channel_2.period_start = 0,
+		.timer_2.channel_2.pulse = 0,
+		.timer_2.channel_2.pulse_start = 0,
+		.timer_2.channel_3.pwm = NULL,
+		.timer_2.channel_3.period = 0,
+		.timer_2.channel_3.period_start = 0,
+		.timer_2.channel_3.pulse = 0,
+		.timer_2.channel_3.pulse_start = 0,
+		.timer_2.channel_4.pwm = NULL,
+		.timer_2.channel_4.period = 0,
+		.timer_2.channel_4.period_start = 0,
+		.timer_2.channel_4.pulse = 0,
+		.timer_2.channel_4.pulse_start = 0,
+		.timer_4.channel_1.pwm = NULL,
+		.timer_4.channel_1.period = 0,
+		.timer_4.channel_1.period_start = 0,
+		.timer_4.channel_1.pulse = 0,
+		.timer_4.channel_1.pulse_start = 0,
+		.timer_4.channel_2.pwm = NULL,
+		.timer_4.channel_2.period = 0,
+		.timer_4.channel_2.period_start = 0,
+		.timer_4.channel_2.pulse = 0,
+		.timer_4.channel_2.pulse_start = 0,
+		.timer_4.channel_3.pwm = NULL,
+		.timer_4.channel_3.period = 0,
+		.timer_4.channel_3.period_start = 0,
+		.timer_4.channel_3.pulse = 0,
+		.timer_4.channel_3.pulse_start = 0,
+		.timer_4.channel_4.pwm = NULL,
+		.timer_4.channel_4.period = 0,
+		.timer_4.channel_4.period_start = 0,
+		.timer_4.channel_4.pulse = 0,
+		.timer_4.channel_4.pulse_start = 0,
+		.timer_5.channel_1.pwm = NULL,
+		.timer_5.channel_1.period = 0,
+		.timer_5.channel_1.period_start = 0,
+		.timer_5.channel_1.pulse = 0,
+		.timer_5.channel_1.pulse_start = 0,
+		.timer_5.channel_2.pwm = NULL,
+		.timer_5.channel_2.period = 0,
+		.timer_5.channel_2.period_start = 0,
+		.timer_5.channel_2.pulse = 0,
+		.timer_5.channel_2.pulse_start = 0,
+		.timer_5.channel_3.pwm = NULL,
+		.timer_5.channel_3.period = 0,
+		.timer_5.channel_3.period_start = 0,
+		.timer_5.channel_3.pulse = 0,
+		.timer_5.channel_3.pulse_start = 0,
+		.timer_5.channel_4.pwm = NULL,
+		.timer_5.channel_4.period = 0,
+		.timer_5.channel_4.period_start = 0,
+		.timer_5.channel_4.pulse = 0,
+		.timer_5.channel_4.pulse_start = 0,
 };
-#pragma pack(pop)
 
 int stm32f4_pwm_open(const void *pwm)
 {
 	if(NULL == pwm)
 		return -1;
 	struct stm32f4_pwm *this = (struct stm32f4_pwm *)pwm;
+	IRQn_Type irq_type = TIM2_IRQn;
 	stm32f4_gpio_open(this->pwm_gpio);
+	if(this->timer_handle->Instance == TIM2) {
+		__HAL_RCC_TIM2_CLK_ENABLE();
+		irq_type = TIM2_IRQn;
+		if(this->channel == TIM_CHANNEL_1)
+			pwm_object.timer_2.channel_1.pwm = this;
+		else if(this->channel == TIM_CHANNEL_2)
+			pwm_object.timer_2.channel_2.pwm = this;
+		else if(this->channel == TIM_CHANNEL_3)
+			pwm_object.timer_2.channel_3.pwm = this;
+		else if(this->channel == TIM_CHANNEL_4)
+			pwm_object.timer_2.channel_4.pwm = this;
+	}
 	if(this->timer_handle->Instance == TIM4) {
 		__HAL_RCC_TIM4_CLK_ENABLE();
+		irq_type = TIM4_IRQn;
+		if(this->channel == TIM_CHANNEL_1)
+			pwm_object.timer_4.channel_1.pwm = this;
+		else if(this->channel == TIM_CHANNEL_2)
+			pwm_object.timer_4.channel_2.pwm = this;
+		else if(this->channel == TIM_CHANNEL_3)
+			pwm_object.timer_4.channel_3.pwm = this;
+		else if(this->channel == TIM_CHANNEL_4)
+			pwm_object.timer_4.channel_4.pwm = this;
+	}
+	if(this->timer_handle->Instance == TIM5) {
+		__HAL_RCC_TIM5_CLK_ENABLE();
+		irq_type = TIM5_IRQn;
+		if(this->channel == TIM_CHANNEL_1)
+			pwm_object.timer_5.channel_1.pwm = this;
+		else if(this->channel == TIM_CHANNEL_2)
+			pwm_object.timer_5.channel_2.pwm = this;
+		else if(this->channel == TIM_CHANNEL_3)
+			pwm_object.timer_5.channel_3.pwm = this;
+		else if(this->channel == TIM_CHANNEL_4)
+			pwm_object.timer_5.channel_4.pwm = this;
 	}
-	HAL_TIM_PWM_Init(this->timer_handle);
-	HAL_TIM_PWM_ConfigChannel(this->timer_handle, this->output_compare_cfg, this->channel);
-
-	TIM_MasterConfigTypeDef sMasterConfig;
-	sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-	sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-	HAL_TIMEx_MasterConfigSynchronization(this->timer_handle, &sMasterConfig);
-
-	TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig;
-	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(this->timer_handle, &sBreakDeadTimeConfig);
-
-
 	HAL_TIM_Base_Start(this->timer_handle);
-	HAL_TIM_PWM_Start(this->timer_handle, this->channel);
+	if((NULL != this->output_compare_cfg) && (NULL != this->master_cfg)) {
+		/* pwm output */
+		HAL_TIM_PWM_Init(this->timer_handle);
+		HAL_TIM_PWM_ConfigChannel(this->timer_handle, this->output_compare_cfg, this->channel);
+		HAL_TIMEx_MasterConfigSynchronization(this->timer_handle, this->master_cfg);
+		HAL_TIM_PWM_Start(this->timer_handle, this->channel);
+	}
+	else if((NULL != this->input_capture_init) && (NULL != this->slave_config)) {
+		/* pwm input */
+		HAL_NVIC_SetPriority(irq_type, 5, 1);
+		HAL_TIM_IC_Init(this->timer_handle);
+		HAL_TIM_IC_ConfigChannel(this->timer_handle, this->input_capture_init, this->channel);
+		HAL_TIM_SlaveConfigSynchronization(this->timer_handle, this->slave_config);
+		HAL_TIM_IC_Start_IT(this->timer_handle, this->channel);
+		HAL_NVIC_EnableIRQ(irq_type);
+	}
 	return 0;
 }
 
@@ -66,7 +171,12 @@ int stm32f4_pwm_close(const void *pwm)
 	stm32f4_pwm_set_duty_cycle(pwm, 0);
 	struct stm32f4_pwm *this = (struct stm32f4_pwm *)pwm;
 	HAL_TIM_Base_Stop(this->timer_handle);
-	HAL_TIM_PWM_Stop(this->timer_handle, this->channel);
+	if((NULL != this->output_compare_cfg) && (NULL != this->master_cfg)) {
+		HAL_TIM_PWM_Stop(this->timer_handle, this->channel);
+	}
+	else if((NULL != this->input_capture_init) && (NULL != this->slave_config)) {
+		HAL_TIM_IC_Stop_IT(this->timer_handle, this->channel);
+	}
 
 	return 0;
 }
@@ -81,3 +191,259 @@ int stm32f4_pwm_set_duty_cycle(const void *pwm, unsigned int duty_cycle_percent)
 
 	return 0;
 }
+
+int stm32f4_pwm_get_period_ns(const void *pwm)
+{
+	if(NULL == pwm)
+		return -1;
+
+	struct stm32f4_pwm *this = (struct stm32f4_pwm *)pwm;
+	int ret = 0;
+
+	if(this->timer_handle->Instance == TIM2) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_2.channel_1.period;
+			return ret * pwm_object.timer_2.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_2.channel_2.period;
+			return ret * pwm_object.timer_2.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_2.channel_3.period;
+			return ret * pwm_object.timer_2.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_2.channel_4.period;
+			return ret * pwm_object.timer_2.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+	else if(this->timer_handle->Instance == TIM4) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_4.channel_1.period;
+			return ret * pwm_object.timer_4.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_4.channel_2.period;
+			return ret * pwm_object.timer_4.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_4.channel_3.period;
+			return ret * pwm_object.timer_4.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_4.channel_4.period;
+			return ret * pwm_object.timer_4.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+	else if(this->timer_handle->Instance == TIM5) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_5.channel_1.period;
+			return ret * pwm_object.timer_5.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_5.channel_2.period;
+			return ret * pwm_object.timer_5.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_5.channel_3.period;
+			return ret * pwm_object.timer_5.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_5.channel_4.period;
+			return ret * pwm_object.timer_5.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+
+	return 0;
+}
+
+int stm32f4_pwm_get_pulse_width_ns(const void *pwm)
+{
+	if(NULL == pwm)
+		return -1;
+
+	struct stm32f4_pwm *this = (struct stm32f4_pwm *)pwm;
+	int ret = 0;
+
+	if(this->timer_handle->Instance == TIM2) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_2.channel_1.pulse;
+			return ret * pwm_object.timer_2.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_2.channel_2.pulse;
+			return ret * pwm_object.timer_2.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_2.channel_3.pulse;
+			return ret * pwm_object.timer_2.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_2.channel_4.pulse;
+			return ret * pwm_object.timer_2.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+	else if(this->timer_handle->Instance == TIM4) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_4.channel_1.pulse;
+			return ret * pwm_object.timer_4.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_4.channel_2.pulse;
+			return ret * pwm_object.timer_4.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_4.channel_3.pulse;
+			return ret * pwm_object.timer_4.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_4.channel_4.pulse;
+			return ret * pwm_object.timer_4.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+	else if(this->timer_handle->Instance == TIM5) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_5.channel_1.pulse;
+			return ret * pwm_object.timer_5.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_5.channel_2.pulse;
+			return ret * pwm_object.timer_5.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_5.channel_3.pulse;
+			return ret * pwm_object.timer_5.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_5.channel_4.pulse;
+			return ret * pwm_object.timer_5.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+
+	return 0;
+}
+
+static void handle_irq_tim2_cc1(TIM_HandleTypeDef *timer_handle)
+{
+	/* Capture compare 1 event */
+	if(__HAL_TIM_GET_FLAG(timer_handle, TIM_FLAG_CC1) != RESET) {
+		if(__HAL_TIM_GET_IT_SOURCE(timer_handle, TIM_IT_CC1) != RESET) {
+			__HAL_TIM_CLEAR_IT(timer_handle, TIM_IT_CC1);
+			/* Input capture event */
+			if((timer_handle->Instance->CCMR2 & TIM_CCMR1_CC1S) != 0x00U) {
+				uint32_t read = timer_handle->Instance->CCR1;
+				if(pwm_object.timer_2.channel_1.pwm->input_capture_init->ICPolarity == TIM_ICPOLARITY_BOTHEDGE) {
+					GPIO_TypeDef* port = pwm_object.timer_2.channel_1.pwm->pwm_gpio->port;
+					uint16_t pin = pwm_object.timer_2.channel_1.pwm->pwm_gpio->pin->Pin;
+					if(GPIO_PIN_SET == HAL_GPIO_ReadPin(port, pin)) {
+						if(read > pwm_object.timer_2.channel_1.period_start)
+							pwm_object.timer_2.channel_1.period = read - pwm_object.timer_2.channel_1.period_start;
+						pwm_object.timer_2.channel_1.period_start = read;
+						pwm_object.timer_2.channel_1.pulse_start = read;
+					}
+					else {
+						if(read > pwm_object.timer_2.channel_1.pulse_start)
+							pwm_object.timer_2.channel_1.pulse = read - pwm_object.timer_2.channel_1.pulse_start;
+					}
+				}
+			}
+		}
+	}
+}
+
+static void handle_irq_tim2_cc2(TIM_HandleTypeDef *timer_handle)
+{
+	/* Capture compare 2 event */
+	if(__HAL_TIM_GET_FLAG(timer_handle, TIM_FLAG_CC2) != RESET) {
+		if(__HAL_TIM_GET_IT_SOURCE(timer_handle, TIM_IT_CC2) != RESET) {
+			__HAL_TIM_CLEAR_IT(timer_handle, TIM_IT_CC2);
+			/* Input capture event */
+			if((timer_handle->Instance->CCMR2 & TIM_CCMR1_CC2S) != 0x00U) {
+				uint32_t read = timer_handle->Instance->CCR2;
+				if(pwm_object.timer_2.channel_2.pwm->input_capture_init->ICPolarity == TIM_ICPOLARITY_BOTHEDGE) {
+					GPIO_TypeDef* port = pwm_object.timer_2.channel_2.pwm->pwm_gpio->port;
+					uint16_t pin = pwm_object.timer_2.channel_2.pwm->pwm_gpio->pin->Pin;
+					if(GPIO_PIN_SET == HAL_GPIO_ReadPin(port, pin)) {
+						if(read > pwm_object.timer_2.channel_2.period_start)
+							pwm_object.timer_2.channel_2.period = read - pwm_object.timer_2.channel_2.period_start;
+						pwm_object.timer_2.channel_2.period_start = read;
+						pwm_object.timer_2.channel_2.pulse_start = read;
+					}
+					else {
+						if(read > pwm_object.timer_2.channel_2.pulse_start)
+							pwm_object.timer_2.channel_2.pulse = read - pwm_object.timer_2.channel_2.pulse_start;
+					}
+				}
+			}
+		}
+	}
+}
+
+static void handle_irq_tim2_cc3(TIM_HandleTypeDef *timer_handle)
+{
+	/* Capture compare 3 event */
+	if(__HAL_TIM_GET_FLAG(timer_handle, TIM_FLAG_CC3) != RESET) {
+		if(__HAL_TIM_GET_IT_SOURCE(timer_handle, TIM_IT_CC3) != RESET) {
+			__HAL_TIM_CLEAR_IT(timer_handle, TIM_IT_CC3);
+			/* Input capture event */
+			if((timer_handle->Instance->CCMR2 & TIM_CCMR2_CC3S) != 0x00U) {
+				uint32_t read = timer_handle->Instance->CCR3;
+				if(pwm_object.timer_2.channel_3.pwm->input_capture_init->ICPolarity == TIM_ICPOLARITY_BOTHEDGE) {
+					GPIO_TypeDef* port = pwm_object.timer_2.channel_3.pwm->pwm_gpio->port;
+					uint16_t pin = pwm_object.timer_2.channel_3.pwm->pwm_gpio->pin->Pin;
+					if(GPIO_PIN_SET == HAL_GPIO_ReadPin(port, pin)) {
+						if(read > pwm_object.timer_2.channel_3.period_start)
+							pwm_object.timer_2.channel_3.period = read - pwm_object.timer_2.channel_3.period_start;
+						pwm_object.timer_2.channel_3.period_start = read;
+						pwm_object.timer_2.channel_3.pulse_start = read;
+					}
+					else {
+						if(read > pwm_object.timer_2.channel_3.pulse_start)
+							pwm_object.timer_2.channel_3.pulse = read - pwm_object.timer_2.channel_3.pulse_start;
+					}
+				}
+			}
+		}
+	}
+}
+
+static void handle_irq_tim2_cc4(TIM_HandleTypeDef *timer_handle)
+{
+	/* Capture compare 4 event */
+	if(__HAL_TIM_GET_FLAG(timer_handle, TIM_FLAG_CC4) != RESET) {
+		if(__HAL_TIM_GET_IT_SOURCE(timer_handle, TIM_IT_CC4) != RESET) {
+			__HAL_TIM_CLEAR_IT(timer_handle, TIM_IT_CC4);
+			/* Input capture event */
+			if((timer_handle->Instance->CCMR2 & TIM_CCMR2_CC4S) != 0x00U) {
+				uint32_t read = timer_handle->Instance->CCR4;
+				if(pwm_object.timer_2.channel_4.pwm->input_capture_init->ICPolarity == TIM_ICPOLARITY_BOTHEDGE) {
+					GPIO_TypeDef* port = pwm_object.timer_2.channel_4.pwm->pwm_gpio->port;
+					uint16_t pin = pwm_object.timer_2.channel_4.pwm->pwm_gpio->pin->Pin;
+					if(GPIO_PIN_SET == HAL_GPIO_ReadPin(port, pin)) {
+						if(read > pwm_object.timer_2.channel_4.period_start)
+							pwm_object.timer_2.channel_4.period = read - pwm_object.timer_2.channel_4.period_start;
+						pwm_object.timer_2.channel_4.period_start = read;
+						pwm_object.timer_2.channel_4.pulse_start = read;
+					}
+					else {
+						if(read > pwm_object.timer_2.channel_4.pulse_start)
+							pwm_object.timer_2.channel_4.pulse = read - pwm_object.timer_2.channel_4.pulse_start;
+					}
+				}
+			}
+		}
+	}
+}
+
+void TIM2_IRQHandler(void)
+{
+	enter_isr();
+
+	handle_irq_tim2_cc1(pwm_object.timer_2.channel_1.pwm->timer_handle);
+	handle_irq_tim2_cc2(pwm_object.timer_2.channel_2.pwm->timer_handle);
+	handle_irq_tim2_cc3(pwm_object.timer_2.channel_3.pwm->timer_handle);
+	handle_irq_tim2_cc4(pwm_object.timer_2.channel_4.pwm->timer_handle);
+
+	exit_isr();
+}
diff --git a/source/firmware/kernel/driver/driver.c b/source/firmware/kernel/driver/driver.c
index aa36474..32d052f 100644
--- a/source/firmware/kernel/driver/driver.c
+++ b/source/firmware/kernel/driver/driver.c
@@ -157,7 +157,13 @@ int drv_ioctl(const struct driver *driver, unsigned int cmd, const void *data)
 	case DRIVER_TYPE_PWM:
 		if(cmd == IOCTL_PWM_SET_DUTY_CYCLE) {
 			unsigned int *duty = (unsigned int *)data;
-			pwm_set_duty_cycle((const struct pwm *)(driver->device_driver), *duty);
+			return pwm_set_duty_cycle((const struct pwm *)(driver->device_driver), *duty);
+		}
+		else if(cmd == IOCTL_PWM_GET_PERIOD_NS) {
+			return pwm_get_period_ns((const struct pwm *)(driver->device_driver));
+		}
+		else if(cmd == IOCTL_PWM_GET_PULSE_WIDTH_NS) {
+			return pwm_get_pulse_width_ns((const struct pwm *)(driver->device_driver));
 		}
 		break;
 	case DRIVER_TYPE_RTC:
diff --git a/source/firmware/kernel/driver/include/pwm.h b/source/firmware/kernel/driver/include/pwm.h
index 6d1d883..c8673ee 100644
--- a/source/firmware/kernel/driver/include/pwm.h
+++ b/source/firmware/kernel/driver/include/pwm.h
@@ -14,22 +14,28 @@ typedef int (*pwm_fp_open_t)(const void*);
 //! \brief Function pointer to the close function.
 typedef int (*pwm_fp_close_t)(const void*);
 
-//! \brief Function pointer to the read function.
 typedef int (*pwm_fp_set_duty_cycle_t)(const void*, unsigned int duty_cycle_percent);
 
+typedef int (*pwm_fp_get_period_ns_t)(const void*);
+typedef int (*pwm_fp_get_pulse_width_ns_t)(const void*);
+
 struct pwm_fp {
 	const pwm_fp_open_t open;
 	const pwm_fp_close_t close;
 	const pwm_fp_set_duty_cycle_t set_duty_cycle;
+	const pwm_fp_get_period_ns_t get_period;
+	const pwm_fp_get_pulse_width_ns_t get_pulse_width;
 };
 
 struct pwm {
 	const void *arch_dep_device;	//!< Architecture depended pwm device (i.e. stm32f10x_pwm_t).
-	const struct pwm_fp *fp;				//!< Function pointer for the pwm driver access.
+	const struct pwm_fp *fp;		//!< Function pointer for the pwm driver access.
 };
 
 int pwm_open(const struct pwm *device);
 int pwm_close(const struct pwm *device);
 int pwm_set_duty_cycle(const struct pwm *device, unsigned int duty_cycle_percent);
+int pwm_get_period_ns(const struct pwm *device);
+int pwm_get_pulse_width_ns(const struct pwm *device);
 
 #endif /* SOURCE_FIRMWARE_KERNEL_DRIVER_INCLUDE_PWM_H_ */
diff --git a/source/firmware/kernel/driver/pwm.c b/source/firmware/kernel/driver/pwm.c
index a8e733e..422c741 100644
--- a/source/firmware/kernel/driver/pwm.c
+++ b/source/firmware/kernel/driver/pwm.c
@@ -31,3 +31,19 @@ int pwm_set_duty_cycle(const struct pwm *device, unsigned int duty_cycle_percent
 	pwm_fp_set_duty_cycle_t set = device->fp->set_duty_cycle;
 	return set(device->arch_dep_device, duty_cycle_percent);
 }
+
+int pwm_get_period_ns(const struct pwm *device)
+{
+	if(NULL == device)
+		return -1;
+	pwm_fp_get_period_ns_t get = device->fp->get_period;
+	return get(device->arch_dep_device);
+}
+
+int pwm_get_pulse_width_ns(const struct pwm *device)
+{
+	if(NULL == device)
+		return -1;
+	pwm_fp_get_pulse_width_ns_t get = device->fp->get_pulse_width;
+	return get(device->arch_dep_device);
+}
diff --git a/source/firmware/kernel/include/isr.h b/source/firmware/kernel/include/isr.h
index 28b78c2..33d826e 100644
--- a/source/firmware/kernel/include/isr.h
+++ b/source/firmware/kernel/include/isr.h
@@ -8,8 +8,8 @@
 #ifndef ISR_H_
 #define ISR_H_
 
-#ifdef ARCH_MSP430
-#include "msp430_isr.h"
+#ifdef ARCH_STM32F4XX
+#include "stm32f4xx_isr.h"
 #endif
 
 #endif /* ISR_H_ */
diff --git a/source/firmware/kernel/interface/driver.h b/source/firmware/kernel/interface/driver.h
index e91349d..6ce5b5d 100644
--- a/source/firmware/kernel/interface/driver.h
+++ b/source/firmware/kernel/interface/driver.h
@@ -8,7 +8,9 @@
 #ifndef SOURCE_FIRMWARE_KERNEL_DRIVER_INCLUDE_DRIVER_H_
 #define SOURCE_FIRMWARE_KERNEL_DRIVER_INCLUDE_DRIVER_H_
 
-#define IOCTL_PWM_SET_DUTY_CYCLE	0
+#define IOCTL_PWM_SET_DUTY_CYCLE		0
+#define IOCTL_PWM_GET_PERIOD_NS			1
+#define IOCTL_PWM_GET_PULSE_WIDTH_NS	2
 
 enum driver_type {
 	DRIVER_TYPE_ADC,
diff --git a/source/test/pwm/main.c b/source/test/pwm/main.c
index 01371b0..9823541 100644
--- a/source/test/pwm/main.c
+++ b/source/test/pwm/main.c
@@ -6,6 +6,8 @@
  */
 
 #include <stdbool.h>
+#include <string.h>
+#include <stdio.h>
 
 #include "driver.h"
 #include "board.h"
@@ -18,24 +20,52 @@
 
 int main(void)
 {
+	char print_buffer[80];
+
 	drv_open(&pwm_1);
 	drv_open(&pwm_2);
 	drv_open(&pwm_3);
 	drv_open(&pwm_4);
+
+	drv_open(&pwm5_c2);
+	drv_open(&pwm2_c4);
+
+	drv_open(&uart_1);
+
 	while(1) {
 		for(unsigned int duty = 0; duty < 100; duty++) {
 			drv_ioctl(&pwm_1, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
 			drv_ioctl(&pwm_2, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
 			drv_ioctl(&pwm_3, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
 			drv_ioctl(&pwm_4, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
-			sleep_ms(10);
+			sleep_ms(100);
+
+			int pulse = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PULSE_WIDTH_NS, NULL);
+			int period = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PERIOD_NS, NULL);
+			int frq = 1000000 / period;
+			sprintf(print_buffer, "Frequency: %dHz\r\n", frq);
+			drv_write(&uart_1, print_buffer, strlen(print_buffer));
+
+			int dist = 343 * pulse / 1000 / 2;
+			sprintf(print_buffer, "distance: %d mm\r\n", dist);
+			drv_write(&uart_1, print_buffer, strlen(print_buffer));
 		}
 		for(unsigned int duty = 98; duty > 0; duty--) {
 			drv_ioctl(&pwm_1, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
 			drv_ioctl(&pwm_2, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
 			drv_ioctl(&pwm_3, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
 			drv_ioctl(&pwm_4, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
-			sleep_ms(10);
+			sleep_ms(100);
+
+			int pulse = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PULSE_WIDTH_NS, NULL);
+			int period = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PERIOD_NS, NULL);
+			int frq = 1000000 / period;
+			sprintf(print_buffer, "Frequency: %dHz\r\n", frq);
+			drv_write(&uart_1, print_buffer, strlen(print_buffer));
+
+			int dist = 343 * pulse / 1000 / 2;
+			sprintf(print_buffer, "distance: %d mm\r\n", dist);
+			drv_write(&uart_1, print_buffer, strlen(print_buffer));
 		}
 	}