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

read mpu in separate thread

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-17 19:59:13 -08:00
parent c74384bf84
commit 87dc10cdc5
3 changed files with 60 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
main/ugv_io.cc
View File

@ -18,8 +18,6 @@ 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 const char *TAG = "ugv_io";
IOClass IO;
IOClass::IOClass() {}

51
main/ugv_io_mpu.cc
View File

@ -15,6 +15,11 @@ 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_ = new mpud::MPU(); }
MPU::~MPU() { delete mpu_; }
@ -33,20 +38,46 @@ void MPU::Init() {
}
mpu_->setAccelFullScale(MPU_ACCEL_FS);
mpu_->setGyroFullScale(MPU_GYRO_FS);
ESP_LOGI(TAG, "MPU initialized");
mutex_ = xSemaphoreCreateMutex();
mpu_data_ = MpuData{};
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) {
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;
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

23
main/ugv_io_mpu.hh
View File

@ -7,15 +7,23 @@
namespace ugv {
namespace io {
using mpud::float_axes_t;
struct Vec3f {
float x;
float y;
float z;
Vec3f();
Vec3f(float x, float y, float z);
Vec3f(const mpud::float_axes_t& axes);
};
struct MpuData {
// G's
float_axes_t accel;
Vec3f accel;
// degrees/s
float_axes_t gyro_rate;
Vec3f gyro_rate;
// flux density uT
float_axes_t mag;
Vec3f mag;
};
class MPU {
@ -30,7 +38,14 @@ class MPU {
private:
mpud::MPU * mpu_;
mpud::raw_axes_t accel_, gyro_, mag_;
TaskHandle_t task_;
SemaphoreHandle_t mutex_;
MpuData mpu_data_;
void DoTask();
static void MPU_Task(void *arg);
};
} // namespace io