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.
119 lines
3.9 KiB
119 lines
3.9 KiB
#include "ugv_io_mpu.hh" |
|
|
|
#include <driver/uart.h> |
|
#include <esp_log.h> |
|
|
|
#include <math.h> |
|
|
|
constexpr float M_PI = atanf(1.f) * 4; |
|
|
|
#include "MPU.hpp" |
|
#include "i2c_mutex.h" |
|
#include "mpu/math.hpp" |
|
|
|
namespace ugv { |
|
namespace io { |
|
|
|
static constexpr gpio_num_t MPU_SDA = GPIO_NUM_5; |
|
static constexpr gpio_num_t MPU_SCL = GPIO_NUM_4; |
|
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::~MPU() { delete mpu_; } |
|
|
|
void MPU::Init() { |
|
xSemaphoreTake(i2c_mutex, portMAX_DELAY); |
|
mpu_bus_ = &i2c0; |
|
// This is shared with the oled, so just use those pins |
|
// mpu_bus_->begin(MPU_SDA, MPU_SCL); |
|
mpu_ = new mpud::MPU(*mpu_bus_); |
|
|
|
esp_err_t ret; |
|
ret = mpu_->testConnection(); |
|
if (ret != ESP_OK) { |
|
uint8_t wai = mpu_->whoAmI(); |
|
ESP_LOGE(TAG, "MPU not connected (whoAmI: %d)", wai); |
|
return; |
|
} |
|
ret = mpu_->compassTestConnection(); |
|
if (ret != ESP_OK) { |
|
uint8_t wai = mpu_->compassWhoAmI(); |
|
ESP_LOGW(TAG, "MPU compass not connected (whoAmI: %d)", wai); |
|
} |
|
ret = mpu_->initialize(); |
|
if (ret != ESP_OK) { |
|
ESP_LOGE(TAG, "MPU initialization error"); |
|
return; |
|
} |
|
Calibrate(); |
|
mpu_->setAccelFullScale(MPU_ACCEL_FS); |
|
mpu_->setGyroFullScale(MPU_GYRO_FS); |
|
// force magnetometer into continuous mode |
|
mpu_->compassWriteByte(0x0A, 0x12); |
|
xSemaphoreGive(i2c_mutex); |
|
|
|
ESP_LOGI(TAG, "MPU initialized"); |
|
} |
|
|
|
void MPU::Calibrate() { |
|
mpud::raw_axes_t accel_offset, gyro_offset; |
|
mpu_->computeOffsets(&accel_offset, &gyro_offset); |
|
mpu_->setAccelOffset(accel_offset); |
|
mpu_->setGyroOffset(gyro_offset); |
|
{ |
|
auto ao = mpud::math::accelGravity(accel_offset, mpud::ACCEL_FS_2G); |
|
auto go = mpud::math::gyroDegPerSec(gyro_offset, mpud::GYRO_FS_250DPS); |
|
ESP_LOGI(TAG, "MPU offsets: accel=(%f, %f, %f) gyro=(%f, %f, %f)", ao.x, |
|
ao.y, ao.z, go.x, go.y, go.z); |
|
} |
|
} |
|
|
|
void MPU::GetData(MpuData &data) { |
|
esp_err_t ret; |
|
// uint8_t mxh, mxl, myh, myl, mzh, mzl, n; |
|
xSemaphoreTake(i2c_mutex, portMAX_DELAY); |
|
ret = mpu_->motion(&accel_, &gyro_); |
|
uint8_t compass_data[7]; |
|
mpu_->setAuxI2CBypass(true); |
|
mpu_bus_->readBytes(mpud::COMPASS_I2CADDRESS, 0x03, 7, compass_data); |
|
mpu_->setAuxI2CBypass(false); |
|
// mpu_->compassReadByte(0x03, &mxl); |
|
// mpu_->compassReadByte(0x04, &mxh); |
|
// mpu_->compassReadByte(0x05, &myl); |
|
// mpu_->compassReadByte(0x06, &myh); |
|
// mpu_->compassReadByte(0x07, &mzl); |
|
// mpu_->compassReadByte(0x08, &mzh); |
|
// mpu_->compassReadByte(0x09, &n); |
|
xSemaphoreGive(i2c_mutex); |
|
if (ret != ESP_OK) { |
|
ESP_LOGE(TAG, "error reading MPU"); |
|
} |
|
// int16_t mx = (static_cast<int16_t>(mxh) << 8) | static_cast<int16_t>(mxl); |
|
// int16_t my = (static_cast<int16_t>(myh) << 8) | static_cast<int16_t>(myl); |
|
// int16_t mz = (static_cast<int16_t>(mzh) << 8) | static_cast<int16_t>(mzl); |
|
int16_t mx = (static_cast<int16_t>(compass_data[1]) << 8) | |
|
static_cast<int16_t>(compass_data[0]); |
|
int16_t my = (static_cast<int16_t>(compass_data[3]) << 8) | |
|
static_cast<int16_t>(compass_data[2]); |
|
int16_t mz = (static_cast<int16_t>(compass_data[5]) << 8) | |
|
static_cast<int16_t>(compass_data[4]); |
|
// ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz); |
|
data.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS); |
|
data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS); |
|
data.mag.x = ((float)mx) * MPU_MAG_TO_FLUX; |
|
data.mag.y = ((float)my) * MPU_MAG_TO_FLUX; |
|
data.mag.z = ((float)mz) * MPU_MAG_TO_FLUX; |
|
} |
|
|
|
}; // namespace io |
|
}; // namespace ugv
|
|
|