diff --git a/lib/Espfc/src/Connect/Cli.cpp b/lib/Espfc/src/Connect/Cli.cpp
index 86c697a..5c31bd0 100644
--- a/lib/Espfc/src/Connect/Cli.cpp
+++ b/lib/Espfc/src/Connect/Cli.cpp
@@ -912,15 +912,16 @@ void Cli::execute(CliCmd& cmd, Stream& s)
     printVersion(s);
     s.println();
 
-    s.print(F("config size: "));
-    s.println(sizeof(ModelConfig));
-
-    s.print(F("  free heap: "));
-    s.println(targetFreeHeap());
-
-    s.print(F("   cpu freq: "));
+    s.print(F("cpu freq: "));
     s.print(targetCpuFreq());
     s.println(F(" MHz"));
+
+    s.print(F("  memory: "));
+    s.print(sizeof(ModelConfig));
+    s.print(F(", "));
+    s.print(sizeof(ModelState));
+    s.print(F(", "));
+    s.println(targetFreeHeap());
   }
   else if(strcmp_P(cmd.args[0], PSTR("get")) == 0)
   {
@@ -1197,7 +1198,7 @@ void Cli::execute(CliCmd& cmd, Stream& s)
     }
     else
     {
-      s.print(F("NO_GYRO"));
+      s.print(F("NO GYRO"));
     }
 
     if(baro)
@@ -1207,10 +1208,6 @@ void Cli::execute(CliCmd& cmd, Stream& s)
       s.print('/');
       s.print(FPSTR(Device::BusDevice::getName(baro->getBus()->getType())));
     }
-    else
-    {
-      s.print(F(", NO_BARO"));
-    }
 
     if(mag)
     {
@@ -1219,9 +1216,10 @@ void Cli::execute(CliCmd& cmd, Stream& s)
       s.print('/');
       s.print(FPSTR(Device::BusDevice::getName(mag->getBus()->getType())));
     }
-    else
+
+    if(_model.state.gps.present)
     {
-      s.print(F(", NO_MAG"));
+      s.print(F(", GPS"));
     }
     s.println();
 
@@ -1243,7 +1241,7 @@ void Cli::execute(CliCmd& cmd, Stream& s)
     };
     const size_t armingDisableNamesLength = sizeof(armingDisableNames) / sizeof(armingDisableNames[0]);
 
-    s.print(F("arming flags:"));
+    s.print(F("   arm flags:"));
     for(size_t i = 0; i < armingDisableNamesLength; i++)
     {
       if(_model.state.mode.armingDisabledFlags & (1 << i)) {
@@ -1489,8 +1487,14 @@ void Cli::printGpsStatus(Stream& s, bool full) const
   s.print(_model.state.gps.dateTime.minute);
   s.print(F(":"));
   s.print(_model.state.gps.dateTime.second);
+  s.print(F("."));
+  s.print(_model.state.gps.dateTime.msec);
   s.println(F(" UTC"));
 
+  s.print(F("Rate: "));
+  s.print(1000000.0f / _model.state.gps.interval);
+  s.println(F(" Hz"));
+
   s.print(F("Sat: "));
   s.print(_model.state.gps.numSats);
   s.print(F(" ("));
diff --git a/lib/Espfc/src/Device/SerialDeviceAdapter.h b/lib/Espfc/src/Device/SerialDeviceAdapter.h
index 61f65e2..7dd914c 100644
--- a/lib/Espfc/src/Device/SerialDeviceAdapter.h
+++ b/lib/Espfc/src/Device/SerialDeviceAdapter.h
@@ -79,7 +79,10 @@ inline void SerialDeviceAdapter<USBCDC>::updateBadRate(int baud) {}
 
 #if defined(ARCH_RP2040)
 template<>
-inline void SerialDeviceAdapter<SerialUART>::updateBadRate(int baud) {}
+inline void SerialDeviceAdapter<SerialUART>::updateBadRate(int baud)
+{
+  _dev.begin(baud);
+}
 
 template<>
 inline void SerialDeviceAdapter<SerialUSB>::updateBadRate(int baud) {}
diff --git a/lib/Espfc/src/Espfc.cpp b/lib/Espfc/src/Espfc.cpp
index 7d996f2..26d7dfd 100644
--- a/lib/Espfc/src/Espfc.cpp
+++ b/lib/Espfc/src/Espfc.cpp
@@ -1,4 +1,5 @@
 #include "Espfc.h"
+#include "Hal/Gpio.h"
 #include "Debug_Espfc.h"
 
 namespace Espfc {
diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h
index a84bfd3..f40a0dd 100644
--- a/lib/Espfc/src/ModelState.h
+++ b/lib/Espfc/src/ModelState.h
@@ -404,6 +404,9 @@ struct GpsState
   uint8_t numSats = 0;
   uint8_t numCh = 0;
   bool present = false;
+  bool frameError = false;
+  uint32_t interval;
+  uint32_t lastMsgTs;
   GpsSupportState support;
   GpsPosition location;
   GpsVelocity velocity;
diff --git a/lib/Espfc/src/Sensor/GpsSensor.cpp b/lib/Espfc/src/Sensor/GpsSensor.cpp
index a2bdccd..10a4fbc 100644
--- a/lib/Espfc/src/Sensor/GpsSensor.cpp
+++ b/lib/Espfc/src/Sensor/GpsSensor.cpp
@@ -2,6 +2,7 @@
 #include <GpsProtocol.hpp>
 #include <Arduino.h>
 #include <cmath>
+#include <tuple>
 
 namespace Espfc::Sensor
 {
@@ -15,25 +16,23 @@ static constexpr std::array<uint16_t, 6> NMEA_MSG_OFF{
   Gps::NMEA_MSG_GSV, Gps::NMEA_MSG_RMC, Gps::NMEA_MSG_VTG,
 };
 
-static constexpr std::array<uint16_t, 2> UBX_MSG_ON{
-  Gps::UBX_NAV_PVT,
-  Gps::UBX_NAV_SAT,
-  //Gps::UBX_NAV_SOL,
-  //Gps::UBX_HNR_PVT,
+static constexpr std::array<std::tuple<uint16_t, uint8_t>, 2> UBX_MSG_ON{
+  std::make_tuple(Gps::UBX_NAV_PVT,  1u),
+  std::make_tuple(Gps::UBX_NAV_SAT, 10u),
 };
 
 GpsSensor::GpsSensor(Model& model): _model(model) {}
 
-int GpsSensor::begin(Device::SerialDevice* port)
+int GpsSensor::begin(Device::SerialDevice* port, int baud)
 {
   _port = port;
+  _targetBaud = _currentBaud = baud;
   _timer.setRate(50);
 
-  _model.logger.info().log(F("GPS")).logln(1);
-
-  setState(WAIT, WAIT, DETECT_BAUD);
-  _timeout = micros() + 3000000;
-
+  _state = DETECT_BAUD;
+  _timeout = micros() + DETECT_TIMEOUT;
+  _counter = 0;
+  setBaud(BAUDS[_counter]);
 
   return 1;
 }
@@ -68,6 +67,8 @@ bool GpsSensor::processUbx(uint8_t c)
   _ubxParser.parse(c, _ubxMsg);
   if (!_ubxMsg.isReady()) return false;
 
+  onMessage();
+
   handle();
   _ubxMsg = Gps::UbxMessage();
 
@@ -79,38 +80,73 @@ void GpsSensor::processNmea(uint8_t c)
   _nmeaParser.parse(c, _nmeaMsg);
   if (!_nmeaMsg.isReady()) return;
 
-  char buff[64];
-  size_t len = std::min(_nmeaMsg.length, sizeof(buff) - 1);
-  std::copy(_nmeaMsg.payload, _nmeaMsg.payload + len, buff);
-  buff[len] = 0;
-  D(buff);
+  //$GNTXT,01,01,01,More than 100 frame errors, UART RX was disabled*70
+  static const char * msg = "GNTXT,01,01,01,More than 100 frame errors";
+
+  if(!_model.state.gps.frameError && std::strncmp(_nmeaMsg.payload, msg, std::strlen(msg)) == 0)
+  {
+    _model.state.gps.frameError = true;
+    if(!_model.isModeActive(MODE_ARMED)) _model.logger.err().logln("GPS RX Frame Err");
+  }
+
+  onMessage();
 
   _nmeaMsg = Gps::NmeaMessage();
 }
 
+void GpsSensor::onMessage()
+{
+  if(_state == DETECT_BAUD)
+  {
+    _state = SET_BAUD;
+    _model.logger.info().log("GPS DET").logln(_currentBaud);
+  }
+}
+
 void GpsSensor::handle()
 {
   switch (_state)
   {
     case DETECT_BAUD:
-      setBaud(BAUDS[_counter]);
-      _counter++;
-      if (_counter < BAUDS.size())
-      {
-        send(Gps::UbxMonVer{}, _state, DETECT_BAUD);
-      }
-      else
+      if(micros() > _timeout)
       {
-        send(Gps::UbxMonVer{}, ERROR, ERROR);
-        _counter = 0;
+        // on timeout check next baud
+        _counter++;
+        if(_counter < BAUDS.size())
+        {
+          setBaud(BAUDS[_counter]);
+        }
+        else
+        {
+          _state = SET_BAUD;
+          _counter = 0;
+        }
+        _timeout = micros() + DETECT_TIMEOUT;
       }
       break;
 
+    case SET_BAUD:
+      send(Gps::UbxCfgPrt20{
+        .portId = 1,
+        .resered1 = 0,
+        .txReady = 0,
+        .mode = 0x08c0,     // 8N1
+        .baudRate = (uint32_t)_targetBaud, // baud
+        .inProtoMask = 0x07,
+        .outProtoMask = 0x07,
+        .flags = 0,
+        .resered2 = 0,
+      }, DISABLE_NMEA, DISABLE_NMEA);
+      delay(30); // wait until transmission complete at 9600bps
+      setBaud(_targetBaud);
+      delay(5);
+      break;
+
     case DISABLE_NMEA:
     {
-      const Gps::UbxCfgMsg8 m{
+      const Gps::UbxCfgMsg3 m{
         .msgId = NMEA_MSG_OFF[_counter],
-        .channels = {0, 0, 0, 0, 0, 0},
+        .rate = 0,
       };
       _counter++;
       if (_counter < NMEA_MSG_OFF.size())
@@ -120,41 +156,22 @@ void GpsSensor::handle()
       else
       {
         _counter = 0;
-        send(m, SET_BAUD);
+        send(m, GET_VERSION);
         _model.logger.info().log(F("GPS NMEA")).logln(0);
       }
     }
     break;
 
-    case SET_BAUD:
-    {
-      const Gps::UbxCfgPrt20 m{
-        .portId = 1,
-        .resered1 = 0,
-        .txReady = 0,
-        .mode = 0x08c0,     // 8N1
-        .baudRate = 115200, // baud
-        .inProtoMask = 0x07,
-        .outProtoMask = 0x07,
-        .flags = 0,
-        .resered2 = 0,
-      };
-      // if(!_counter++) {
-      send(m, ENABLE_UBX);
-      //} else {
-      //  send(m, ENABLE_UBX, ERROR);
-      _counter = 0;
-      //}
-      delay(35); // wait until transmission complete at 9600bps
-      setBaud(115200);
-    }
-    break;
+    case GET_VERSION:
+      send(Gps::UbxMonVer{}, ENABLE_UBX);
+      _timeout = micros() + 3 * TIMEOUT;
+      break;
 
     case ENABLE_UBX:
     {
-      const Gps::UbxCfgMsg8 m{
-        .msgId = UBX_MSG_ON[_counter],
-        .channels = {0, 1, 0, 0, 0, 0},
+      const Gps::UbxCfgMsg3 m{
+        .msgId = std::get<0>(UBX_MSG_ON[_counter]),
+        .rate = std::get<1>(UBX_MSG_ON[_counter]),
       };
       _counter++;
       if (_counter < UBX_MSG_ON.size())
@@ -172,7 +189,6 @@ void GpsSensor::handle()
     break;
 
     case ENABLE_SBAS:
-    {
       if (_model.state.gps.support.sbas)
       {
         const Gps::UbxCfgSbas8 m{
@@ -189,13 +205,12 @@ void GpsSensor::handle()
         setState(SET_RATE);
       }
       _model.logger.info().log(F("GPS SBAS")).logln(_model.state.gps.support.sbas);
-    }
-    break;
+      break;
 
     case SET_RATE:
     {
-      const uint16_t mRate = 200;
-      const uint16_t nRate = 10;
+      const uint16_t mRate = _currentBaud > 100000 ? 100 : 200;
+      const uint16_t nRate = 1;
       const Gps::UbxCfgRate6 m{
         .measRate = mRate,
         .navRate = nRate,
@@ -232,7 +247,7 @@ void GpsSensor::handle()
         else if (_ubxMsg.isResponse(Gps::UbxMonVer::ID))
         {
           handleVersion();
-          _state = DISABLE_NMEA;
+          _state = _ackState;
           _counter = 0;
         }
         else if (_ubxMsg.isResponse(Gps::UbxNavPvt92::ID))
@@ -243,6 +258,9 @@ void GpsSensor::handle()
         {
           handleNavSat();
         }
+        else
+        {
+        }
       }
       else if (_state == WAIT && micros() > _timeout)
       {
@@ -256,8 +274,12 @@ void GpsSensor::handle()
 
 void GpsSensor::setBaud(int baud)
 {
-  _port->updateBadRate(baud);
-  _model.logger.info().log(F("GPS BAUD")).logln(baud);
+  if(baud != _currentBaud)
+  {
+    _port->updateBadRate(baud);
+    _currentBaud = baud;
+    _model.logger.info().log(F("GPS BAUD")).logln(baud);
+  }
 }
 
 void GpsSensor::setState(State state, State ackState, State timeoutState)
@@ -290,7 +312,7 @@ void GpsSensor::handleNavPvt() const
   _model.state.gps.accuracy.horizontal = m.hAcc; // mm
   _model.state.gps.accuracy.vertical = m.vAcc; // mm
   _model.state.gps.accuracy.speed = m.sAcc; // mm/s
-  _model.state.gps.accuracy.heading = (m.headAcc + 5000) / 10000; // deg * 0.1
+  _model.state.gps.accuracy.heading = m.headAcc; // deg * 1e5
 
   _model.state.gps.location.raw.lat = m.lat;
   _model.state.gps.location.raw.lon = m.lon;
@@ -315,8 +337,16 @@ void GpsSensor::handleNavPvt() const
     _model.state.gps.dateTime.hour = m.hour;
     _model.state.gps.dateTime.minute = m.min;
     _model.state.gps.dateTime.second = m.sec;
-    _model.state.gps.dateTime.msec = m.nano >= 0 ? m.nano / 1000 : 0;
+    int32_t msec = m.nano / 1000000;
+    if(msec < 0) {
+      msec += 1000;
+    }
+    _model.state.gps.dateTime.msec = msec;
   }
+
+  uint32_t now = micros();
+  _model.state.gps.interval = now - _model.state.gps.lastMsgTs;
+  _model.state.gps.lastMsgTs = now;
 }
 
 void GpsSensor::handleNavSat() const
diff --git a/lib/Espfc/src/Sensor/GpsSensor.hpp b/lib/Espfc/src/Sensor/GpsSensor.hpp
index 50f91e7..6423a94 100644
--- a/lib/Espfc/src/Sensor/GpsSensor.hpp
+++ b/lib/Espfc/src/Sensor/GpsSensor.hpp
@@ -15,15 +15,16 @@ class GpsSensor
 public:
   GpsSensor(Model& model);
 
-  int begin(Device::SerialDevice* port);
+  int begin(Device::SerialDevice* port, int baud);
 
   int update();
 
 private:
   enum State {
     DETECT_BAUD,
-    DISABLE_NMEA,
     SET_BAUD,
+    DISABLE_NMEA,
+    GET_VERSION,
     ENABLE_UBX,
     ENABLE_SBAS,
     SET_RATE,
@@ -38,6 +39,8 @@ class GpsSensor
   void processNmea(uint8_t c);
   void setBaud(int baud);
 
+  void onMessage();
+
   template<typename MsgType>
   void send(const MsgType m, State ackState, State timeoutState = ERROR)
   {
@@ -59,6 +62,7 @@ class GpsSensor
   void checkSupport(const char* payload) const;
 
   static constexpr uint32_t TIMEOUT = 300000;
+  static constexpr uint32_t DETECT_TIMEOUT = 2200000;
 
   Model& _model;
 
@@ -67,6 +71,8 @@ class GpsSensor
   State _timeoutState = DETECT_BAUD;
   size_t _counter = 0;
   uint32_t _timeout = 0;
+  int _currentBaud = 0;
+  int _targetBaud = 0;
 
   Gps::UbxParser _ubxParser;
   Gps::UbxMessage _ubxMsg;
diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp
index fa81db2..56b57d7 100644
--- a/lib/Espfc/src/SerialManager.cpp
+++ b/lib/Espfc/src/SerialManager.cpp
@@ -97,11 +97,6 @@ int SerialManager::begin()
     else if(spc.functionMask & SERIAL_FUNCTION_TELEMETRY_IBUS)
     {
       sdc.baud = 115200;
-      _ibus.begin(port);
-    }
-    else if(spc.functionMask & SERIAL_FUNCTION_GPS)
-    {
-      _gps.begin(port);
     }
 
     if(!sdc.baud)
@@ -117,6 +112,15 @@ int SerialManager::begin()
       initDebugStream(port);
     }
 
+    if(spc.functionMask & SERIAL_FUNCTION_TELEMETRY_IBUS)
+    {
+      _ibus.begin(port);
+    }
+    else if(spc.functionMask & SERIAL_FUNCTION_GPS)
+    {
+      _gps.begin(port, sdc.baud);
+    }
+
     _model.logger.info().log(F("UART")).log(i).log(spc.id).log(spc.functionMask).log(sdc.baud).log(i == ESPFC_SERIAL_DEBUG_PORT).log(sdc.tx_pin).logln(sdc.rx_pin);
   }
 
diff --git a/lib/Espfc/src/Target/TargetRP2040.h b/lib/Espfc/src/Target/TargetRP2040.h
index c73de2a..07bf225 100644
--- a/lib/Espfc/src/Target/TargetRP2040.h
+++ b/lib/Espfc/src/Target/TargetRP2040.h
@@ -37,7 +37,7 @@
 #define ESPFC_SERIAL_USB_FN (SERIAL_FUNCTION_MSP)
 
 #define ESPFC_SERIAL_REMAP_PINS
-#define SERIAL_TX_FIFO_SIZE 128
+#define SERIAL_TX_FIFO_SIZE 256
 #define ESPFC_SERIAL_DEBUG_PORT SERIAL_USB
 
 #define ESPFC_SPI_0
@@ -109,12 +109,13 @@ inline int targetSerialInit(T& dev, const SerialDeviceConfig& conf)
 
   dev.setFIFOSize(SERIAL_TX_FIFO_SIZE);
   dev.setPinout(conf.tx_pin, conf.rx_pin);
-  dev.begin(conf.baud, sc);
-
   if(conf.inverted) {
-    gpio_set_inover(conf.rx_pin, GPIO_OVERRIDE_INVERT);
-    gpio_set_outover(conf.tx_pin, GPIO_OVERRIDE_INVERT);
+    //gpio_set_inover(conf.rx_pin, GPIO_OVERRIDE_INVERT);
+    //gpio_set_outover(conf.tx_pin, GPIO_OVERRIDE_INVERT);
+    dev.setInvertRX();
+    dev.setInvertTX();
   }
+  dev.begin(conf.baud, sc);
 
   return 1;
 }
diff --git a/lib/Gps/src/GpsProtocol.hpp b/lib/Gps/src/GpsProtocol.hpp
index ca53765..af29694 100644
--- a/lib/Gps/src/GpsProtocol.hpp
+++ b/lib/Gps/src/GpsProtocol.hpp
@@ -181,6 +181,14 @@ class UbxCfgMsg2
   uint16_t msgId;
 } __attribute__((packed));
 
+class UbxCfgMsg3
+{
+public:
+  static constexpr MsgId ID = UBX_CFG_MSG;
+  uint16_t msgId;
+  uint8_t rate;
+} __attribute__((packed));
+
 class UbxCfgMsg8
 {
 public: