diff --git a/main/u8g2_esp32_hal.c b/main/u8g2_esp32_hal.c
index 3301e24..8b40537 100644
--- a/main/u8g2_esp32_hal.c
+++ b/main/u8g2_esp32_hal.c
@@ -68,7 +68,7 @@ uint8_t u8g2_esp32_spi_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
       dev_config.duty_cycle_pos   = 0;
       dev_config.cs_ena_posttrans = 0;
       dev_config.cs_ena_pretrans  = 0;
-      dev_config.clock_speed_hz   = 10000;
+      dev_config.clock_speed_hz   = 5000;//10000;
       dev_config.spics_io_num     = u8x8->pins[U8X8_PIN_CS];
       dev_config.flags            = 0;
       dev_config.queue_size       = 200;
diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc
index a3edfc9..4faf400 100644
--- a/main/ugv_comms.cc
+++ b/main/ugv_comms.cc
@@ -127,6 +127,7 @@ void CommsClass::RunTask() {
 
   uint8_t *rx_buf = new uint8_t[rx_buf_len];
   int      rx_len;
+  lora.Flush();
 #endif
 
   UGV_Message ugv_message;
diff --git a/main/ugv_display.cc b/main/ugv_display.cc
index 99d9757..db87bbb 100644
--- a/main/ugv_display.cc
+++ b/main/ugv_display.cc
@@ -14,6 +14,7 @@ using comms::CommsClass;
 DisplayClass::DisplayClass(CommsClass* comms) : comms_(comms) {}
 
 void DisplayClass::Init() {
+  return;
   ESP_LOGD(TAG, "Initializing u8g2 display");
   // For Heltec ESP32 LoRa
   // oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, 16, 15, 4);
diff --git a/main/ugv_io_gps.cc b/main/ugv_io_gps.cc
index 2652778..1b3712b 100644
--- a/main/ugv_io_gps.cc
+++ b/main/ugv_io_gps.cc
@@ -12,8 +12,8 @@ namespace io {
 
 static constexpr size_t      GPS_BUF_SIZE         = 1024;
 static constexpr uart_port_t GPS_UART             = UART_NUM_1;
-static constexpr int         GPS_UART_TX_PIN      = 25;
-static constexpr int         GPS_UART_RX_PIN      = 26;
+static constexpr int         GPS_UART_TX_PIN      = /*25*/5;
+static constexpr int         GPS_UART_RX_PIN      = /*26*/4;
 static constexpr int         GPS_UART_BAUD        = 9600;
 static constexpr size_t      GPS_UART_RX_BUF_SIZE = 1024;
 static constexpr size_t      GPS_UART_TX_BUF_SIZE = 0;
diff --git a/main/ugv_io_mpu.cc b/main/ugv_io_mpu.cc
index 65354b6..0a792d8 100644
--- a/main/ugv_io_mpu.cc
+++ b/main/ugv_io_mpu.cc
@@ -14,8 +14,8 @@ constexpr float M_PI = atanf(1.f) * 4;
 namespace ugv {
 namespace io {
 
-static constexpr gpio_num_t       MPU_SDA         = GPIO_NUM_5;
-static constexpr gpio_num_t       MPU_SCL         = GPIO_NUM_4;
+static constexpr gpio_num_t       MPU_SDA         = GPIO_NUM_25;
+static constexpr gpio_num_t       MPU_SCL         = GPIO_NUM_26;
 static constexpr mpud::accel_fs_t MPU_ACCEL_FS    = mpud::ACCEL_FS_2G;
 static constexpr mpud::gyro_fs_t  MPU_GYRO_FS     = mpud::GYRO_FS_500DPS;
 static constexpr float            MPU_MAG_TO_FLUX = (4912.f) / (32760.f);
@@ -33,12 +33,13 @@ MPU::~MPU() { delete mpu_; }
 
 void MPU::Init() {
   xSemaphoreTake(i2c_mutex, portMAX_DELAY);
-  mpu_bus_ = &i2c0;
+  mpu_bus_ = &i2c1;
   // This is shared with the oled, so just use those pins
-  //  mpu_bus_->begin(MPU_SDA, MPU_SCL);
+   mpu_bus_->begin(MPU_SDA, MPU_SCL, GPIO_PULLUP_ENABLE, GPIO_PULLUP_ENABLE, 100000);
   mpu_ = new mpud::MPU(*mpu_bus_);
 
   esp_err_t ret;
+  mpu_->setAuxI2CBypass(false);
   ret = mpu_->testConnection();
   if (ret != ESP_OK) {
     uint8_t wai = mpu_->whoAmI();
@@ -80,27 +81,16 @@ void MPU::Calibrate() {
 
 void MPU::GetData(MpuData &data) {
   esp_err_t ret;
-  //  uint8_t mxh, mxl, myh, myl, mzh, mzl, n;
-  xSemaphoreTake(i2c_mutex, portMAX_DELAY);
+  if (!xSemaphoreTake(i2c_mutex, pdMS_TO_TICKS(10))) return;
   ret = mpu_->motion(&accel_, &gyro_);
   uint8_t compass_data[7];
   mpu_->setAuxI2CBypass(true);
   mpu_bus_->readBytes(mpud::COMPASS_I2CADDRESS, 0x03, 7, compass_data);
   mpu_->setAuxI2CBypass(false);
-  //  mpu_->compassReadByte(0x03, &mxl);
-  //  mpu_->compassReadByte(0x04, &mxh);
-  //  mpu_->compassReadByte(0x05, &myl);
-  //  mpu_->compassReadByte(0x06, &myh);
-  //  mpu_->compassReadByte(0x07, &mzl);
-  //  mpu_->compassReadByte(0x08, &mzh);
-  //  mpu_->compassReadByte(0x09, &n);
   xSemaphoreGive(i2c_mutex);
   if (ret != ESP_OK) {
     ESP_LOGE(TAG, "error reading MPU");
   }
-  //  int16_t mx = (static_cast<int16_t>(mxh) << 8) | static_cast<int16_t>(mxl);
-  //  int16_t my = (static_cast<int16_t>(myh) << 8) | static_cast<int16_t>(myl);
-  //  int16_t mz = (static_cast<int16_t>(mzh) << 8) | static_cast<int16_t>(mzl);
   int16_t mx = (static_cast<int16_t>(compass_data[1]) << 8) |
                static_cast<int16_t>(compass_data[0]);
   int16_t my = (static_cast<int16_t>(compass_data[3]) << 8) |
diff --git a/main/ugv_main.cc b/main/ugv_main.cc
index 47ac6d9..9fac6f2 100644
--- a/main/ugv_main.cc
+++ b/main/ugv_main.cc
@@ -19,7 +19,7 @@ extern "C" {
 SemaphoreHandle_t i2c_mutex;
 }
 
-constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
+constexpr uint64_t LOOP_PERIOD_US = 1e6 / 10;
 static const float PI             = atanf(1.0) * 4.0;
 
 extern "C" void OnTimeout(void *arg);