diff --git a/PSE-firmware b/PSE-firmware
index 7bcb3a7..d087767 160000
--- a/PSE-firmware
+++ b/PSE-firmware
@@ -1 +1 @@
-Subproject commit 7bcb3a7afa7bb6ca19d1c33939bd28d2a9db5b98
+Subproject commit d0877672a3653eeed5fe7b65185ec45d93943f26
diff --git a/stm32f1xx_hal.c b/stm32f1xx_hal.c
index ae85de9..342fb11 100644
--- a/stm32f1xx_hal.c
+++ b/stm32f1xx_hal.c
@@ -1,4 +1,5 @@
 #include "stm32f1xx_hal.h"
+#include <bits/time.h>
 #include <pthread.h>
 #include <stdio.h>
 #include <unistd.h>
@@ -18,8 +19,8 @@ void HAL_GPIO_TogglePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin){
   printf("toggling pin\n");
 }
 GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin){
-  printf("reading pin\n");
   uint8_t pin = get_1st_pin_from_bitmask(GPIO_Pin);
+  printf("reading pin %d\n", gpio_status[GPIOx->index][pin]);
   return gpio_status[GPIOx->index][pin];
 }
 void HAL_GPIO_WritePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin, GPIO_PinState PinState){
@@ -30,10 +31,22 @@ void HAL_GPIO_WritePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin, GPIO_PinState Pin
 
 static void* tim_handler(void* arg){
   TIM_HandleTypeDef* htim = arg;
+
+  struct timespec start, current;
+  clock_gettime(CLOCK_MONOTONIC, &start);
+  
   while(htim->started){
-    uint64_t delay_ps = 15625 * htim->presc * htim->period;
+    uint64_t delay_ps = (uint64_t) 15625 * (htim->presc+1) * (htim->period+1);
     printf("delay : %lu\n", delay_ps);
-    usleep(delay_ps / 1000000);
+
+    clock_gettime(CLOCK_MONOTONIC, &current);
+    uint64_t time = (uint64_t) 1000000000 * (current.tv_sec - start.tv_sec) + (current.tv_nsec - start.tv_nsec);
+    if(delay_ps / 1000 > time)
+      usleep((delay_ps / 1000 - time) / 1000);
+    else printf("oups");
+    
+    clock_gettime(CLOCK_MONOTONIC, &start);
+    
     HAL_TIM_PeriodElapsedCallback(htim);
   }
   return NULL;
diff --git a/stm32f1xx_hal.h b/stm32f1xx_hal.h
index 65db0c7..d3f367b 100644
--- a/stm32f1xx_hal.h
+++ b/stm32f1xx_hal.h
@@ -66,10 +66,12 @@ GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin);
 void HAL_GPIO_WritePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin, GPIO_PinState PinState);
 
 #define __HAL_TIM_SET_AUTORELOAD(__HANDLE__, __AUTORELOAD__) \
-  printf("setting autoreload to %d\n", __AUTORELOAD__);
+  __HANDLE__->period = __AUTORELOAD__; \
+  printf("setting period %d\n", __AUTORELOAD__);
 
 #define __HAL_TIM_SET_PRESCALER(__HANDLE__, __PRESC__) \
-  printf("setting prescaler to %d\n", __PRESC__);
+  __HANDLE__->presc = __PRESC__; \
+  printf("setting presc %d\n", __PRESC__);
 
 HAL_StatusTypeDef HAL_TIM_Base_Start_IT(TIM_HandleTypeDef *htim);
 HAL_StatusTypeDef HAL_TIM_Base_Stop_IT(TIM_HandleTypeDef *htim);