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

Extracted out mpu into it's own class

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-17 19:44:14 -08:00
parent 399ded8801
commit c74384bf84
7 changed files with 119 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
main/CMakeLists.txt
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
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
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
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
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 Normal file
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 Normal file
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