seems like I have implemented GPS support

.gitmodules

@@ -9,4 +9,7 @@
 	url = https://github.com/natanaeljr/esp32-MPU-driver.git
 [submodule "components/I2Cbus"]
 	path = components/I2Cbus
-	url = https://github.com/natanaeljr/esp32-I2Cbus.git
+	url = https://github.com/natanaeljr/esp32-I2Cbus.git
+[submodule "components/minmea/minmea"]
+	path = components/minmea/minmea
+	url = https://github.com/kosma/minmea.git

components/minmea/CMakeLists.txt

@@ -0,0 +1,7 @@
+set(COMPONENT_SRCS
+    "minmea/minmea.c")
+set(COMPONENT_ADD_INCLUDEDIRS "minmea")
+
+register_component()
+
+component_compile_options(PUBLIC -Dtimegm=mktime)

components/minmea/minmea

@@ -0,0 +1 @@
+Subproject commit ae4dd9442a9041345d5ef108f062e7e4ec6954f2

components/protobuf/protobuf

@@ -0,0 +1 @@
+Subproject commit e8ae137c96444ea313485ed1118c5e43b2099cf1

main/CMakeLists.txt

@@ -8,11 +8,12 @@ nanopb_generate(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
 list(APPEND COMPONENT_SRCS "ugv_main.cc"
     "ugv_comms.c"
     "ugv_io.cc"
+    "ugv_io_gps.cc"
     "u8g2_esp32_hal.c"
     "Print.cpp"
     ${PROTO_SRCS})
 set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT})
-set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "nanopb" "MPU-driver")
+set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "nanopb" "MPU-driver" "minmea")
 
 register_component()
 

main/ugv_io.cc

@@ -2,6 +2,7 @@
 
 #include <driver/gpio.h>
 #include <driver/mcpwm.h>
+#include <driver/uart.h>
 #include <esp_log.h>
 #include <math.h>
 
@@ -20,19 +21,35 @@ 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);
 
+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      = 17;
+static constexpr int         GPS_UART_RX_PIN      = 23;
+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;
+static constexpr size_t      GPS_UART_QUEUE_SIZE  = 8;
+
+const char NMEA_OUTPUT_RMCGGA[] =
+    "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
+const char NMEA_SET_UPDATE_1HZ[] = "$PMTK220,1000*1F\r\n";
+const char NMEA_Q_RELEASE[]      = "$PMTK605*31\r\n";
+
 static const char *TAG = "ugv_io";
 
-IO io;
+IOClass IO;
 
-IO::IO() { mpu_ = new mpud::MPU(); }
+IOClass::IOClass() { mpu_ = new mpud::MPU(); gps_ = new UART_GPS(); }
 
-IO::~IO() {
+IOClass::~IOClass() { delete mpu_; delete gps_; }
-  delete mpu_;
-}
 
-void IO::Init() {
+void IOClass::Init() {
-  esp_err_t ret;
+  InitMotors();
+  InitMPU();
+  gps_->Init();
+}
 
+void IOClass::InitMotors() {
   gpio_set_direction(MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT);
   gpio_set_direction(MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT);
   gpio_set_direction(MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT);
@@ -47,30 +64,31 @@ void IO::Init() {
   mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER;  // for symmetric pwm
   mcpwm_config.duty_mode    = MCPWM_DUTY_MODE_0;      // active high
   mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config);
+}
+
+void IOClass::InitMPU() {
+  esp_err_t ret;
+  ret = mpu_->testConnection();
+  if (ret != ESP_OK) {
+    ESP_LOGE(TAG, "MPU not connected");
+    return;
+  }
+  ret = mpu_->initialize();
+  if (ret != ESP_OK) {
+    ESP_LOGE(TAG, "MPU initialization error");
+    return;
+  }
+  mpu_->setAccelFullScale(MPU_ACCEL_FS);
+  mpu_->setGyroFullScale(MPU_GYRO_FS);
+  ESP_LOGI(TAG, "MPU initialized");
+}
 
-  do {
+void IOClass::ReadInputs(Inputs &inputs) {
-    ret = mpu_->testConnection();
-    if (ret != ESP_OK) {
-      ESP_LOGE(TAG, "MPU not connected");
-      break;
-    }
-    ret = mpu_->initialize();
-    if (ret != ESP_OK) {
-      ESP_LOGE(TAG, "MPU initialization error");
-      break;
-    }
-    mpu_->setAccelFullScale(MPU_ACCEL_FS);
-    mpu_->setGyroFullScale(MPU_GYRO_FS);
-    ESP_LOGI(TAG, "MPU initialized");
-  } while (0);
-
-}  // namespace io
-
-void IO::ReadInputs(Inputs &inputs) {
   esp_err_t ret = mpu_->motion(&accel_, &gyro_, &mag_);
   if (ret != ESP_OK) {
     ESP_LOGE(TAG, "error reading MPU");
   }
+  gps_->GetInfo(inputs.gps_info);
   inputs.accel     = mpud::accelGravity(accel_, MPU_ACCEL_FS);
   inputs.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
   inputs.mag.x     = ((float)mag_.x) * MPU_MAG_TO_FLUX;
@@ -78,7 +96,7 @@ void IO::ReadInputs(Inputs &inputs) {
   inputs.mag.z     = ((float)mag_.z) * MPU_MAG_TO_FLUX;
 }
 
-void IO::WriteOutputs(const Outputs &outputs) {
+void IOClass::WriteOutputs(const Outputs &outputs) {
   mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A,
                  fabs(outputs.left_motor));
   mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_B,

main/ugv_io.hh

@@ -1,34 +1,16 @@
 #pragma once
 
+#include <freertos/FreeRTOS.h>
 #include <stdint.h>
 #include <MPU.hpp>
 
+#include "ugv_io_gps.hh"
+
 namespace ugv {
 namespace io {
 
 using mpud::float_axes_t;
 
-enum GpsFixQual {
-  GPS_FIX_INVALID   = 0,  // invalid gps fix
-  GPS_FIX_GPS       = 1,  // GPS fix
-  GPS_FIX_DGPS      = 2,  // differential GPS fix
-  GPS_FIX_PPS       = 3,  // PPS fix
-  GPS_FIX_RTK       = 4,  // Real Time Kinematic fix
-  GPS_FIX_FRTK      = 5,  // Float Real Time Kinematic fix
-  GPS_FIX_ESTIMATED = 6,  // Estimated fix
-  GPS_FIX_MANUAL    = 7,  // Manual fix
-  GPS_FIX_SIMULATED = 8,  // Simulated fix
-};
-
-struct GpsInfo {
-  GpsFixQual fix_quality;
-  uint8_t    num_satellites;
-
-  double latitude;   // degrees +/-
-  double longitude;  // degrees +/-
-  double altitude;   // meters
-};
-
 struct Inputs {
   // G's
   float_axes_t accel;
@@ -45,10 +27,10 @@ struct Outputs {
   float right_motor;  // right motor power -1.0 to +1.0
 };
 
-class IO {
+class IOClass {
  public:
-  explicit IO();
+  explicit IOClass();
-  ~IO();
+  ~IOClass();
 
   void Init();
 
@@ -58,9 +40,14 @@ class IO {
  private:
   mpud::MPU *      mpu_;
   mpud::raw_axes_t accel_, gyro_, mag_;
+
+  UART_GPS *gps_;
+
+  void InitMotors();
+  void InitMPU();
 };
 
-extern IO io;
+extern IOClass IO;
 
 }  // namespace io
 }  // namespace ugv

main/ugv_io_gps.cc

@@ -0,0 +1,216 @@
+#include "ugv_io_gps.hh"
+
+#include <driver/uart.h>
+#include <esp_log.h>
+#include <cstring>
+#include <string>
+
+#include "minmea.h"
+
+namespace ugv {
+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      = 17;
+static constexpr int         GPS_UART_RX_PIN      = 23;
+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;
+static constexpr size_t      GPS_UART_QUEUE_SIZE  = 8;
+
+const char NMEA_OUTPUT_RMCONLY[] =
+    "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29";
+const char NMEA_OUTPUT_RMCGGA[] =
+    "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28";
+const char NMEA_OUTPUT_ALL[] =
+    "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28";
+const char NMEA_OUTPUT_OFF[] =
+    "$PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28";
+
+const char NMEA_UPDATE_1HZ[]  = "$PMTK220,1000*1F";
+const char NMEA_UPDATE_5HZ[]  = "$PMTK220,200*2C";
+const char NMEA_UPDATE_10HZ[] = "$PMTK220,100*2F";
+
+const char NMEA_Q_RELEASE[] = "$PMTK605*31";
+
+const char NMEA_END_CMD[] = "\r\n";
+
+std::string st;
+
+static const char *TAG = "ugv_io";
+
+UART_GPS::UART_GPS() {}
+
+UART_GPS::~UART_GPS() {}
+
+void UART_GPS::Init() {
+  esp_err_t ret;
+
+  uart_config_t gps_uart_config;
+  gps_uart_config.baud_rate           = GPS_UART_BAUD;
+  gps_uart_config.data_bits           = UART_DATA_8_BITS;
+  gps_uart_config.parity              = UART_PARITY_DISABLE;
+  gps_uart_config.stop_bits           = UART_STOP_BITS_1;
+  gps_uart_config.flow_ctrl           = UART_HW_FLOWCTRL_DISABLE;
+  gps_uart_config.rx_flow_ctrl_thresh = 122;
+  gps_uart_config.use_ref_tick        = false;
+  ret = uart_param_config(GPS_UART, &gps_uart_config);
+  if (ret != ESP_OK) {
+    ESP_LOGE(TAG, "uart_param_config: %d", ret);
+    return;
+  }
+  uart_set_pin(GPS_UART, GPS_UART_TX_PIN, GPS_UART_TX_PIN, UART_PIN_NO_CHANGE,
+               UART_PIN_NO_CHANGE);
+  ret = uart_driver_install(GPS_UART, GPS_UART_RX_BUF_SIZE,
+                            GPS_UART_TX_BUF_SIZE, GPS_UART_QUEUE_SIZE, NULL, 0);
+  if (ret != ESP_OK) {
+    ESP_LOGE(TAG, "uart_driver_install: %d", ret);
+    return;
+  }
+  ESP_LOGI(TAG, "gps uart configured");
+
+  mutex_ = xSemaphoreCreateMutex();
+  info_  = GpsInfo{};
+
+  BaseType_t xRet = xTaskCreate(UART_GPS::GPS_Task, "ugv_io_gps", 2 * 1024,
+                                this, 1, &this->task_);
+  if (xRet != pdTRUE) {
+    ESP_LOGE(TAG, "error creating GPS task");
+    return;
+  }
+}
+
+void UART_GPS::GetInfo(GpsInfo &info) {
+  xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
+  info = GpsInfo(info_);
+  xSemaphoreGive(mutex_);
+}
+
+esp_err_t UART_GPS::WriteCommand(const char *cmd, size_t len) {
+  esp_err_t ret;
+  ret = uart_write_bytes(GPS_UART, cmd, len);
+  if (ret != ESP_OK) goto err;
+  ret = uart_write_bytes(GPS_UART, NMEA_END_CMD, sizeof(NMEA_END_CMD));
+  if (ret != ESP_OK) goto err;
+  ret = uart_wait_tx_done(GPS_UART, 1000);
+  if (ret != ESP_OK) goto err;
+
+  return ESP_OK;
+
+err:
+  const char *error_name = esp_err_to_name(ret);
+  ESP_LOGE(TAG, "WriteCommand error: %s (%d)", error_name, ret);
+  return ret;
+}
+
+void UART_GPS::ProcessLine(const char *line, size_t len) {
+  ESP_LOGI(TAG, "gps data: %.*s", len, line);
+  enum minmea_sentence_id id = minmea_sentence_id(line, false);
+  switch (id) {
+    case MINMEA_INVALID:
+    invalid:
+      ESP_LOGE(TAG, "invalid nmea sentence: %s", line);
+      return;
+    case MINMEA_SENTENCE_RMC: {
+      minmea_sentence_rmc rmc;
+      bool                parse_success;
+      parse_success = minmea_parse_rmc(&rmc, line);
+      if (!parse_success) {
+        goto invalid;
+      }
+      xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
+      info_.valid       = rmc.valid;
+      info_.latitude    = minmea_tofloat(&rmc.latitude);
+      info_.longitude   = minmea_tofloat(&rmc.longitude);
+      info_.speed       = minmea_tofloat(&rmc.speed);
+      info_.course      = minmea_tofloat(&rmc.course);
+      info_.last_update = xTaskGetTickCount();
+      xSemaphoreGive(mutex_);
+    }
+    case MINMEA_SENTENCE_GGA: {
+      minmea_sentence_gga gga;
+      bool                parse_success;
+      parse_success = minmea_parse_gga(&gga, line);
+      if (!parse_success) {
+        goto invalid;
+      }
+      xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
+      info_.fix_quality    = (GpsFixQual)gga.fix_quality;
+      info_.num_satellites = gga.satellites_tracked;
+      info_.latitude       = minmea_tofloat(&gga.latitude);
+      info_.longitude      = minmea_tofloat(&gga.longitude);
+      if (gga.altitude_units != 'M') {
+        ESP_LOGW(TAG, "unknown altitude units: %c", gga.altitude_units);
+      }
+      info_.altitude    = minmea_tofloat(&gga.altitude);
+      info_.last_update = xTaskGetTickCount();
+      xSemaphoreGive(mutex_);
+    }
+    default: ESP_LOGW(TAG, "unsupported nmea sentence: %s", line);
+  }
+}
+
+void UART_GPS::GPS_Task(void *arg) {
+  UART_GPS *gps = (UART_GPS *)arg;
+  (void)gps;
+
+  ESP_LOGI(TAG, "gps_task start");
+  char * buf1    = (char *)malloc(GPS_BUF_SIZE);
+  char * buf2    = (char *)malloc(GPS_BUF_SIZE);
+  char * buf_end = buf1;
+  size_t read_bytes;
+
+  esp_err_t ret;
+  ret = gps->WriteCommand(NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA));
+  ESP_LOGI(TAG, "sent output rmc and gga: %d", ret);
+  ret = gps->WriteCommand(NMEA_UPDATE_1HZ, sizeof(NMEA_UPDATE_1HZ));
+  ESP_LOGI(TAG, "sent update 1hz: %d", ret);
+
+  vTaskDelay(pdMS_TO_TICKS(100));
+  ret = gps->WriteCommand(NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE));
+  ESP_LOGI(TAG, "sent q release: %d", ret);
+
+  while (true) {
+    size_t buf_remaining = GPS_BUF_SIZE - (buf_end - buf1);
+    read_bytes = uart_read_bytes(GPS_UART, (uint8_t *)buf_end, buf_remaining,
+                                 portMAX_DELAY);
+    if (read_bytes <= 0) {
+      ESP_LOGW(TAG, "GPS error: %d", read_bytes);
+      continue;
+    }
+    ESP_LOGW(TAG, "GPS bytes received: %.*s", read_bytes, buf_end);
+    buf_end += read_bytes;
+    int lines_received = 0;
+    for (char *c = buf1; c < buf_end; c++) {
+      if (*c != '\n') {  // wait for \n, if not found loop again
+        continue;
+      }
+      lines_received++;
+      c++;  // now c is one past the end of the whole string including \n
+
+      size_t remaining = buf_end - c;
+      memcpy(buf2, c, remaining);  // copy remaining text from buf1 to buf2
+      buf_end = buf2 + remaining;  // update buf_end to point to the end of the
+                                   // remaining text in buf2
+
+      char * str_ptr = buf1;
+      size_t str_len = c - buf1;
+      str_ptr[str_len] =
+          '\0';  // append the NULL byte (safe because old data was copied)
+      gps->ProcessLine(str_ptr, str_len);  // process that line
+
+      std::swap(buf1, buf2);  // swap buffers
+      c = buf1;  // start over on text which was copied to buf2 (now buf1)
+    }
+    if (lines_received == 0) {
+      ESP_LOGW(TAG, "no lines received, waiting for more data");
+    }
+  }
+  free(buf1);
+  free(buf2);
+  vTaskDelete(NULL);
+}
+
+};  // namespace io
+};  // namespace ugv

main/ugv_io_gps.hh

@@ -0,0 +1,57 @@
+#pragma once
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+#include <freertos/task.h>
+#include <stdint.h>
+
+namespace ugv {
+namespace io {
+
+enum GpsFixQual {
+  GPS_FIX_INVALID   = 0,  // invalid gps fix
+  GPS_FIX_GPS       = 1,  // GPS fix
+  GPS_FIX_DGPS      = 2,  // differential GPS fix
+  GPS_FIX_PPS       = 3,  // PPS fix
+  GPS_FIX_RTK       = 4,  // Real Time Kinematic fix
+  GPS_FIX_FRTK      = 5,  // Float Real Time Kinematic fix
+  GPS_FIX_ESTIMATED = 6,  // Estimated fix
+  GPS_FIX_MANUAL    = 7,  // Manual fix
+  GPS_FIX_SIMULATED = 8,  // Simulated fix
+};
+
+struct GpsInfo {
+  TickType_t last_update;
+  bool       valid;
+  GpsFixQual fix_quality;
+  uint8_t    num_satellites;
+
+  float latitude;   // degrees +/-
+  float longitude;  // degrees +/-
+  float altitude;   // meters
+  float speed;      // knots
+  float course;     // degrees clockwise of north
+};
+
+class UART_GPS {
+ public:
+  explicit UART_GPS();
+  ~UART_GPS();
+
+  void Init();
+
+  void GetInfo(GpsInfo& info);
+
+ private:
+  TaskHandle_t      task_;
+  SemaphoreHandle_t mutex_;
+  GpsInfo           info_;
+
+  esp_err_t WriteCommand(const char* cmd, size_t len);
+  void      ProcessLine(const char* line, size_t len);
+
+  static void GPS_Task(void* arg);
+};
+
+}  // namespace io
+}  // namespace ugv

main/ugv_main.cc

@@ -1,14 +1,17 @@
-#include <driver/uart.h>
 #include <esp_log.h>
 #include <string.h>
 #include <u8g2.h>
 
+#include "U8g2lib.hh"
 #include "sx127x_driver.h"
 #include "sx127x_registers.h"
-#include "u8g2_esp32_hal.h"
 #include "ugv_comms.h"
 #include "ugv_config.h"
-#include "U8g2lib.hh"
+#include "ugv_io.hh"
+
+namespace ugv {
+
+using ugv::io::IO;
 
 static const char *TAG = "ugv_main";
 
@@ -21,69 +24,12 @@ void setup_oled(void) {
   oled->setPowerSave(false);
 }
 
-const size_t GPS_BUF_SIZE = 1024;
-
-const char NMEA_OUTPUT_RMCGGA[] =
-    "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
-const char NMEA_SET_UPDATE_1HZ[] = "$PMTK220,1000*1F\r\n";
-const char NMEA_Q_RELEASE[]      = "$PMTK605*31\r\n";
-
-static void gps_task(void *arg) {
-  ESP_LOGI(TAG, "gps_task start");
-  uint8_t * data = (uint8_t*) malloc(GPS_BUF_SIZE);
-  size_t    read_bytes;
-  esp_err_t ret;
-  uart_write_bytes(GPS_UART, NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA));
-  ret = uart_wait_tx_done(GPS_UART, 1000);
-  ESP_LOGI(TAG, "sent output rmc and gga: %d", ret);
-  uart_write_bytes(GPS_UART, NMEA_SET_UPDATE_1HZ, sizeof(NMEA_SET_UPDATE_1HZ));
-  ret = uart_wait_tx_done(GPS_UART, 1000);
-  ESP_LOGI(TAG, "sent update 1hz: %d", ret);
-  vTaskDelay(pdMS_TO_TICKS(100));
-  uart_write_bytes(GPS_UART, NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE));
-  ret = uart_wait_tx_done(GPS_UART, 1000);
-  ESP_LOGI(TAG, "sent q release: %d", ret);
-  while (true) {
-    read_bytes =
-        uart_read_bytes(GPS_UART, data, GPS_BUF_SIZE, pdMS_TO_TICKS(100));
-    if (read_bytes > 0) {
-      ESP_LOGI(TAG, "gps data: %.*s", read_bytes, data);
-    }
-  }
-}
-
-void setup_gps(void) {
-  uart_config_t gps_uart_config;
-  esp_err_t     ret;
-  // gps_uart_config.baud_rate = 9600;
-  gps_uart_config.baud_rate           = 57600;
-  gps_uart_config.data_bits           = UART_DATA_8_BITS;
-  gps_uart_config.parity              = UART_PARITY_DISABLE;
-  gps_uart_config.stop_bits           = UART_STOP_BITS_1;
-  gps_uart_config.flow_ctrl           = UART_HW_FLOWCTRL_DISABLE;
-  gps_uart_config.rx_flow_ctrl_thresh = 122;
-  gps_uart_config.use_ref_tick        = false;
-  ret = uart_param_config(GPS_UART, &gps_uart_config);
-  if (ret != ESP_OK) {
-    ESP_LOGE(TAG, "uart_param_config: %d", ret);
-    return;
-  }
-  uart_set_pin(GPS_UART, 17, 23, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
-  ret = uart_driver_install(GPS_UART, 1024, 0, 8, NULL, 0);
-  if (ret != ESP_OK) {
-    ESP_LOGE(TAG, "uart_driver_install: %d", ret);
-    return;
-  }
-  ESP_LOGI(TAG, "gps uart configured");
-  xTaskCreate(&gps_task, "gps_task", 2 * 1024, NULL, 1, NULL);
-}
-
 void setup(void) {
   ESP_LOGI(TAG, "setup");
 
-  setup_gps();
   setup_oled();
   ugv_comms_init();
+  IO.Init();
 }
 
 #define BUF_SZ 32
@@ -96,6 +42,10 @@ void loop(void) {
   static int8_t     last_packet_snr;
 
   static char buf[BUF_SZ];
+  static io::Inputs inputs;
+
+  IO.ReadInputs(inputs);
+  // ESP_LOGI(TAG, "inputs %s", inputs.ToString());
 
   oled->firstPage();
   sx127x_read_rssi(ugv_comms_state.lora, &lora_rssi);
@@ -143,6 +93,8 @@ void loopTask(void *pvUser) {
   }
 }
 
+}  // namespace ugv
+
 extern "C" void app_main() {
-  xTaskCreatePinnedToCore(loopTask, "loopTask", 8192, NULL, 1, NULL, 1);
+  xTaskCreatePinnedToCore(ugv::loopTask, "loopTask", 8192, NULL, 1, NULL, 1);
 }