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

add mpu calibration stuff

Browse Source
This commit is contained in:
Alex Mikhalev 2019-04-10 20:47:21 -07:00
parent 7278e528f0
commit 4f695b2775
2 changed files with 39 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
main/ugv_io_mpu.cc
View File

@ -20,6 +20,12 @@ 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 mpud::gyro_fs_t MPU_GYRO_FS = mpud::GYRO_FS_500DPS;
static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f); static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f);
static const Vec3f ACCEL_OFFSET = {0., 0., 0.};
static const Mat3f ACCEL_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
static const Vec3f MAG_OFFSET = {0., 0., 0.};
static const Mat3f MAG_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
static const Vec3f GYRO_OFFSET = {0., 0., 0.};
static const char *TAG = "ugv_io_mpu"; static const char *TAG = "ugv_io_mpu";
Vec3f::Vec3f() : x(0), y(0), z(0) {} Vec3f::Vec3f() : x(0), y(0), z(0) {}
@ -35,7 +41,7 @@ void MPU::Init() {
xSemaphoreTake(i2c_mutex, portMAX_DELAY); xSemaphoreTake(i2c_mutex, portMAX_DELAY);
mpu_bus_ = &i2c0; mpu_bus_ = &i2c0;
// This is shared with the oled, so just use those pins // This is shared with the oled, so just use those pins
// mpu_bus_->begin(MPU_SDA, MPU_SCL); mpu_bus_->begin(MPU_SDA, MPU_SCL, 100000);
mpu_ = new mpud::MPU(*mpu_bus_); mpu_ = new mpud::MPU(*mpu_bus_);
esp_err_t ret; esp_err_t ret;
@ -55,7 +61,7 @@ void MPU::Init() {
ESP_LOGE(TAG, "MPU initialization error"); ESP_LOGE(TAG, "MPU initialization error");
return; return;
} }
Calibrate(); // Calibrate();
mpu_->setAccelFullScale(MPU_ACCEL_FS); mpu_->setAccelFullScale(MPU_ACCEL_FS);
mpu_->setGyroFullScale(MPU_GYRO_FS); mpu_->setGyroFullScale(MPU_GYRO_FS);
// force magnetometer into continuous mode // force magnetometer into continuous mode
@ -109,10 +115,13 @@ void MPU::GetData(MpuData &data) {
static_cast<int16_t>(compass_data[4]); static_cast<int16_t>(compass_data[4]);
// ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz); // ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz);
data.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS); data.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS);
data.accel = ACCEL_MAT * (data.accel + ACCEL_OFFSET);
data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS); data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
data.gyro_rate += GYRO_OFFSET;
data.mag.x = ((float)mx) * MPU_MAG_TO_FLUX; data.mag.x = ((float)mx) * MPU_MAG_TO_FLUX;
data.mag.y = ((float)my) * MPU_MAG_TO_FLUX; data.mag.y = ((float)my) * MPU_MAG_TO_FLUX;
data.mag.z = ((float)mz) * MPU_MAG_TO_FLUX; data.mag.z = ((float)mz) * MPU_MAG_TO_FLUX;
data.mag = MAG_MAT * (data.mag + MAG_OFFSET);
} }
}; // namespace io }; // namespace io

21
main/ugv_io_mpu.hh
View File

@ -14,6 +14,27 @@ struct Vec3f {
Vec3f(); Vec3f();
Vec3f(float x, float y, float z); Vec3f(float x, float y, float z);
Vec3f(const mpud::float_axes_t& axes); Vec3f(const mpud::float_axes_t& axes);
Vec3f operator+(const Vec3f& a) const { return {x + a.x, y + a.y, z + a.z}; }
Vec3f& operator+=(const Vec3f& a) {
x += a.x;
y += a.y;
z += a.z;
return *this;
}
};
struct Mat3f {
float xx, xy, xz;
float yx, yy, yz;
float zx, zy, zz;
Vec3f operator*(const Vec3f& v) const {
return {xx * v.x + xy * v.y + xz + v.z,
yx * v.x + yy * v.y + yz * v.z,
zx * v.x + zy * v.y + zz * v.z};
}
}; };
struct MpuData { struct MpuData {