You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
89 lines
2.4 KiB
89 lines
2.4 KiB
#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 gpio_num_t MPU_SDA = GPIO_NUM_2; |
|
static constexpr gpio_num_t MPU_SCL = GPIO_NUM_3; |
|
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"; |
|
|
|
Vec3f::Vec3f() : x(0), y(0), z(0) {} |
|
Vec3f::Vec3f(float x, float y, float z) : x(x), y(y), z(z) {} |
|
Vec3f::Vec3f(const mpud::float_axes_t &axes) |
|
: x(axes.x), y(axes.y), z(axes.z) {} |
|
|
|
MPU::MPU() : mpu_(nullptr), mpu_data_() {} |
|
|
|
MPU::~MPU() { delete mpu_; } |
|
|
|
void MPU::Init() { |
|
mutex_ = xSemaphoreCreateMutex(); |
|
mpu_data_ = MpuData{}; |
|
|
|
mpu_bus_ = &i2c0; |
|
mpu_bus_->begin(MPU_SDA, MPU_SCL); |
|
mpu_ = new mpud::MPU(*mpu_bus_); |
|
|
|
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); |
|
|
|
BaseType_t xRet = |
|
xTaskCreate(MPU::MPU_Task, "ugv_io_mpu", 2 * 1024, this, 3, &this->task_); |
|
if (xRet != pdTRUE) { |
|
ESP_LOGE(TAG, "error creating MPU task"); |
|
return; |
|
} |
|
ESP_LOGI(TAG, "MPU initialized and task started"); |
|
} |
|
|
|
void MPU::GetData(MpuData &data) { |
|
xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); |
|
data = MpuData(mpu_data_); |
|
xSemaphoreGive(mutex_); |
|
} |
|
|
|
void MPU::DoTask() { |
|
esp_err_t ret; |
|
while (true) { |
|
vTaskDelay(pdMS_TO_TICKS(50)); |
|
ret = mpu_->motion(&accel_, &gyro_, &mag_); |
|
if (ret != ESP_OK) { |
|
ESP_LOGE(TAG, "error reading MPU"); |
|
continue; |
|
} |
|
xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); |
|
mpu_data_.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS); |
|
mpu_data_.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS); |
|
mpu_data_.mag.x = ((float)mag_.x) * MPU_MAG_TO_FLUX; |
|
mpu_data_.mag.y = ((float)mag_.y) * MPU_MAG_TO_FLUX; |
|
mpu_data_.mag.z = ((float)mag_.z) * MPU_MAG_TO_FLUX; |
|
xSemaphoreGive(mutex_); |
|
} |
|
vTaskDelete(NULL); |
|
} |
|
|
|
void MPU::MPU_Task(void *arg) { ((MPU *)arg)->DoTask(); } |
|
|
|
}; // namespace io |
|
}; // namespace ugv
|
|
|