diff --git a/main/gps.cpp b/main/gps.cpp
index 187788e..969790e 100644
--- a/main/gps.cpp
+++ b/main/gps.cpp
@@ -76,7 +76,7 @@ static union
   } ;
 } GPS_Burst;
                                                                                                    // for the autobaud on the GPS port
-const int GPS_BurstTimeout = 50; // [ms]
+const int GPS_BurstTimeout = 100; // [ms]
 
 // static const uint8_t  BaudRates=7;                                                                 // number of possible baudrates choices
 // static       uint8_t  BaudRateIdx=0;                                                               // actual choice
@@ -307,8 +307,7 @@ static void GPS_BurstStart(void)                                           // wh
 #endif
 #ifdef WITH_GPS_MTK
         if(Parameters.NavRate)
-        { strcpy(GPS_Cmd, "$PMTK220,");                                        // MTK command to change the navigation rate
-          uint8_t Len = strlen(GPS_Cmd);
+        { uint8_t Len = Format_String(GPS_Cmd, "$PMTK220,");                   // navigation rate
           uint16_t OneSec = 1000;
           Len += Format_UnsDec(GPS_Cmd+Len, OneSec/Parameters.NavRate);
           Len += NMEA_AppendCheck(GPS_Cmd, Len);
@@ -316,7 +315,18 @@ static void GPS_BurstStart(void)                                           // wh
           GPS_Cmd[Len++]='\n';
           GPS_Cmd[Len]=0;
           Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
-          strcpy(GPS_Cmd, "$PMTK353,");
+          GPS_Status.ModeConfig=1; }
+        if(Parameters.NavMode)
+        { uint8_t Len = Format_String(GPS_Cmd, "$PMTK886,");                                        // MTK command to change the navigation mode
+          GPS_Cmd[Len++]='0'+Parameters.NavMode;
+          Len += NMEA_AppendCheck(GPS_Cmd, Len);
+          GPS_Cmd[Len++]='\r';
+          GPS_Cmd[Len++]='\n';
+          GPS_Cmd[Len]=0;
+          Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
+          GPS_Status.ModeConfig=1; }
+        if(Parameters.GNSS)
+        { uint8_t Len = Format_String(GPS_Cmd, "$PMTK353,"); // GNSS configuration
           GPS_Cmd[Len++]='0'+Parameters.EnableGPS;   // search (or not) for GPS satellites
           GPS_Cmd[Len++]=',';
           GPS_Cmd[Len++]='0'+Parameters.EnableGLO;   // search (or not) for GLONASS satellites (L86 supports)
@@ -330,20 +340,7 @@ static void GPS_BurstStart(void)                                           // wh
           GPS_Cmd[Len++]='\r';
           GPS_Cmd[Len++]='\n';
           GPS_Cmd[Len]=0;
-          Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
-          GPS_Status.ModeConfig=1;
-        }
-        if(Parameters.NavMode)
-        { strcpy(GPS_Cmd, "$PMTK886,");                                        // MTK command to change the navigation mode
-          uint8_t Len = strlen(GPS_Cmd);
-          GPS_Cmd[Len++]='0'+Parameters.NavMode;
-          Len += NMEA_AppendCheck(GPS_Cmd, Len);
-          GPS_Cmd[Len++]='\r';
-          GPS_Cmd[Len++]='\n';
-          GPS_Cmd[Len]=0;
-          Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
-          GPS_Status.ModeConfig=1;
-        }
+          Format_String(GPS_UART_Write, GPS_Cmd, Len, 0); }
 #endif
       }
       if(!GPS_Status.BaudConfig)                                             // if GPS baud config is not done yet
@@ -472,7 +469,7 @@ static void GPS_BurstComplete(void)                                        // wh
       }
     }
     if(GPS_Pos[GPS_PosIdx].isValid())                                         // position is complete and locked
-    { if(Parameters.manGeoidSepar)                                         // if GeoidSepar is "manual" - this implies the GPS does not correct for it
+    { if(Parameters.manGeoidSepar)                                            // if GeoidSepar is "manual" - this implies the GPS does not correct for it
       { GPS_Pos[GPS_PosIdx].GeoidSeparation = Parameters.GeoidSepar;          // copy the manually set GeoidSepar
         GPS_Pos[GPS_PosIdx].Altitude -= Parameters.GeoidSepar; }              // correct the Altitude - we likely need a separate flag for this
       GPS_Pos[GPS_PosIdx].calcLatitudeCosine();
@@ -525,7 +522,7 @@ static void GPS_BurstComplete(void)                                        // wh
   }
   uint8_t NextPosIdx = (GPS_PosIdx+1)&PosPipeIdxMask;                         // next position to be recorded
   if( GPS_Pos[GPS_PosIdx].isTimeValid() && GPS_Pos[NextPosIdx].isTimeValid() )
-  { int32_t Period = GPS_Pos[GPS_PosIdx].calcTimeDiff(GPS_Pos[NextPosIdx]);
+  { int32_t Period = GPS_Pos[GPS_PosIdx].calcTimeDiff(GPS_Pos[NextPosIdx]);   // [1/100sec]
     if(Period>0) GPS_PosPeriod = (Period+GPS_PosPipeSize/2)/(GPS_PosPipeSize-1);
 #ifdef DEBUG_PRINT
     xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
@@ -545,9 +542,20 @@ static void GPS_BurstComplete(void)                                        // wh
   // Sec++; if(Sec>=60) Sec=0;
   // GPS_Pos[NextPosIdx].Sec=Sec;                                           // set the correct time for the next position
   GPS_Pos[NextPosIdx].copyTime(GPS_Pos[GPS_PosIdx]);                        // copy time from current position
-  GPS_Pos[NextPosIdx].incrTime();                                        // increment time by 1 sec
+  GPS_Pos[NextPosIdx].incrTimeFrac(GPS_PosPeriod);                             // increment time by the expected period
   Flight.Process(GPS_Pos[GPS_PosIdx]);
   // GPS_Pos[NextPosIdx].copyDate(GPS_Pos[GPS_PosIdx]);
+#ifdef DEBUG_PRINT
+  xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
+  Format_String(CONS_UART_Write, "GPS -> ");
+  Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[NextPosIdx].Sec, 2);
+  CONS_UART_Write('.');
+  Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[NextPosIdx].FracSec, 2);
+  Format_String(CONS_UART_Write, "s ");
+  Format_UnsDec(CONS_UART_Write, GPS_PosPeriod, 3, 2);
+  Format_String(CONS_UART_Write, "s\n");
+  xSemaphoreGive(CONS_Mutex);
+#endif
   GPS_PosIdx=NextPosIdx;                                                      // advance the index
   xEventGroupSetBits(GPS_Event, GPSevt_NewPos);
 }
@@ -557,16 +565,16 @@ static void GPS_BurstEnd(void)                                             // wh
 
 // ----------------------------------------------------------------------------
 
-GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac) // return GPS position closest to the given Sec.Frac
+GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac, bool Ready) // return GPS position closest to the given Sec.Frac
 { int16_t TargetTime = Frac+(int16_t)Sec*100;                            // target time including the seconds
   BestIdx=0; BestRes=0x7FFF;
   for(uint8_t Idx=0; Idx<GPS_PosPipeSize; Idx++)                         // run through the GPS positions stored in the pipe
   { GPS_Position *Pos=GPS_Pos+Idx;
-    if(!Pos->isReady) continue;                                          // skip those not-ready yet
+    if(Ready) if(!Pos->isReady) continue;                                // if only Ready positions requested: skip those not-ready yet
     int16_t Diff = TargetTime - (Pos->FracSec + (int16_t)Pos->Sec*100);  // difference from the target time
     if(Diff<(-3000)) Diff+=6000;                                         // wrap-around 60 sec
     else if(Diff>=3000) Diff-=6000;
-    if(abs(Diff)<abs(BestRes)) { BestRes=Diff; BestIdx=Idx; }          // store the smallest difference from target
+    if(abs(Diff)<abs(BestRes)) { BestRes=Diff; BestIdx=Idx; }            // store the smallest difference from target
   }
   return BestRes==0x7FFF ? 0:GPS_Pos+BestIdx; }
 
diff --git a/main/gps.h b/main/gps.h
index 6bc4522..9e3782c 100644
--- a/main/gps.h
+++ b/main/gps.h
@@ -48,7 +48,7 @@ uint32_t GPS_getBaudRate(void);             // [bps]
 
 GPS_Position *GPS_getPosition(void);
 GPS_Position *GPS_getPosition(int8_t Sec);                                                  // return GPS position for given Sec
-GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac); // return GPS position closest to the given Sec.Frac
+GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac, bool Ready=1); // return GPS position closest to the given Sec.Frac
 
 int16_t GPS_AverageSpeed(void);             // [0.1m/s] calc. average speed based on most recent GPS positions
 
diff --git a/main/ogn.h b/main/ogn.h
index 4a92163..bc55eb3 100644
--- a/main/ogn.h
+++ b/main/ogn.h
@@ -666,6 +666,11 @@ class GPS_Time
    bool isDateValid(void) const                      // is the GPS date valid ?
    { return (Year>=0) && (Month>=0) && (Day>=0); }
 
+   void incrTimeFrac(int8_t Frac)
+   { if(Frac>=100) { incrTime(); Frac-=100; }
+     if(Frac>=100-FracSec) { incrTime(); Frac-=100; }
+     FracSec+=Frac; }
+
    uint8_t incrTime(void)                            // increment HH:MM:SS by one second
    { Sec++;  if(Sec<60) return 0;
      Sec=0;
diff --git a/main/sens.cpp b/main/sens.cpp
index 8b1170e..a81c3f3 100644
--- a/main/sens.cpp
+++ b/main/sens.cpp
@@ -201,9 +201,11 @@ static void ProcBaro(void)
     uint16_t msTime = TimeSync_msTime(MeasTick);
 
     uint8_t Frac = Sec%10;                                           // [0.1s]
-    if(Frac==0)
-    { GPS_Position *PosPtr = GPS_getPosition(Sec/10);                // get GPS position record for this second
-#ifdef DEBUG_PRINT
+    // if(Frac==0)
+    { // GPS_Position *PosPtr = GPS_getPosition(Sec/10);                // get GPS position record for this second
+      uint8_t BestIdx; int16_t BestRes;
+      GPS_Position *PosPtr = GPS_getPosition(BestIdx, BestRes, Sec/10, Frac*10, 0);
+// #ifdef DEBUG_PRINT
       xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
       Format_String(CONS_UART_Write, "ProcBaro: ");
       Format_UnsDec(CONS_UART_Write, (uint16_t)Sec, 3, 1);
@@ -213,10 +215,12 @@ static void ProcBaro(void)
         CONS_UART_Write('.');
         Format_UnsDec(CONS_UART_Write, (uint16_t)PosPtr->FracSec, 2);
         CONS_UART_Write('s'); }
+      Format_String(CONS_UART_Write, " => ");
+      Format_SignDec(CONS_UART_Write, BestRes, 3, 2);
       Format_String(CONS_UART_Write, "\n");
       xSemaphoreGive(CONS_Mutex);
-#endif
-      if(PosPtr)                                                     // if found:
+// #endif
+      if(PosPtr && abs(BestRes)<=20)                                 // if found and small time error
       { PosPtr->Pressure    = Pressure;                              // [0.25Pa]
         PosPtr->StdAltitude = StdAltitude;                           // store standard pressure altitude
         PosPtr->ClimbRate   = ClimbRate/10;                          // [0.1m/s]