From c74384bf843eb665541cbe4bb062c702f27acedd Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 17 Jan 2019 19:44:14 -0800 Subject: [PATCH] Extracted out mpu into it's own class --- main/CMakeLists.txt | 1 + main/ugv_io.cc | 54 +++++---------------------------------------- main/ugv_io.hh | 22 +++++------------- main/ugv_io_gps.cc | 30 ++++++++++++------------- main/ugv_io_gps.hh | 6 ++--- main/ugv_io_mpu.cc | 52 +++++++++++++++++++++++++++++++++++++++++++ main/ugv_io_mpu.hh | 37 +++++++++++++++++++++++++++++++ 7 files changed, 119 insertions(+), 83 deletions(-) create mode 100644 main/ugv_io_mpu.cc create mode 100644 main/ugv_io_mpu.hh diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index ea404b4..9c72633 100644 --- a/main/CMakeLists.txt +++ b/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}) diff --git a/main/ugv_io.cc b/main/ugv_io.cc index 2e5accb..f2cba11 100644 --- a/main/ugv_io.cc +++ b/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) { diff --git a/main/ugv_io.hh b/main/ugv_io.hh index 7a0f704..abaf5b3 100644 --- a/main/ugv_io.hh +++ b/main/ugv_io.hh @@ -1,10 +1,7 @@ #pragma once -#include -#include -#include - #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; diff --git a/main/ugv_io_gps.cc b/main/ugv_io_gps.cc index f84cc6c..f38be92 100644 --- a/main/ugv_io_gps.cc +++ b/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); diff --git a/main/ugv_io_gps.hh b/main/ugv_io_gps.hh index 2566289..524fe72 100644 --- a/main/ugv_io_gps.hh +++ b/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); diff --git a/main/ugv_io_mpu.cc b/main/ugv_io_mpu.cc new file mode 100644 index 0000000..3143a6f --- /dev/null +++ b/main/ugv_io_mpu.cc @@ -0,0 +1,52 @@ +#include "ugv_io_mpu.hh" + +#include +#include + +#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 diff --git a/main/ugv_io_mpu.hh b/main/ugv_io_mpu.hh new file mode 100644 index 0000000..9563ca4 --- /dev/null +++ b/main/ugv_io_mpu.hh @@ -0,0 +1,37 @@ +#pragma once + +#include +#include +#include + +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