add mpu calibration stuff

main/ugv_io_mpu.cc

@@ -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 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";
 
 Vec3f::Vec3f() : x(0), y(0), z(0) {}
@@ -35,7 +41,7 @@ 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_bus_->begin(MPU_SDA, MPU_SCL, 100000);
   mpu_ = new mpud::MPU(*mpu_bus_);
 
   esp_err_t ret;
@@ -55,7 +61,7 @@ void MPU::Init() {
     ESP_LOGE(TAG, "MPU initialization error");
     return;
   }
-  Calibrate();
+  // Calibrate();
   mpu_->setAccelFullScale(MPU_ACCEL_FS);
   mpu_->setGyroFullScale(MPU_GYRO_FS);
   // force magnetometer into continuous mode
@@ -109,10 +115,13 @@ void MPU::GetData(MpuData &data) {
                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.accel     = ACCEL_MAT * (data.accel + ACCEL_OFFSET);
   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.y = ((float)my) * MPU_MAG_TO_FLUX;
   data.mag.z = ((float)mz) * MPU_MAG_TO_FLUX;
+  data.mag   = MAG_MAT * (data.mag + MAG_OFFSET);
 }
 
 };  // namespace io

main/ugv_io_mpu.hh

@@ -14,6 +14,27 @@ struct Vec3f {
   Vec3f();
   Vec3f(float x, float y, float z);
   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 {