diff --git a/.gitmodules b/.gitmodules index 2616867..af1ab5c 100644 --- a/.gitmodules +++ b/.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 \ No newline at end of file diff --git a/components/I2Cbus b/components/I2Cbus new file mode 160000 index 0000000..5afed1c --- /dev/null +++ b/components/I2Cbus @@ -0,0 +1 @@ +Subproject commit 5afed1c1db9d87bf4efb79f76a0dbc0f93771bc6 diff --git a/components/MPU-driver b/components/MPU-driver new file mode 160000 index 0000000..82a6092 --- /dev/null +++ b/components/MPU-driver @@ -0,0 +1 @@ +Subproject commit 82a609236b7773ceaafc11f817ca117371314ce2 diff --git a/components/mpu_driver/CMakeLists.txt b/components/mpu_driver/CMakeLists.txt deleted file mode 100644 index 599bddd..0000000 --- a/components/mpu_driver/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -set(COMPONENT_SRCS - "mpu_driver.c") -set(COMPONENT_ADD_INCLUDEDIRS ".") - -register_component() - -component_compile_options("-Werror=incompatible-pointer-types") diff --git a/components/mpu_driver/mpu_driver.c b/components/mpu_driver/mpu_driver.c deleted file mode 100644 index 02231f0..0000000 --- a/components/mpu_driver/mpu_driver.c +++ /dev/null @@ -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; -} diff --git a/components/mpu_driver/mpu_driver.h b/components/mpu_driver/mpu_driver.h deleted file mode 100644 index 10fde86..0000000 --- a/components/mpu_driver/mpu_driver.h +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include - -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); diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index d7f1e81..ba2c069 100644 --- a/main/CMakeLists.txt +++ b/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() diff --git a/main/ugv_io.c b/main/ugv_io.c deleted file mode 100644 index 50631d8..0000000 --- a/main/ugv_io.c +++ /dev/null @@ -1,45 +0,0 @@ -#include "ugv_io.h" - -#include -#include -#include - -#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); -} \ No newline at end of file diff --git a/main/ugv_io.cc b/main/ugv_io.cc new file mode 100644 index 0000000..9bed146 --- /dev/null +++ b/main/ugv_io.cc @@ -0,0 +1,94 @@ +#include "ugv_io.hh" + +#include +#include +#include +#include + +#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 diff --git a/main/ugv_io.h b/main/ugv_io.h deleted file mode 100644 index d36bcac..0000000 --- a/main/ugv_io.h +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once - -#include - -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); diff --git a/main/ugv_io.hh b/main/ugv_io.hh new file mode 100644 index 0000000..154d354 --- /dev/null +++ b/main/ugv_io.hh @@ -0,0 +1,66 @@ +#pragma once + +#include +#include + +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