diff --git a/Mcu/f051/Inc/comparator.h b/Mcu/f051/Inc/comparator.h
index 0d659ed74..bf6a4933c 100644
--- a/Mcu/f051/Inc/comparator.h
+++ b/Mcu/f051/Inc/comparator.h
@@ -14,7 +14,7 @@
 //#define COMP_PA4 0b1000101
 //#define COMP_PA5 0b1010101
 
-//// High speed comparator
+// High speed comparator
  #define COMP_PA0 0b1100001
  #define COMP_PA4 0b1000001
  #define COMP_PA5 0b1010001
diff --git a/Mcu/f051/Src/comparator.c b/Mcu/f051/Src/comparator.c
index 21868340c..f32f14895 100644
--- a/Mcu/f051/Src/comparator.c
+++ b/Mcu/f051/Src/comparator.c
@@ -12,7 +12,6 @@
 COMP_TypeDef* active_COMP = COMP1;
 
 uint8_t getCompOutputLevel() { return LL_COMP_ReadOutputLevel(active_COMP); }
-uint8_t medium_speed_set;
 
 void maskPhaseInterrupts()
 {
@@ -36,12 +35,9 @@ void changeCompInput()
     }
 if((average_interval < 400)){
 COMP->CSR = COMP->CSR & ~(1<<2);
-
-medium_speed_set = 0;
 }
 if((average_interval > 600)){
 COMP->CSR  = COMP->CSR | 1<<2;
-medium_speed_set = 1;
 }
     if (rising) {
         EXTI->RTSR = 0x0;
diff --git a/Mcu/f421/Src/comparator.c b/Mcu/f421/Src/comparator.c
index bd4fcbf86..225bffb7b 100644
--- a/Mcu/f421/Src/comparator.c
+++ b/Mcu/f421/Src/comparator.c
@@ -6,8 +6,8 @@
  */
 
 #include "comparator.h"
-
 #include "targets.h"
+#include "common.h"
 
 uint8_t getCompOutputLevel() { return CMP->ctrlsts_bit.cmpvalue; }
 
@@ -39,6 +39,12 @@ void changeCompInput()
 //        EXINT->polcfg1 |= (uint32_t)EXTI_LINE;
 //        EXINT->polcfg2 = 0;
 //    }
+if((average_interval < 150)){
+CMP->ctrlsts = CMP->ctrlsts & ~(1<<2);
+}
+if((average_interval > 250)){
+CMP->ctrlsts  = CMP->ctrlsts | 1<<2;
+}
 		EXINT->polcfg1 = !rising << 21;
     EXINT->polcfg2 = rising << 21;
 }