diff --git a/F1-nolib/chronometer/chrono.bin b/F1-nolib/chronometer/chrono.bin
index 2f8e1f6..8577c2e 100755
Binary files a/F1-nolib/chronometer/chrono.bin and b/F1-nolib/chronometer/chrono.bin differ
diff --git a/F1-nolib/chronometer/hardware.c b/F1-nolib/chronometer/hardware.c
index 22b4f60..61f6630 100644
--- a/F1-nolib/chronometer/hardware.c
+++ b/F1-nolib/chronometer/hardware.c
@@ -89,8 +89,8 @@ void hw_setup(){
 }
 
 void exti1_isr(){ // PPS - PA1
-    DBG("exti1");
     systick_correction();
+    DBG("exti1");
     EXTI->PR = EXTI_PR_PR1;
 }
 
diff --git a/F1-nolib/chronometer/hardware.h b/F1-nolib/chronometer/hardware.h
index cb2d123..00a3842 100644
--- a/F1-nolib/chronometer/hardware.h
+++ b/F1-nolib/chronometer/hardware.h
@@ -26,17 +26,6 @@
 
 #include "stm32f1.h"
 
-// usb commands
-// lower and upper limits to capture
-#define CMD_DISTMIN     "distmin"
-#define CMD_DISTMAX     "distmax"
-#define CMD_ADC1MIN     "adc1min"
-#define CMD_ADC2MIN     "adc2min"
-#define CMD_ADC1MAX     "adc1max"
-#define CMD_ADC2MAX     "adc2max"
-#define CMD_PRINTTIME   "time"
-#define CMD_STORECONF   "store"
-
 // onboard LEDs - PB8/PB9
 #define LED0_port   GPIOB
 #define LED0_pin    (1<<8)
diff --git a/F1-nolib/chronometer/main.c b/F1-nolib/chronometer/main.c
index 164e02b..cd68b43 100644
--- a/F1-nolib/chronometer/main.c
+++ b/F1-nolib/chronometer/main.c
@@ -39,8 +39,10 @@ volatile uint32_t Tms = 0;
 
 /* Called when systick fires */
 void sys_tick_handler(void){
-    ++Tms;
-    increment_timer();
+    ++Tms; // increment pseudo-milliseconds counter
+    if(++Timer == 1000){ // increment milliseconds counter
+        time_increment();
+    }
 }
 
 void iwdg_setup(){
@@ -171,57 +173,6 @@ static char *get_USB(){
     return NULL;
 }
 
-static void parse_USBCMD(char *cmd){
-#define CMP(a,b)  cmpstr(a, b, sizeof(b)-1)
-#define GETNUM(x) if(getnum(cmd+sizeof(x)-1, &N)) goto bad_number;
-    static uint8_t conf_modified = 0;
-    uint8_t succeed = 0;
-    int32_t N;
-    if(!cmd || !*cmd) return;
-    if(*cmd == '?'){ // help
-        USB_send("Commands:\n"
-                 CMD_DISTMIN   " - min distance threshold (cm)\n"
-                 CMD_DISTMAX   " - max distance threshold (cm)\n"
-                 CMD_PRINTTIME " - print time\n"
-                 CMD_STORECONF " - store new configuration in flash\n"
-                 );
-    }else if(CMP(cmd, CMD_PRINTTIME) == 0){
-        USB_send(get_time(&current_time, get_millis()));
-    }else if(CMP(cmd, CMD_DISTMIN) == 0){ // set low limit
-        DBG("CMD_DISTMIN");
-        GETNUM(CMD_DISTMIN);
-        if(N < 0 || N > 0xffff) goto bad_number;
-        if(the_conf.dist_min != (uint16_t)N){
-            conf_modified = 1;
-            the_conf.dist_min = (uint16_t) N;
-            succeed = 1;
-        }
-    }else if(CMP(cmd, CMD_DISTMAX) == 0){ // set low limit
-        DBG("CMD_DISTMAX");
-        GETNUM(CMD_DISTMAX);
-        if(N < 0 || N > 0xffff) goto bad_number;
-        if(the_conf.dist_max != (uint16_t)N){
-            conf_modified = 1;
-            the_conf.dist_max = (uint16_t) N;
-            succeed = 1;
-        }
-    }else if(CMP(cmd, CMD_STORECONF) == 0){ // store everything
-        DBG("Store");
-        if(conf_modified){
-            if(store_userconf()){
-                USB_send("Error: can't save data!\n");
-            }else{
-                conf_modified = 0;
-                succeed = 1;
-            }
-        }
-    }
-    if(succeed) USB_send("Success!\n");
-    return;
-  bad_number:
-    USB_send("Error: bad number!\n");
-}
-
 int main(void){
     uint32_t lastT = 0;
     sysreset();
@@ -230,7 +181,7 @@ int main(void){
     LED1_off();
     USBPU_OFF();
     usarts_setup();
-    SysTick_Config(SYSTICK_DEFLOAD);
+    SysTick_Config(SYSTICK_DEFCONF); // function SysTick_Config decrements argument!
     SEND("Chronometer version " VERSION ".\n");
     if(RCC->CSR & RCC_CSR_IWDGRSTF){ // watchdog reset occured
         SEND("WDGRESET=1\n");
@@ -280,10 +231,10 @@ int main(void){
         int r = 0;
         char *txt;
         if((txt = get_USB())){
-            parse_USBCMD(txt);
             DBG("Received data over USB:");
             DBG(txt);
-            USB_send(txt); // echo all back
+            if(parse_USBCMD(txt))
+                USB_send(txt); // echo back non-commands data
         }
 #if defined EBUG || defined USART1PROXY
         if(usartrx(1)){ // usart1 received data, store in in buffer
diff --git a/F1-nolib/chronometer/str.c b/F1-nolib/chronometer/str.c
index 1929765..f22cc17 100644
--- a/F1-nolib/chronometer/str.c
+++ b/F1-nolib/chronometer/str.c
@@ -16,8 +16,12 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include "flash.h"
 #include "str.h"
+#include "time.h"
 #include "usart.h"
+#include "usb.h"
+
 /**
  * @brief cmpstr - the same as strncmp
  * @param s1,s2 - strings to compare
@@ -44,3 +48,60 @@ char *getchr(const char *str, char symbol){
     }while(*(++str));
     return NULL;
 }
+
+/**
+ * @brief parse_USBCMD - parsing of string buffer got by USB
+ * @param cmd - buffer with commands
+ * @return 0 if got command, 1 if command not recognized
+ */
+int parse_USBCMD(char *cmd){
+#define CMP(a,b)  cmpstr(a, b, sizeof(b)-1)
+#define GETNUM(x) if(getnum(cmd+sizeof(x)-1, &N)) goto bad_number;
+    static uint8_t conf_modified = 0;
+    uint8_t succeed = 0;
+    int32_t N;
+    if(!cmd || !*cmd) return 0;
+    if(*cmd == '?'){ // help
+        USB_send("Commands:\n"
+                 CMD_DISTMIN   " - min distance threshold (cm)\n"
+                 CMD_DISTMAX   " - max distance threshold (cm)\n"
+                 CMD_PRINTTIME " - print time\n"
+                 CMD_STORECONF " - store new configuration in flash\n"
+                 );
+    }else if(CMP(cmd, CMD_PRINTTIME) == 0){
+        USB_send(get_time(&current_time, get_millis()));
+    }else if(CMP(cmd, CMD_DISTMIN) == 0){ // set low limit
+        DBG("CMD_DISTMIN");
+        GETNUM(CMD_DISTMIN);
+        if(N < 0 || N > 0xffff) goto bad_number;
+        if(the_conf.dist_min != (uint16_t)N){
+            conf_modified = 1;
+            the_conf.dist_min = (uint16_t) N;
+            succeed = 1;
+        }
+    }else if(CMP(cmd, CMD_DISTMAX) == 0){ // set low limit
+        DBG("CMD_DISTMAX");
+        GETNUM(CMD_DISTMAX);
+        if(N < 0 || N > 0xffff) goto bad_number;
+        if(the_conf.dist_max != (uint16_t)N){
+            conf_modified = 1;
+            the_conf.dist_max = (uint16_t) N;
+            succeed = 1;
+        }
+    }else if(CMP(cmd, CMD_STORECONF) == 0){ // store everything
+        DBG("Store");
+        if(conf_modified){
+            if(store_userconf()){
+                USB_send("Error: can't save data!\n");
+            }else{
+                conf_modified = 0;
+                succeed = 1;
+            }
+        }
+    }else return 1;
+    if(succeed) USB_send("Success!\n");
+    return 0;
+  bad_number:
+    USB_send("Error: bad number!\n");
+    return 0;
+}
diff --git a/F1-nolib/chronometer/str.h b/F1-nolib/chronometer/str.h
index 4b2e50d..95d86ea 100644
--- a/F1-nolib/chronometer/str.h
+++ b/F1-nolib/chronometer/str.h
@@ -19,7 +19,18 @@
 #ifndef STR_H__
 #define STR_H__
 
+// usb commands
+// lower and upper limits to capture
+#define CMD_DISTMIN     "distmin"
+#define CMD_DISTMAX     "distmax"
+#define CMD_ADC1MIN     "adc1min"
+#define CMD_ADC2MIN     "adc2min"
+#define CMD_ADC1MAX     "adc1max"
+#define CMD_ADC2MAX     "adc2max"
+#define CMD_PRINTTIME   "time"
+#define CMD_STORECONF   "store"
+
 int cmpstr(const char *s1, const char *s2, int n);
 char *getchr(const char *str, char symbol);
-
+int parse_USBCMD(char *cmd);
 #endif // STR_H__
diff --git a/F1-nolib/chronometer/time.c b/F1-nolib/chronometer/time.c
index 6f8f0c3..64908ba 100644
--- a/F1-nolib/chronometer/time.c
+++ b/F1-nolib/chronometer/time.c
@@ -18,20 +18,26 @@
 
 #include "GPS.h"
 #include "time.h"
+#ifdef EBUG
+#include "usart.h"
+#endif
 #include "usb.h"
 #include <string.h>
 
 volatile uint32_t Timer; // milliseconds counter
 curtime current_time = TMNOTINI;
-volatile int need_sync = 1;
 
-// SysTick->LOAD values for all milliseconds (RVR0) and last millisecond (RVR1)
-static uint32_t RVR0 = SYSTICK_DEFLOAD, RVR1 = SYSTICK_DEFLOAD;
+// ms counter in last correction by PPS
+static uint32_t last_corr_time = 0;
 
 static inline uint8_t atou(const char *b){
     return (b[0]-'0')*10 + b[1]-'0';
 }
 
+/**
+ * @brief set_time - set current time from GPS data
+ * @param buf - buffer with time data (HHMMSS)
+ */
 void set_time(const char *buf){
     uint8_t H = atou(buf) + TIMEZONE_GMT_PLUS;
     if(H > 23) H -= 24;
@@ -54,6 +60,10 @@ void time_increment(){
                 current_time.H = 0;
         }
     }
+#ifdef EBUG
+    SEND("time_increment():  ");
+    SEND(get_time(&current_time, 0));
+#endif
 }
 
 /**
@@ -93,9 +103,9 @@ char *get_time(curtime *Tm, uint32_t T){
         strcpy(bptr, " (not valid)");
         bptr += 12;
     }
-    if(need_sync){
-        strcpy(bptr, " need synchronisation");
-        bptr += 21;
+    if(Tms - last_corr_time > 1000){
+        strcpy(bptr, " need PPS sync");
+        bptr += 14;
     }
     *bptr++ = '\n';
     *bptr = 0;
@@ -103,15 +113,39 @@ char *get_time(curtime *Tm, uint32_t T){
 }
 
 /**
- * @brief get_millis - calculate milliseconds due to global fix parameter
- * @return milliseconds value
+ * @brief systick_correction
+ * Makes correction of system timer
+ * The default frequency of timer is 1kHz - 72000 clocks per interrupt
+ * So we check how much ticks there was for last one second - between PPS interrupts
+ * Their amount equal to M = `Timer` value x (SysTick->LOAD+1) + (SysTick->LOAD+1 - SysTick->VAL)
+ *    if `Timer` is very small, add 1000 to its value.
+ * We need 1000xN ticks instead of M
+ * if L = LOAD+1, then
+ * M = Timer*L + L - VAL; newL = L + D = M/1000
+ * 1000*D = M - 1000*L = L(Timer+1-1000) - VAL ->
+ * D = [L*(Timer-999) - VAL]/1000
+ * So correction equal to
+ *      [ (SysTick->LOAD + 1) * (Timer - 999) - SysTick->VAL ] / 1000
  */
-uint32_t get_millis(){
-    // TODO: calculate right millis
-    return Tms % 1000; // temporary gag
-}
-
 void systick_correction(){
+    SysTick->CTRL &= ~SysTick_CTRL_ENABLE_Msk; // stop systick for a while
+    int32_t systick_val = SysTick->VAL, L = SysTick->LOAD + 1, timer_val = Timer;
+    SysTick->VAL = SysTick->LOAD;
+    SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk; // start it again
+    Timer = 0;
+    if(Tms - last_corr_time < 2000){ // calculate corrections only if Timer was zeroed last time
+        if(timer_val < 500) timer_val += 1000; // timer already incremented in SysTick interrupt
+        else time_increment(); // counter less than 1000 -> need to increment time
+        int32_t D = L * (timer_val - 999) - systick_val;
+        D /= 1000;
+#ifdef EBUG
+        SEND("Delta: "); if(D < 0){usart_putchar(1, '-'); printu(1, -D);} else printu(1, D); newline();
+        SEND(get_time(&current_time, 0));
+#endif
+        SysTick->LOAD += D;
+    }
+    last_corr_time = Tms;
+#if 0
     uint32_t t = 0, ticks;
     static uint32_t ticksavr = 0, N = 0, last_corr_time = 0;
     // correct
@@ -150,14 +184,6 @@ void systick_correction(){
     }
 theend:
     last_corr_time = t;
+#endif
 }
 
-void increment_timer(){
-    ++Timer;
-    if(Timer == 999){
-        SysTick->LOAD = RVR1;
-    }else if(Timer == 1000){
-        SysTick->LOAD = RVR0;
-        time_increment();
-    }
-}
diff --git a/F1-nolib/chronometer/time.h b/F1-nolib/chronometer/time.h
index d4f8f6c..569524d 100644
--- a/F1-nolib/chronometer/time.h
+++ b/F1-nolib/chronometer/time.h
@@ -34,6 +34,9 @@
 
 #define TMNOTINI  {25,61,61}
 
+// current milliseconds
+#define get_millis()  (Timer)
+
 typedef struct{
     uint8_t H;
     uint8_t M;
@@ -51,9 +54,7 @@ extern volatile int need_sync;
 
 char *get_time(curtime *T, uint32_t m);
 void set_time(const char *buf);
-uint32_t get_millis(); // current milliseconds
 void time_increment();
 void systick_correction();
-void increment_timer();
 
 #endif // TIME_H__