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

add calibration values

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-09 21:16:20 -07:00
parent 2c63020f75
commit 3f8ab289a9
2 changed files with 46 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
main/ugv_io_mpu.cc
View File

@ -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 Vec3f ACCEL_OFFSET = {0., 0., 0.};
static const Mat3f ACCEL_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.}; 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 = {-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 = {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 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 Mat3f GYRO_MAT = {1., 0., 0., 0., -1., 0., 0., 0., -1.};
static const Vec3f GYRO_OFFSET = {0., 0., 0.}; static const Vec3f GYRO_OFFSET = {-4.33655, -2.76826, -0.908427};
static const char *TAG = "ugv_io_mpu"; static const char *TAG = "ugv_io_mpu";

42
main/ugv_io_mpu.hh
View File

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