diff --git a/G4:G431/Servo/hardware.c b/G4:G431/Servo/hardware.c
index b9b9bb1..578ec1c 100644
--- a/G4:G431/Servo/hardware.c
+++ b/G4:G431/Servo/hardware.c
@@ -23,7 +23,6 @@
 #include "servo.h"
 
 volatile uint32_t Tms = 0;
-volatile bool tim_triggered[SERVO_AMOUNT];
 
 /* Called when systick fires */
 void sys_tick_handler(){
@@ -65,18 +64,14 @@ static void tim3_setup(){
     TIM3->CCR2 = the_conf.startpulse[1];
     TIM3->CCR3 = the_conf.startpulse[2];
     TIM3->CCR4 = the_conf.startpulse[3];
-    // enable CCx interrupts
-    TIM3->DIER = TIM_DIER_CC1IE | TIM_DIER_CC2IE | TIM_DIER_CC3IE | TIM_DIER_CC4IE;
-    // enable main output
-    //TIM3->BDTR |= TIM_BDTR_MOE;
+    // enable main output (don't need for TIM3)
+    //TIMx->BDTR |= TIM_BDTR_MOE;
     // enable PWM output
     TIM3->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E |TIM_CCER_CC3E | TIM_CCER_CC4E;
     // enable timer & ARR buffering
     TIM3->CR1 |= TIM_CR1_CEN | TIM_CR1_ARPE;
     // update buffers
     TIM3->EGR = TIM_EGR_UG;
-    // and enable interrupt
-    NVIC_EnableIRQ(TIM3_IRQn);
 }
 
 // TIM3 channels: 1 - PA6 (AF2), 2 - PA7 (AF2), 3 - PB0 (AF2), 4 - PB1 (AF2)
@@ -99,12 +94,3 @@ void gpio_setup(){
     SysTick_Config(SysFreq / 1000); // arg should be < 0xffffff
     tim3_setup();
 }
-
-// update event flags
-void tim3_isr(){
-    if(TIM3->SR & TIM_SR_CC1IF) tim_triggered[0] = true;
-    if(TIM3->SR & TIM_SR_CC2IF) tim_triggered[1] = true;
-    if(TIM3->SR & TIM_SR_CC3IF) tim_triggered[2] = true;
-    if(TIM3->SR & TIM_SR_CC4IF) tim_triggered[3] = true;
-    TIM3->SR = 0;
-}
diff --git a/G4:G431/Servo/hardware.h b/G4:G431/Servo/hardware.h
index 5dee284..7e0b65b 100644
--- a/G4:G431/Servo/hardware.h
+++ b/G4:G431/Servo/hardware.h
@@ -35,7 +35,6 @@
 #define MAX_USART_SPEED 3000000
 
 extern volatile uint32_t Tms;
-extern volatile bool tim_triggered[SERVO_AMOUNT];
 
 void gpio_setup();
 
diff --git a/G4:G431/Servo/servo.bin b/G4:G431/Servo/servo.bin
index 0aaa07a..d3b6ec6 100755
Binary files a/G4:G431/Servo/servo.bin and b/G4:G431/Servo/servo.bin differ
diff --git a/G4:G431/Servo/servo.c b/G4:G431/Servo/servo.c
index e5c5189..76660c4 100644
--- a/G4:G431/Servo/servo.c
+++ b/G4:G431/Servo/servo.c
@@ -16,6 +16,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <stm32g4.h>
 #include <stdint.h>
 
 #include "flash.h"
@@ -26,10 +27,21 @@ static uint16_t servo_tagpos[SERVO_AMOUNT] = {SG90_MIDPULSE, SG90_MIDPULSE, SG90
 static uint16_t servo_speed[SERVO_AMOUNT] = {0, 0, 0, 0};
 
 void process_servo(){
+    static bool notinited = true;
+#if SERVO_AMOUNT != 4
+#error "Add timers' variables"
+#endif
+    static const uint32_t trgflags[SERVO_AMOUNT] = {TIM_SR_CC1IF, TIM_SR_CC2IF, TIM_SR_CC3IF, TIM_SR_CC4IF};
+    if(notinited){ // init tagpos with starting values from settings at first run
+        for(int i = 0; i < SERVO_AMOUNT; ++i) servo_tagpos[i] = the_conf.startpulse[i];
+        notinited = false;
+    }
+    uint32_t sr = TIM3->SR;
+    uint32_t clear_mask = 0;
     for(uint8_t i = 0; i < SERVO_AMOUNT; ++i){
         uint16_t curpos;
-        if(!get_servo(i, &curpos) || !tim_triggered[i]) continue;
-        tim_triggered[i] = false;
+        if(!get_servo(i, &curpos) || !(sr & trgflags[i])) continue;
+        clear_mask |= trgflags[i];
         if(servo_tagpos[i] == curpos || servo_speed[i] < 1) continue;
         int32_t newpos = curpos;
         if(servo_tagpos[i] > curpos){
@@ -41,12 +53,13 @@ void process_servo(){
         }
         set_servo(i, (uint16_t) newpos);
     }
+    if(clear_mask) TIM3->SR &= ~clear_mask;
 }
 
 // set speed in steps per tick
 bool servo_set_speed(uint8_t N, uint16_t s){
     if(N >= SERVO_AMOUNT) return false;
-    if(s < 1 || s > the_conf.maxspeed[N]) return false;
+    if(s > the_conf.maxspeed[N]) return false; // let s==0: this will allow to stop mowing to tagpos
     servo_speed[N] = s;
     return true;
 }
diff --git a/G4:G431/Servo/version.inc b/G4:G431/Servo/version.inc
index df3da95..aa08bfc 100644
--- a/G4:G431/Servo/version.inc
+++ b/G4:G431/Servo/version.inc
@@ -1,2 +1,2 @@
-#define BUILD_NUMBER "16"
+#define BUILD_NUMBER "28"
 #define BUILD_DATE "2026-07-29"