diff --git a/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp b/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp
index b28c5d8931..5208976cc8 100644
--- a/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp
+++ b/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp
@@ -16,6 +16,7 @@
  */
 
 #include "SoftSigReader.h"
+#include "hwdef/common/stm32_util.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 
@@ -77,7 +78,10 @@ bool SoftSigReader::attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan,
     _icu_drv->tim->DCR = STM32_TIM_DCR_DBA(0x0D) | STM32_TIM_DCR_DBL(1);
     //Enable DMA
     dmaStreamEnable(dma);
-    
+
+    //sets input filtering to 4 timer clock
+    stm32_timer_set_input_filter(_icu_drv->tim, chan, 2);
+
     //Start Timer
     icuStartCapture(_icu_drv);
     return true;
diff --git a/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp
index feffd17bdf..a93ddd8044 100644
--- a/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp
+++ b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp
@@ -15,6 +15,7 @@
  */
 
 #include "SoftSigReaderInt.h"
+#include "hwdef/common/stm32_util.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 
@@ -47,6 +48,8 @@ void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan)
     channel_config.alvl = EICU_INPUT_ACTIVE_HIGH;
     channel_config.capture_cb = _irq_handler;
     eicuStart(_icu_drv, &icucfg);
+    //sets input filtering to 4 timer clock
+    stm32_timer_set_input_filter(_icu_drv->tim, chan, 2);
     eicuEnable(_icu_drv);
 }
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk
index 5738e31c14..fa102ba65c 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk
@@ -128,7 +128,8 @@ CSRC = $(STARTUPSRC) \
 	   $(HWDEF)/common/flash.c \
 	   $(HWDEF)/common/malloc.c \
 	   $(HWDEF)/common/stdio.c \
-	   $(HWDEF)/common/hrt.c
+	   $(HWDEF)/common/hrt.c \
+	   $(HWDEF)/common/stm32_util.c
 
 ifeq ($(USE_FATFS),yes)
 CSRC += $(HWDEF)/common/posix.c
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
new file mode 100644
index 0000000000..c108ffb148
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
@@ -0,0 +1,34 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "stm32_util.h"
+
+void stm32_timer_set_input_filter(stm32_tim_t *tim, uint8_t channel, uint8_t filter_mode)
+{
+    switch (channel) {
+    case 0:
+        tim->CCMR1 |= STM32_TIM_CCMR1_IC1F(filter_mode);
+        break;
+    case 1:
+        tim->CCMR1 |= STM32_TIM_CCMR1_IC2F(filter_mode);
+        break;
+    case 2:
+        tim->CCMR2 |= STM32_TIM_CCMR2_IC3F(filter_mode);
+        break;
+    case 3:
+        tim->CCMR2 |= STM32_TIM_CCMR2_IC4F(filter_mode);
+        break;
+    }
+}
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
new file mode 100644
index 0000000000..9a93dce6bf
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
@@ -0,0 +1,28 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include "hal.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void stm32_timer_set_input_filter(stm32_tim_t *tim, uint8_t channel, uint8_t filter_mode);
+
+#ifdef __cplusplus
+}
+#endif
+