diff --git a/Software/Core/Inc/stm32h7xx_it.h b/Software/Core/Inc/stm32h7xx_it.h
index e75b0f9..ca985e9 100644
--- a/Software/Core/Inc/stm32h7xx_it.h
+++ b/Software/Core/Inc/stm32h7xx_it.h
@@ -61,6 +61,7 @@ void FDCAN1_IT0_IRQHandler(void);
 void FDCAN2_IT0_IRQHandler(void);
 void FDCAN1_IT1_IRQHandler(void);
 void FDCAN2_IT1_IRQHandler(void);
+void TIM8_CC_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 
 /* USER CODE END EFP */
diff --git a/Software/Core/Src/main.c b/Software/Core/Src/main.c
index 8b5cf2b..4b91a37 100644
--- a/Software/Core/Src/main.c
+++ b/Software/Core/Src/main.c
@@ -58,6 +58,8 @@ static uint8_t  dio_values[NUM_DIO_PINS];
 TIM_HandleTypeDef* PWM_TIM_MAP[3] = {&htim1, &htim4, &htim3};
 
 static uint8_t  pwm_ch_active[8];
+
+static uint16_t wss_flanks[2];
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
@@ -164,6 +166,10 @@ int main(void)
   // Init all channels as stopped
   memset(pwm_ch_active, 0, 8);
 
+  // Start input capture for WSS
+  HAL_TIM_IC_Start_IT(&htim8, TIM_CHANNEL_1);
+  HAL_TIM_IC_Start_IT(&htim8, TIM_CHANNEL_2);
+
   /* USER CODE END 2 */
 
   /* Infinite loop */
@@ -207,7 +213,8 @@ int main(void)
               break;
 
             case FIN:
-              /* NYI */
+              value = wss_flanks[signal->channel];
+              wss_flanks[signal->channel] = 0;
               break;
 
             default:
@@ -319,6 +326,45 @@ void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
   }
 }
 
+/*
+ * Input capture timing calculations:
+ * wheel speed = 8000 rpm * (11/48) = 30.55 rot/s
+ * if both flanks then 64 flanks per rot = 1956 flanks/s
+ * That's ~500µs per flank or ~1ms per notch
+ *
+ * TIM8 trigger prescaled = 96MHz/64 = 3MHz
+ * Max Filter is 15 samples, at 666.666ns per sample
+ * that's 10µs of filtering (flanks that are unstable
+ * for this long will not trigger a capture interrupt)
+ *
+ * If we want to do further filtering, we can either
+ * increase the prescaler and decrease the counter
+ * period or do it in software using
+ * HAL_TIM_ReadCapturedValue(&htim8, TIM_CHANNEL_X);
+ * to measure the time between events.
+ */
+
+void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
+
+  if (htim != &htim8 || htim->Channel < 1 || htim->Channel > 2)
+    return;
+
+  /*
+   * Channels go 1,2,4,8... so we count the trailing zeros to get the index.
+   * For this, we could use the nifty CTZ instruction, which gives us exactly that.
+   * BUT according to godbolt we save 2 instructions by using subtraction,
+   * which we can only do because we only use the first two channels.
+   * Why 2 instructions? well, one because -1 can be done in the immediate when
+   * writing to memory. Another one because ARMv7 32bit doesn't have CTZ but only
+   * CLZ, so it must do RBIT before to reverse the result.
+   *
+   * CAREFUL: When using more than 2 channels, you MUST use CTZ instead.
+   */
+  wss_flanks[htim->Channel-1]++;
+  //wss_flanks[__builtin_ctz(htim->Channel)]++;
+
+}
+
 void SetCCR(TIM_TypeDef* Instance, unsigned int ch, uint8_t dc) {
   (&(Instance->CCR1))[ch] = dc;
 }
diff --git a/Software/Core/Src/stm32h7xx_it.c b/Software/Core/Src/stm32h7xx_it.c
index 2aef363..50b6234 100644
--- a/Software/Core/Src/stm32h7xx_it.c
+++ b/Software/Core/Src/stm32h7xx_it.c
@@ -59,6 +59,7 @@ extern DMA_HandleTypeDef hdma_adc1;
 extern ADC_HandleTypeDef hadc1;
 extern FDCAN_HandleTypeDef hfdcan1;
 extern FDCAN_HandleTypeDef hfdcan2;
+extern TIM_HandleTypeDef htim8;
 /* USER CODE BEGIN EV */
 
 /* USER CODE END EV */
@@ -285,6 +286,20 @@ void FDCAN2_IT1_IRQHandler(void)
   /* USER CODE END FDCAN2_IT1_IRQn 1 */
 }
 
+/**
+  * @brief This function handles TIM8 capture compare interrupt.
+  */
+void TIM8_CC_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM8_CC_IRQn 0 */
+
+  /* USER CODE END TIM8_CC_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim8);
+  /* USER CODE BEGIN TIM8_CC_IRQn 1 */
+
+  /* USER CODE END TIM8_CC_IRQn 1 */
+}
+
 /* USER CODE BEGIN 1 */
 
 /* USER CODE END 1 */
diff --git a/Software/Core/Src/tim.c b/Software/Core/Src/tim.c
index bf5ba40..d1477d3 100644
--- a/Software/Core/Src/tim.c
+++ b/Software/Core/Src/tim.c
@@ -251,9 +251,9 @@ void MX_TIM8_Init(void)
 
   /* USER CODE END TIM8_Init 1 */
   htim8.Instance = TIM8;
-  htim8.Init.Prescaler = 0;
+  htim8.Init.Prescaler = 63;
   htim8.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim8.Init.Period = 65535;
+  htim8.Init.Period = 14999;
   htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   htim8.Init.RepetitionCounter = 0;
   htim8.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
@@ -268,10 +268,10 @@ void MX_TIM8_Init(void)
   {
     Error_Handler();
   }
-  sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
+  sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE;
   sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
   sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
-  sConfigIC.ICFilter = 0;
+  sConfigIC.ICFilter = 15;
   if (HAL_TIM_IC_ConfigChannel(&htim8, &sConfigIC, TIM_CHANNEL_1) != HAL_OK)
   {
     Error_Handler();
@@ -364,6 +364,9 @@ void HAL_TIM_IC_MspInit(TIM_HandleTypeDef* tim_icHandle)
     GPIO_InitStruct.Alternate = GPIO_AF3_TIM8;
     HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
 
+    /* TIM8 interrupt Init */
+    HAL_NVIC_SetPriority(TIM8_CC_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM8_CC_IRQn);
   /* USER CODE BEGIN TIM8_MspInit 1 */
 
   /* USER CODE END TIM8_MspInit 1 */
@@ -522,6 +525,8 @@ void HAL_TIM_IC_MspDeInit(TIM_HandleTypeDef* tim_icHandle)
     */
     HAL_GPIO_DeInit(GPIOC, WS1_Pin|WS2_Pin);
 
+    /* TIM8 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM8_CC_IRQn);
   /* USER CODE BEGIN TIM8_MspDeInit 1 */
 
   /* USER CODE END TIM8_MspDeInit 1 */
diff --git a/Software/sensor-node.ioc b/Software/sensor-node.ioc
index 339a94c..6d5b1d6 100644
--- a/Software/sensor-node.ioc
+++ b/Software/sensor-node.ioc
@@ -276,6 +276,7 @@ NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false
+NVIC.TIM8_CC_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 PA0.GPIOParameters=GPIO_Label
 PA0.GPIO_Label=A12
@@ -637,7 +638,13 @@ TIM6.Prescaler=4800-1
 TIM6.TIM_MasterOutputTrigger=TIM_TRGO_UPDATE
 TIM8.Channel-Input_Capture1_from_TI1=TIM_CHANNEL_1
 TIM8.Channel-Input_Capture2_from_TI2=TIM_CHANNEL_2
-TIM8.IPParameters=Channel-Input_Capture2_from_TI2,Channel-Input_Capture1_from_TI1
+TIM8.ICFilter_CH1=15
+TIM8.ICFilter_CH2=15
+TIM8.ICPolarity_CH1=TIM_INPUTCHANNELPOLARITY_BOTHEDGE
+TIM8.ICPolarity_CH2=TIM_INPUTCHANNELPOLARITY_BOTHEDGE
+TIM8.IPParameters=Channel-Input_Capture2_from_TI2,Channel-Input_Capture1_from_TI1,Prescaler,Period,ICFilter_CH1,ICFilter_CH2,ICPolarity_CH1,ICPolarity_CH2
+TIM8.Period=14999
+TIM8.Prescaler=63
 VP_MEMORYMAP_VS_MEMORYMAP.Mode=CurAppReg
 VP_MEMORYMAP_VS_MEMORYMAP.Signal=MEMORYMAP_VS_MEMORYMAP
 VP_SYS_VS_Systick.Mode=SysTick