add calibration values

main/ugv_io_mpu.cc

@@ -23,11 +23,11 @@ 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   = {-7.79683, 3.6735, 32.3868};
-static const Vec3f MAG_OFFSET   = {0., 0., 0.};
+static const Vec3f MAG_OFFSET   = {-118.902, 18.8173, -39.209};
 //static const Mat3f MAG_MAT      = {0., -0.0281408, 0., -0.0284409, 0., 0., 0., 0., 0.0261544};
-static const Mat3f MAG_MAT      = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
-static const Mat3f GYRO_MAT    = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
-static const Vec3f GYRO_OFFSET  = {0., 0., 0.};
+static const Mat3f MAG_MAT      = {0.0330167, 0., 0., 0., 0.0335989, 0., 0., 0., 0.0335989};
+static const Mat3f GYRO_MAT    = {1., 0., 0., 0., -1., 0., 0., 0., -1.};
+static const Vec3f GYRO_OFFSET  = {-4.33655, -2.76826, -0.908427};
 
 static const char *TAG = "ugv_io_mpu";
 

main/ugv_io_mpu.hh

@@ -15,6 +15,15 @@ struct Vec3f {
   Vec3f(float x, float y, float z);
   Vec3f(const mpud::float_axes_t& axes);
 
+  Vec3f operator*(float s) const { return {s * x, s * y, s * z}; }
+
+  Vec3f& operator*=(float s) {
+    x *= s;
+    y *= s;
+    z *= s;
+    return *this;
+  }
+
   Vec3f operator+(const Vec3f& a) const { return {x + a.x, y + a.y, z + a.z}; }
 
   Vec3f& operator+=(const Vec3f& a) {
@@ -23,17 +32,44 @@ struct Vec3f {
     z += a.z;
     return *this;
   }
+
+  float dot(const Vec3f& v) const {
+    return x * v.x + y * v.y + z * v.z;
+  }
 };
 
 struct Mat3f {
-  float xx, xy, xz;
-  float yx, yy, yz;
-  float zx, zy, zz;
+  union {
+    Vec3f rx;
+    float xx, xy, xz;
+  };
+  union {
+    Vec3f ry;
+    float yx, yy, yz;
+  };
+  union {
+    Vec3f rz;
+    float zx, zy, zz;
+  };
+
+  Mat3f(Vec3f rx_, Vec3f ry_, Vec3f rz_)
+      : rx(rx_), ry(ry_), rz(rz_) {}
+
+  Mat3f(float xx, float xy, float xz, float yx, float yy, float yz, float zx,
+        float zy, float zz)
+      : rx(xx, xy, xz), ry(yx, yy, yz), rz(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};
+    return {rx.dot(v), ry.dot(v), rz.dot(v)};
+  }
+
+  Mat3f transpose() const {
+    return {xx, yx, zx, xy, yy, zy, xz, yz, zz};
+  }
+
+  Mat3f operator*(const Mat3f& m) const {
+    Mat3f mt = m.transpose();
+    return {mt * rx, mt * ry, mt * rz};
   }
 };