begin using mpu driver lib

.gitmodules

@@ -1,3 +1,12 @@
 [submodule "components/nanopb/nanopb"]
 	path = components/nanopb/nanopb
 	url = https://github.com/nanopb/nanopb.git
+[submodule "components/u8g2"]
+	path = components/u8g2
+	url = https://github.com/olikraus/u8g2.git
+[submodule "MPU-driver"]
+	path = components/MPU-driver
+	url = https://github.com/natanaeljr/esp32-MPU-driver.git
+[submodule "components/I2Cbus"]
+	path = components/I2Cbus
+	url = https://github.com/natanaeljr/esp32-I2Cbus.git

components/I2Cbus

@@ -0,0 +1 @@
+Subproject commit 5afed1c1db9d87bf4efb79f76a0dbc0f93771bc6

components/MPU-driver

@@ -0,0 +1 @@
+Subproject commit 82a609236b7773ceaafc11f817ca117371314ce2

components/mpu_driver/CMakeLists.txt

@@ -1,7 +0,0 @@
-set(COMPONENT_SRCS
-    "mpu_driver.c")
-set(COMPONENT_ADD_INCLUDEDIRS ".")
-
-register_component()
-
-component_compile_options("-Werror=incompatible-pointer-types")

components/mpu_driver/mpu_driver.c

@@ -1,18 +0,0 @@
-#include "mpu_driver.h"
-
-typedef struct mpu_s {
-} mpu_t;
-
-esp_err_t mpu_init(mpu_config_t *config, mpu_t **hndl_out) {
-  mpu_t *hndl = malloc(sizeof(mpu_t));
-  if (!hndl) return ESP_ERR_NO_MEM;
-  *hndl_out = hndl;
-
-  return ESP_OK;
-}
-
-esp_err_t mpu_free(mpu_t *hndl) {
-  free(hndl);
-
-  return ESP_OK;
-}

components/mpu_driver/mpu_driver.h

@@ -1,11 +0,0 @@
-#pragma once
-
-#include <esp_err.h>
-
-typedef struct mpu_config_s {
-} mpu_config_t;
-
-typedef struct mpu_s *mpu_hndl_t;
-
-esp_err_t mpu_init(mpu_config_t *config, mpu_hndl_t *hndl_out);
-esp_err_t mpu_free(mpu_hndl_t hndl);

main/CMakeLists.txt

@@ -7,12 +7,12 @@ nanopb_generate(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
 
 list(APPEND COMPONENT_SRCS "ugv_main.cc"
     "ugv_comms.c"
-    "ugv_io.c"
+    "ugv_io.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")
 
 register_component()
 

main/ugv_io.c

@@ -1,45 +0,0 @@
-#include "ugv_io.h"
-
-#include <driver/gpio.h>
-#include <driver/mcpwm.h>
-#include <math.h>
-
-#define IO_MCPWM_UNIT MCPWM_UNIT_0
-#define IO_MOTOR_LEFT_PWM 0
-#define IO_MOTOR_LEFT_DIR 0
-#define IO_MOTOR_RIGHT_PWM 0
-#define IO_MOTOR_RIGHT_DIR 0
-
-typedef struct ugv_io_s {
-} ugv_io_t;
-
-static ugv_io_t io;
-
-void ugv_io_init() {
-  gpio_set_direction(IO_MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT);
-  gpio_set_direction(IO_MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT);
-  gpio_set_direction(IO_MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT);
-  gpio_set_direction(IO_MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT);
-  mcpwm_gpio_init(IO_MCPWM_UNIT, MCPWM0A, IO_MOTOR_LEFT_PWM);
-  mcpwm_gpio_init(IO_MCPWM_UNIT, MCPWM0B, IO_MOTOR_RIGHT_PWM);
-
-  mcpwm_config_t mcpwm_config;
-  mcpwm_config.frequency    = 20000;  // 20khz
-  mcpwm_config.cmpr_a       = 0;
-  mcpwm_config.cmpr_b       = 0;
-  mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER;  // for symmetric pwm
-  mcpwm_config.duty_mode    = MCPWM_DUTY_MODE_0;      // active high
-  mcpwm_init(IO_MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config);
-}
-
-void ugv_io_write_outputs(ugv_outputs_t *outputs) {
-  mcpwm_set_duty(IO_MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A,
-                 fabs(outputs->left_motor));
-  mcpwm_set_duty(IO_MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_B,
-                 fabs(outputs->right_motor));
-  bool left_dir, right_dir;
-  left_dir  = outputs->left_motor < 0.f;
-  right_dir = outputs->right_motor < 0.f;
-  gpio_set_level(IO_MOTOR_LEFT_DIR, left_dir);
-  gpio_set_level(IO_MOTOR_RIGHT_DIR, right_dir);
-}

main/ugv_io.cc

@@ -0,0 +1,94 @@
+#include "ugv_io.hh"
+
+#include <driver/gpio.h>
+#include <driver/mcpwm.h>
+#include <esp_log.h>
+#include <math.h>
+
+#include "MPU.hpp"
+#include "mpu/math.hpp"
+
+namespace ugv {
+namespace io {
+
+static constexpr mcpwm_unit_t     MCPWM_UNIT      = MCPWM_UNIT_0;
+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 const char *TAG = "ugv_io";
+
+IO io;
+
+IO::IO() { mpu_ = new mpud::MPU(); }
+
+IO::~IO() {
+  delete mpu_;
+}
+
+void IO::Init() {
+  esp_err_t ret;
+
+  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);
+  gpio_set_direction(MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT);
+  mcpwm_gpio_init(MCPWM_UNIT, MCPWM0A, MOTOR_LEFT_PWM);
+  mcpwm_gpio_init(MCPWM_UNIT, MCPWM0B, MOTOR_RIGHT_PWM);
+
+  mcpwm_config_t mcpwm_config;
+  mcpwm_config.frequency    = 20000;  // 20khz
+  mcpwm_config.cmpr_a       = 0;
+  mcpwm_config.cmpr_b       = 0;
+  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);
+
+  do {
+    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");
+  }
+  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;
+}
+
+void IO::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,
+                 fabs(outputs.right_motor));
+  bool left_dir, right_dir;
+  left_dir  = outputs.left_motor < 0.f;
+  right_dir = outputs.right_motor < 0.f;
+  gpio_set_level(MOTOR_LEFT_DIR, left_dir);
+  gpio_set_level(MOTOR_RIGHT_DIR, right_dir);
+}
+
+};  // namespace io
+};  // namespace ugv

main/ugv_io.h

@@ -1,44 +0,0 @@
-#pragma once
-
-#include <stdint.h>
-
-typedef enum gps_fix_qual_e {
-  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
-} gps_fix_qual_t;
-
-typedef struct ugv_inputs_s {
-  float gyro_x;   // degrees
-  float gyro_y;   // degrees
-  float gyro_z;   // degrees
-  float accel_x;  // gs
-  float accel_y;  // gs
-  float accel_z;  // gs
-  float mag_x;    // flux density uT
-  float mag_y;    // flux density uT
-  float mag_z;    // flux density uT
-
-  gps_fix_qual_t gps_fix_quality;
-  uint8_t        gps_num_satellites;
-
-  double gps_lat;       // degrees +/-
-  double gps_long;      // degrees +/-
-  double gps_altitude;  // meters
-} ugv_inputs_t;
-
-typedef struct ugv_outputs_s {
-  float left_motor;   // left motor power -1.0 to +1.0
-  float right_motor;  // right motor power -1.0 to +1.0
-} ugv_outputs_t;
-
-void ugv_io_init();
-
-void ugv_io_read_inputs(ugv_inputs_t *inputs);
-void ugv_io_write_outputs(ugv_outputs_t *outputs);

main/ugv_io.hh

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <stdint.h>
+#include <MPU.hpp>
+
+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;
+  // degrees/s
+  float_axes_t gyro_rate;
+  // flux density uT
+  float_axes_t mag;
+
+  GpsInfo gps_info;
+};
+
+struct Outputs {
+  float left_motor;   // left motor power -1.0 to +1.0
+  float right_motor;  // right motor power -1.0 to +1.0
+};
+
+class IO {
+ public:
+  explicit IO();
+  ~IO();
+
+  void Init();
+
+  void ReadInputs(Inputs &inputs);
+  void WriteOutputs(const Outputs &outputs);
+
+ private:
+  mpud::MPU *      mpu_;
+  mpud::raw_axes_t accel_, gyro_, mag_;
+};
+
+extern IO io;
+
+}  // namespace io
+}  // namespace ugv