amikhalev
/
uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

Extracted out mpu into it's own class

ugv_io
Alex Mikhalev 6 years ago
parent
399ded8801
commit
c74384bf84
7 changed files with 119 additions and 83 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      main/CMakeLists.txt
  2. 54
      main/ugv_io.cc
  3. 22
      main/ugv_io.hh
  4. 30
      main/ugv_io_gps.cc
  5. 6
      main/ugv_io_gps.hh
  6. 52
      main/ugv_io_mpu.cc
  7. 37
      main/ugv_io_mpu.hh

1
main/CMakeLists.txt
Unescape View File

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

54
main/ugv_io.cc
Unescape View File

@ -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_LEFT_DIR = GPIO_NUM_0;
static constexpr gpio_num_t MOTOR_RIGHT_PWM = 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 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"; static const char *TAG = "ugv_io";
IOClass 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() { void IOClass::Init() {
InitMotors(); InitMotors();
InitMPU(); mpu_.Init();
gps_->Init(); gps_.Init();
} }
void IOClass::InitMotors() { void IOClass::InitMotors() {
@ -66,34 +49,9 @@ void IOClass::InitMotors() {
mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config); 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) { void IOClass::ReadInputs(Inputs &inputs) {
esp_err_t ret = mpu_->motion(&accel_, &gyro_, &mag_); gps_.GetData(inputs.gps);
if (ret != ESP_OK) { mpu_.GetData(inputs.mpu);
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;
} }
void IOClass::WriteOutputs(const Outputs &outputs) { void IOClass::WriteOutputs(const Outputs &outputs) {

22
main/ugv_io.hh
Unescape View File

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

30
main/ugv_io_gps.cc
Unescape View File

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

6
main/ugv_io_gps.hh
Unescape View File

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

52
main/ugv_io_mpu.cc
Unescape View File

@ -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

37
main/ugv_io_mpu.hh
Unescape View File

@ -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
Write Preview
Loading…
Cancel
Save