Extracted out mpu into it's own class

main/CMakeLists.txt

@@ -9,6 +9,7 @@ list(APPEND COMPONENT_SRCS "ugv_main.cc"
     "ugv_comms.c"
     "ugv_io.cc"
     "ugv_io_gps.cc"
+    "ugv_io_mpu.cc"
     "u8g2_esp32_hal.c"
     "Print.cpp"
     ${PROTO_SRCS})

main/ugv_io.cc

@@ -17,36 +17,19 @@ static constexpr gpio_num_t       MOTOR_LEFT_PWM  = GPIO_NUM_0;
 static constexpr gpio_num_t       MOTOR_LEFT_DIR  = GPIO_NUM_0;
 static constexpr gpio_num_t       MOTOR_RIGHT_PWM = GPIO_NUM_0;
 static constexpr gpio_num_t       MOTOR_RIGHT_DIR = GPIO_NUM_0;
-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";
 
 IOClass IO;
 
-IOClass::IOClass() { mpu_ = new mpud::MPU(); gps_ = new UART_GPS(); }
+IOClass::IOClass() {}
 
-IOClass::~IOClass() { delete mpu_; delete gps_; }
+IOClass::~IOClass() {}
 
 void IOClass::Init() {
   InitMotors();
-  InitMPU();
-  gps_->Init();
+  mpu_.Init();
+  gps_.Init();
 }
 
 void IOClass::InitMotors() {
@@ -66,34 +49,9 @@ void IOClass::InitMotors() {
   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");
-}
-
 void IOClass::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;
-  inputs.mag.y     = ((float)mag_.y) * MPU_MAG_TO_FLUX;
-  inputs.mag.z     = ((float)mag_.z) * MPU_MAG_TO_FLUX;
+  gps_.GetData(inputs.gps);
+  mpu_.GetData(inputs.mpu);
 }
 
 void IOClass::WriteOutputs(const Outputs &outputs) {

main/ugv_io.hh

@@ -1,10 +1,7 @@
 #pragma once
 
-#include <freertos/FreeRTOS.h>
-#include <stdint.h>
-#include <MPU.hpp>
-
 #include "ugv_io_gps.hh"
+#include "ugv_io_mpu.hh"
 
 namespace ugv {
 namespace io {
@@ -12,14 +9,8 @@ namespace io {
 using mpud::float_axes_t;
 
 struct Inputs {
-  // G's
-  float_axes_t accel;
-  // degrees/s
-  float_axes_t gyro_rate;
-  // flux density uT
-  float_axes_t mag;
-
-  GpsInfo gps_info;
+  MpuData mpu;
+  GpsData gps;
 };
 
 struct Outputs {
@@ -38,13 +29,10 @@ class IOClass {
   void WriteOutputs(const Outputs &outputs);
 
  private:
-  mpud::MPU *      mpu_;
-  mpud::raw_axes_t accel_, gyro_, mag_;
-
-  UART_GPS *gps_;
+  UART_GPS gps_;
+  MPU mpu_;
 
   void InitMotors();
-  void InitMPU();
 };
 
 extern IOClass IO;

main/ugv_io_gps.cc

@@ -71,7 +71,7 @@ void UART_GPS::Init() {
   ESP_LOGI(TAG, "gps uart configured");
 
   mutex_ = xSemaphoreCreateMutex();
-  info_  = GpsInfo{};
+  data_  = GpsData{};
 
   BaseType_t xRet = xTaskCreate(UART_GPS::GPS_Task, "ugv_io_gps", 2 * 1024,
                                 this, 1, &this->task_);
@@ -81,9 +81,9 @@ void UART_GPS::Init() {
   }
 }
 
-void UART_GPS::GetInfo(GpsInfo &info) {
+void UART_GPS::GetData(GpsData &data) {
   xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
-  info = GpsInfo(info_);
+  data = GpsData(data_);
   xSemaphoreGive(mutex_);
 }
 
@@ -120,12 +120,12 @@ void UART_GPS::ProcessLine(const char *line, size_t len) {
         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();
+      data_.valid       = rmc.valid;
+      data_.latitude    = minmea_tofloat(&rmc.latitude);
+      data_.longitude   = minmea_tofloat(&rmc.longitude);
+      data_.speed       = minmea_tofloat(&rmc.speed);
+      data_.course      = minmea_tofloat(&rmc.course);
+      data_.last_update = xTaskGetTickCount();
       xSemaphoreGive(mutex_);
     }
     case MINMEA_SENTENCE_GGA: {
@@ -136,15 +136,15 @@ void UART_GPS::ProcessLine(const char *line, size_t len) {
         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);
+      data_.fix_quality    = (GpsFixQual)gga.fix_quality;
+      data_.num_satellites = gga.satellites_tracked;
+      data_.latitude       = minmea_tofloat(&gga.latitude);
+      data_.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();
+      data_.altitude    = minmea_tofloat(&gga.altitude);
+      data_.last_update = xTaskGetTickCount();
       xSemaphoreGive(mutex_);
     }
     default: ESP_LOGW(TAG, "unsupported nmea sentence: %s", line);

main/ugv_io_gps.hh

@@ -20,7 +20,7 @@ enum GpsFixQual {
   GPS_FIX_SIMULATED = 8,  // Simulated fix
 };
 
-struct GpsInfo {
+struct GpsData {
   TickType_t last_update;
   bool       valid;
   GpsFixQual fix_quality;
@@ -40,12 +40,12 @@ class UART_GPS {
 
   void Init();
 
-  void GetInfo(GpsInfo& info);
+  void GetData(GpsData& data);
 
  private:
   TaskHandle_t      task_;
   SemaphoreHandle_t mutex_;
-  GpsInfo           info_;
+  GpsData           data_;
 
   esp_err_t WriteCommand(const char* cmd, size_t len);
   void      ProcessLine(const char* line, size_t len);

main/ugv_io_mpu.cc

@@ -0,0 +1,52 @@
+#include "ugv_io_mpu.hh"
+
+#include <driver/uart.h>
+#include <esp_log.h>
+
+#include "MPU.hpp"
+#include "mpu/math.hpp"
+
+namespace ugv {
+namespace io {
+
+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 const char *TAG = "ugv_io_mpu";
+
+MPU::MPU() { mpu_ = new mpud::MPU(); }
+
+MPU::~MPU() { delete mpu_; }
+
+void MPU::Init() {
+  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");
+}
+
+void MPU::GetData(MpuData &data) {
+  esp_err_t ret = mpu_->motion(&accel_, &gyro_, &mag_);
+  if (ret != ESP_OK) {
+    ESP_LOGE(TAG, "error reading MPU");
+  }
+  data.accel     = mpud::accelGravity(accel_, MPU_ACCEL_FS);
+  data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
+  data.mag.x     = ((float)mag_.x) * MPU_MAG_TO_FLUX;
+  data.mag.y     = ((float)mag_.y) * MPU_MAG_TO_FLUX;
+  data.mag.z     = ((float)mag_.z) * MPU_MAG_TO_FLUX;
+}
+
+};  // namespace io
+};  // namespace ugv

main/ugv_io_mpu.hh

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <freertos/FreeRTOS.h>
+#include <stdint.h>
+#include <MPU.hpp>
+
+namespace ugv {
+namespace io {
+
+using mpud::float_axes_t;
+
+struct MpuData {
+  // G's
+  float_axes_t accel;
+  // degrees/s
+  float_axes_t gyro_rate;
+  // flux density uT
+  float_axes_t mag;
+};
+
+class MPU {
+ public:
+  explicit MPU();
+  ~MPU();
+
+  void Init();
+
+  void GetData(MpuData &data);
+
+ private:
+  mpud::MPU *      mpu_;
+  mpud::raw_axes_t accel_, gyro_, mag_;
+  MpuData mpu_data_;
+};
+
+}  // namespace io
+}  // namespace ugv